// ADC Variables
double temp_res;
const double res = 0.25;
// PID Variables
double Input, Output;
double errSum, lastErr;
double dErr,error;
unsigned long lastTime,now;
double timeChange;
// Clock Variable
int time;
// Position contral variables
double xPos,yPos,zPos;
double theta_1N,theta_2N,theta_3N;
double theta_1O,theta_2O,theta_3O;
double setPoint;
const int link_1 = 6;
const int link_2 = 4;
double d_1,DD;
double phi;
char uart_rd[7];
int Duty,x,y,_rd,flag;
// Resolution adjustment
double resAdjust(double theta_1N){
setPoint=0;
if(theta_1N < res){
setPoint = res;
}
else{
while(setPoint < theta_1N){
setPoint+=res;
}
}
return setPoint;
}
// Time function
void time() {
if(INTCON.T0IF) { // TMR0 Overflow Interrupt Flag bit 1=> has overflowed & 0=> did not overflow
time++ ; // Increase each 260us
INTCON.T0IF = 0 ;
}
}
// PID Computation Function
double Compute(double Input,double setPoint){
/*How long since we last calculated*/
now=time;
timeChange = (now - lastTime);
/*Compute all the working error variables*/
error = setPoint - Input;
errSum += (error * timeChange);
dErr = (error - lastErr) / timeChange;
/*Compute PID Output*/
Output = error + errSum + dErr;
/*Remember some variables for next time*/
lastErr = error;
lastTime = time;
return error;
}
void serialPort(){
for(x=0; x<7; x++){
uart_rd[x] = '\0'; // Initialize string with all '\0'
}
if (PIR1.RCIF) { // test the interrupt for uart rx
UART1_Read_Text(uart_rd,"s", 255); // read input from uart
PORTB.RB7=1; delay_ms(100);
PORTB.RB7=0; delay_ms(100);
flag = 1;
}
if(flag == 1){
for(y=1; y<=atoi(uart_rd); y++){
PORTB.RB3=1; delay_ms(100);
PORTB.RB3=0; delay_ms(100);
}
flag = 0;
}
/*
_rd = strcmp(uart_rd, "g");
if(_rd == 0){
Duty = 0;
PWM1_Set_Duty(Duty);
}
*/
}
void main() {
ADCON1=0b111110;
PORTC=0;
PORTB=0;
TRISB=0b000001;
CMCON=7;
OPTION_REG.T0CS=0;
INTCON.GIE = 1;
INTCON.PEIE = 1;
INTCON.TMR0IE = 1;
PIE1.RCIE = 1; //enable interrupt.
PWM1_Init(20000);
PWM1_Start();
Duty = 255;
PWM1_Set_Duty(Duty);
UART1_Init(9600); // Initialize UART module at 9600 bps
Delay_ms(100); // Wait for UART module to stabilize
while(1){
serialPort();
time();
theta_1N = 35;
theta_1O = 100;
setPoint = resAdjust(theta_1N);
temp_res = ADC_Read(0);
temp_res *= 0.25;
temp_res = Compute(temp_res,setPoint);
// Motor Forward & Backward
if(temp_res < 0.25){
PORTB.RB5 = 0;
PORTB.RB1 = 0;
}
else{
if(theta_1N > theta_1O){
PORTB.RB1 = PORTC.RC2;
PORTB.RB5 = 0;
}
else{
PORTB.RB5 = PORTC.RC2;
PORTB.RB1 = 0;
}
}
}
theta_1O = theta_1N;
}