int main(void)
{
//int acc_Angle;
unsigned char pwm_value = 0; // variable for velocity control
init_adxl();
init_devices1();
SetTunings(1,1,1);
start_timer4();
while(1)
{
Input = (double)acc_angle();
Compute();
pr_int(2,1,Input,3);
pr_int(1,1,Output,5);
}
//_delay_ms(10);
//acc_Angle=millis();
//lcd_print(1,1,acc_Angle,5);
/*while(0)
{
acc_Angle=acc_angle();
Input=(double)acc_Angle;
Compute();
if (Output>0)
{
set_PWM_value(18+Output/100);
forward();
}
else
{
set_PWM_value(18+Output/-100);
back();
}
pr_int(2,1,acc_Angle,3);
pr_int(1,1,Output,5);
} */
}
//Main Function
int main()
{
init_devices1();
while(1);
}
int main(void) //Main program starts from here
{
double acc_Angle;
int gyro_Angle;
int filt_Angle;
unsigned int pwm_value;
init_adxl(); //Initialise accelerometer
init_gyro(); //Initialise gyroscope
init_devices1();
uart0_init(); //Initailize UART1 for serial communiaction
start_timer4(); //Timer for timing calculations
SetTunings(8.1,8,5);
lcd_print(1,1,kp*10,4);
lcd_print(1,6,ki*10,4);
lcd_print(1,11,kd*10,4);
while(1)
{
acc_Angle=0.1*acc_angle(); //Accelerometer angle
gyro_Angle=gyro_Rate(); //Anugular rate from Gyroscope
filt_Angle=comp_filter(acc_Angle,gyro_Angle); //Filtered angle after passing through Complementary filter
Input=filt_Angle; //Input for error calculation of PID
Compute(); //Calling PID
if (Output>0) //Mapping PID output to velocity of motors
{
pwm_value = (Output+THRESHOLD);
if(pwm_value>=255)
{
pwm_value=255;
}
set_PWM_value(pwm_value);
forward();
}
else if(Output<0)
{
pwm_value = (-Output)+THRESHOLD;
if(pwm_value>=255)
{
pwm_value=255;
}
set_PWM_value(pwm_value);
back();
}
else if(Input==0)
{
stop();
}
//_delay_ms(20);
UDR0=0xFF;
_delay_ms(1);
UDR0=(uint8_t)(filt_Angle+100);
_delay_ms(1);
uint8_t op=(Output/2)+127;
UDR0=op;
}
}
