template <class dataType, class floatType> void Pid<dataType, floatType>::Init(
dataType kp,
dataType ki,
dataType kd,
ctrlDir_t controllerDir,
outputMode_t outputMode,
floatType samplePeriodMs,
dataType minOutput,
dataType maxOutput,
dataType setPoint)
{
SetOutputLimits(minOutput, maxOutput);
this->samplePeriodMs = samplePeriodMs;
SetControllerDirection(controllerDir);
this->outputMode = outputMode;
// Set tunings with provided constants
SetTunings(kp, ki, kd);
this->setPoint = setPoint;
prevInput = 0;
prevOutput = 0;
pTerm = floatType(0.0);
iTerm = floatType(0.0);
dTerm = floatType(0.0);
}
/* SetSampleTime(...) *********************************************************
* sets the period, in Milliseconds, at which the calculation is performed
******************************************************************************/
void PID::SetSampleTime(float NewSampleTime)
{
if (NewSampleTime > 0)
{
SampleTime = NewSampleTime;
SetTunings(dispKp, dispKi, dispKd);
}
}
void setup() {
pinMode(4, INPUT); //set the second encoder channel up
attachInterrupt(0, integrate, RISING);
Serial.begin(9600);
digitalWrite(motorEnablePin, LOW); //safety
digitalWrite(motorDirectionPin, LOW); //safety
digitalWrite(motorDrivePin, HIGH); //safety
//pid setup
SetMode(AUTOMATIC); //auto PID mode
SetTunings(1, 0, 0);
SetOutputLimits(-3, 3); //+-24V maximum
SetSampleTime(5); //something smaller than the loop delay
Setpoint = 12.56;
digitalWrite(motorEnablePin, HIGH); //let it go
}
QuadPID::QuadPID(float kp, float ki, float kd, float *input, float *output, float *setpoint, MilliSec_t sample_period)
{
// pointeurs, entrées, sorties consignes
Input = input;
Output = output;
Setpoint = setpoint;
// période de calcule par défaut 50Hz : 20ms
DefaultSamplePeriod = SamplePeriod = sample_period;
// mode automatique (par défaut)
Mode = QUADPID_AUTOMATIC;
// mise en place des paramètres
SetTunings(kp, ki, kd);
SetDirection(QUADPID_DIRECT);
SetOutputLimits(0, 255);
}
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_devices();
lcd_set_4bit();
lcd_init();
int max = 100 ;
while(1)
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
flag=0;
senser_value_L = print_sensor(1,5,3); //Prints value of White Line Sensor1
senser_value_C = print_sensor(1,1,2); //Prints Value of White Line Sensor2
senser_value_R = print_sensor(1,9,1); //Prints Value of White Line Sensor3
SetTunings();
pid = PID(senser_value_L-senser_value_R);
if (pid < 30)
{
forward();
velocity(max+pid,max);
}
if (pid > 30)
{
forward();
velocity(max,max+pid);
}
/*else if((Left_white_line < 0x30) && (flag==0))
{
pid = PID(senser_value_L);
flag=1;
forward();
velocity(max,max+pid);
}
else if((Right_white_line<0x30) && (flag==0))
{
pid = PID(senser_value_R);
flag=1;
forward();
velocity(max+pid,max);
}
if(Center_white_line>0x10 && Left_white_line>0x10 && Right_white_line>0x10)
{
forward();
velocity(0,0);
}*/
lcd_print(1,13,pid,3);
}
}
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;
}
}
/*
//Function Name - Main
//Input - Nothing
//Output - control left and right Motor speeds
//Logic - add pid correction error here.
*/
int main()
{
init_devices();
lcd_set_4bit();
lcd_init();
signed int max = 250;
speed_L = 255;
speed_R = 255;
while(1)
{
data_received [0] = ADC_Conversion(3); //Getting data of sensor-0 WL Sensor
data_received [1] = ADC_Conversion(2); //Getting data of sensor-1 WL Sensor
data_received [2] = ADC_Conversion(1); //Getting data of sensor-2 WL Sensor
data_received [3] = spi_master_tx_and_rx(0); //Getting data of sensor-3 WL sensor connected to slave microcontroller.
data_received [4] = spi_master_tx_and_rx(1); //Getting data of sensor-4 WL sensor connected to slave microcontroller.
data_received [5] = spi_master_tx_and_rx(2); //Getting data of sensor-5 WL sensor connected to slave microcontroller.
data_received [6] = spi_master_tx_and_rx(3); //Getting data of sensor-6 WL sensor connected to slave microcontroller.
/*lcd_print(1, 1,data_received [0], 3);
lcd_print(1, 5,data_received [1], 3);
lcd_print(1, 9,data_received [2], 3);
lcd_print(1, 14,data_received [3], 3);
lcd_print(2, 1,data_received [4], 3);
lcd_print(2, 5,data_received [5], 3);
lcd_print(2, 9,data_received [6], 3);
*/
SetTunings();
sensor_value[0] = sensor_on_line(data_received [0]);
sensor_value[1] = sensor_on_line(data_received [1]);
sensor_value[2] = sensor_on_line(data_received [2]);
sensor_value[3] = sensor_on_line(data_received [3]);
sensor_value[4] = sensor_on_line(data_received [4]);
sensor_value[5] = sensor_on_line(data_received [5]);
sensor_value[6] = sensor_on_line(data_received [6]);
/*senser_value_sum = data_received [0] + data_received [1] + data_received [2] + data_received [3] + data_received [4] + data_received [5] + data_received [6] ;
weight = ((-3)*data_received [0] + (-2)*data_received [1] + (-1)*data_received [2] + (0)*data_received [3] + (1)*data_received [4] + (2)*data_received [5] + (3)*data_received [6]);
*/
senser_value_sum = sensor_value[0] + sensor_value[1] + sensor_value[2] + sensor_value[3] + sensor_value[4] + sensor_value[5] + sensor_value[6] ;
weight = 10*((-3)*sensor_value[0] + (-2)*sensor_value[1]+ (-1)*sensor_value[2] + (0)*sensor_value[3] + (1)*sensor_value[4] + (2)*sensor_value[5] + (3)*sensor_value[6]);
//control variable
value_on_line = weight/senser_value_sum ;
//lcd_print(1, 3,100-value_on_line, 3);
/*lcd_print(2, 10,sensor_value[0], 1);
lcd_print(2, 11,sensor_value[1], 1);
lcd_print(2, 12,sensor_value[2], 1);
lcd_print(2, 13,sensor_value[3], 1);
lcd_print(2, 14,sensor_value[4], 1);
lcd_print(2, 15,sensor_value[5], 1);
lcd_print(2, 16,sensor_value[6], 1);
*/
pid = PID(value_on_line);
//pid = PID(weight);
if (pid <= -max)
{
pid = -max ;
}
if (pid >= max)
{
pid = max;
}
if (senser_value_sum == 0)
{
stop();
}
else
{
if (pid == 0)
{
//integral = 0;
forward();
velocity(speed_L,speed_R);
/*lcd_print(2,1,speed_L,3);
lcd_print(2,5,speed_R,3);
lcd_print(1,13,2000-pid, 4);*/
}
if(pid<0)
{
if(pid > -180)
{
forward();
velocity(speed_L+pid,speed_R);
/*lcd_print(2,1,speed_L+pid,3);
lcd_print(2,5,speed_R,3);
lcd_print(1,13,2000-pid, 4);*/
}
else
{
left();
velocity(speed_L-50,speed_R-50);
/*lcd_print(2,1,speed_L-50,3);
lcd_print(2,5,speed_R-50,3);
lcd_cursor(1,13);
}*/
}
}
if (pid>0)
{
if(pid<180)
{
forward();
velocity(speed_L,speed_R-pid);
/*lcd_print(2,1,speed_L,3);
lcd_print(2,5,speed_R-pid,3);
lcd_print(1,13,2000-pid, 4);*/
}
else
{
right();
velocity(speed_L-50 , speed_R-50);
/*lcd_print(2,1,speed_L-50,3);
lcd_print(2,5,speed_R-50,3);
lcd_cursor(1,13);
}*/
}
}
}
}
}
