arduino pid循迹代码.zip

#include "line.h" #include "uart.h" #include "motor.h" ///**** //int line_number = 0; //int T_number = 0; //int line_flag = 1; //int T_flag = 1; //int T_signal = 0; //int T_temp1 = 0; //int T_temp2 = 1; //int T_temp3 = 2; //double time_1; //double time_now; //int track_zhixian2() //{ // unsigned int Temp[2] = { 0 }; //鏁版嵁缂撳瓨鍖� // Read_Data(Temp); // int stop_signal = 0; // switch(Temp[0]) // { //765 4321 // //case 0x00: motor(40,40); break; //000 0000 // //Serial3.write(DM_Action0, 5); // // delay(2000); // // case 0x7F: motor(40,40); break; //111 1111 // //case 0x00: motor(10,-20); break; //111 1100 // //case 0x78: motor(-15,-40); break; //111 1000 //// case 0x3E: motor(-40,-40); break; //011 1110 //// case 0x7B: motor(25,40); break; //111 1011 //// case 0x73: motor(38,40); break; //111 0011 // case 0x63: motor(35,35); break; //1100011 //姝ｄ腑闂翠綅缃� // case 0x73: motor(25,40); break; //1110011 //鍙冲皬灏忓皬鍋� // case 0x71: motor(15,50); break; //1110001 //鍙冲皬灏忓亸 // case 0x79: motor(-25,45); break; //1111001 //鍙冲皬鍋� // case 0x78: motor(-45,40); break; //111 1000 //鍙冲ぇ鍋� // case 0x7C: motor(-65,35); break; //111 1100 //鍙冲ぇ澶у亸 // case 0x7E: motor(-85,30); break; //111 1110 //鍙冲ぇ澶уぇ鍋� // case 0x67: motor(45,25); break; //1100111 //宸﹀皬灏忓皬鍋� // case 0x47: motor(55,15); break; //1000111 //宸﹀皬灏忓亸 // case 0x4F: motor(65,5); break; //1001111 //宸﹀皬鍋� // case 0x0F: motor(85,-15); break; //0001111 //宸﹀ぇ鍋� // case 0x1F: motor(100,-70); break; //0011111 //宸﹀ぇ澶у亸 // case 0x3F: motor(120,-100); break; //0111111 //宸﹀ぇ澶уぇ鍋� // case 0x00: line_number+=line_flag; break; //0000000 //鍏ㄩ粦 璁℃暟 // case 0x7F: motor(35,35); break; //1111111 //鍏ㄧ櫧 鍚庨�� // case 0x01: ; break; //0000001 //T鍨� 璁℃暟 // case 0x03: ; break; //0000011 //T鍨� 璁℃暟 // case 0x07: ; break; //0000111 //T鍨� 璁℃暟 ///**** // case 0x67: motor(40,38); break; //110 0111 // case 0x6F: motor(40,25); break; //110 1111 // case 0x4F: motor(40,20); break; //100 1111 // case 0x5F: motor(40,15); break; //101 1111 // case 0x1F: motor(40,10); break; //001 1111 // case 0x3F: motor(-40,0); break; //011 1111 // default : motor(0,0); break; // } // if(Temp[0] == 0x00) // line_flag =0; // else // line_flag =1; // if(Temp[0] == 0x01||Temp[0] == 0x03||Temp[0] == 0x07){ // delay(10); // if( T_temp2 == T_temp1 && T_temp1 == Temp[0]){ // T_number += T_signal * T_flag; // T_signal = 0; // } // else // { // T_signal = 1; // } // // } // else // { // T_signal = 0; // } // T_temp3 = T_temp2; // T_temp2 = T_temp1; // T_temp1 = Temp[0]; // if(T_number == 3 ||T_number == 8){ // // motor(-85,-85); // delay(50); //// motor(0,0); // stop_signal = 1; // delay(3000); // } // if(T_number == 3){ // //Serial3.write(DM_Action1, 5); // // delay(2000); // // Serial3.write(DM_Action2, 5); // // delay(2000); // time_1 = millis(); // } // // if(T_number == 8){ // // Serial3.write(DM_Action3, 5); // // delay(2000); // } // if(millis() - time_1 < 16){ // T_flag = 0; // } // else // T_flag = 1; // if(stop_signal){ // T_number++; // } // else // delay(5); // return line_number; //} ///************************************* //*鍑芥暟鍚嶇О锛歵rack_PID //*鍑芥暟鍔熻兘锛氱洿绾垮惊杩癸紝鐢ㄤ覆鍙ｇ嚎杩炴帴锛屽彧杈撳嚭鍋忕Щ鍊� //*鍙傛暟锛歱wm锛氭渶澶ч�熷害鍊硷紝P锛氭瘮渚嬬郴鏁� //*璇存槑锛� //* //**************************************/ void track_PID(int pwm,float P) { static float Integral_error,Last_error; unsigned int temp_data[2] = { 0 }; //鏁版嵁缂撳瓨鍖� int error = 0; //鍋忓樊鍊� int L_Pwm,R_Pwm; //宸﹀彸杞�熷害 float I = 0,D = 0; //绉垎绯绘暟锛屽井鍒嗙郴鏁� Read_Data(temp_data); if(temp_data[0]==0) { error = -temp_data[1]; } else { error = temp_data[1]; } Integral_error += error; R_Pwm = (pwm-(error*P+Integral_error*I+(error-Last_error)*D)); L_Pwm = (pwm+(error*P+Integral_error*I+(error-Last_error)*D)); Last_error = error; if(pwm > 0) { if(L_Pwm > (pwm+10)) L_Pwm = (pwm+10); if(R_Pwm > (pwm+10)) R_Pwm = (pwm+10); if(L_Pwm <= 15) L_Pwm = 15; if(R_Pwm <= 15) R_Pwm = 15; } } // // motor(L_Pwm,R_Pwm); //}