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); }*/ } } } } }