|
发表于 2014-6-26 20:49:39
|
显示全部楼层
本帖最后由 iohongwal 于 2014-6-30 21:29 编辑
- unsigned char Re_buf[11],counter=0;
- unsigned char sign=0;
- int M11=5;
- int M12=6;
- int M21=9;
- int M22=10;
- int Pwm_out = 0;
- int Turn_Need = 0;
- int Speed_Need = 0;
- int speed_output_R , speed_output_L;
- int count_l=13;
- int count_r=8;
- int count_left;
- int count_right;
- float speeds , speeds_filter, positions;
- float diff_speeds,diff_speeds_all;
- float a[3],w[3],Angle[3],T;
- short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
- float Kap = 24.5; //
- float Kad = 10; //
- float Ksp = 2.8; //
- float Ksi = 0.11; //
- void setup()
- {
- Serial.begin(115200);
- pinMode(5,OUTPUT);
- pinMode(6,OUTPUT);
- pinMode(9,OUTPUT);
- pinMode(10,OUTPUT);
- analogWrite(5,0);
- analogWrite(6,0);
- analogWrite(9,0);
- analogWrite(10,0);
- }
- void Motor(float v1,float v2)
- {
- if (v1>255){v1=255;analogWrite(M11,0);analogWrite(M12,v1);}
- else if (v1>0) {analogWrite(M11,0);analogWrite(M12,v1);}
- else if (v1>-255) {analogWrite(M12,0);analogWrite(M11,-v1);}
- else {v1=-255;analogWrite(M12,0);analogWrite(M11,-v1);}
-
- if (v2>255){v2=255;analogWrite(M21,0);analogWrite(M22,v2);}
- else if (v2>0) {analogWrite(M21,0);analogWrite(M22,v2);}
- else if (v2>-255) {analogWrite(M22,0);analogWrite(M21,-v2);}
- else {v2=-255;analogWrite(M22,0);analogWrite(M21,-v2);}
- }
- void loop()
- {
- }
- void PID_PWM()
- {
- speeds=(count_left + count_right)*0.5;
- diff_speeds = count_left - count_right;
- diff_speeds_all += diff_speeds;
- speeds_filter *=0.85; //一阶互补滤波
- speeds_filter +=speeds*0.15;
- positions += speeds_filter;
- positions += Speed_Need;
- positions = constrain(positions, -2300, 2300);//抗积分饱和
- Pwm_out = Kap*Angle[0] + Kad*w[0] + Ksp*speeds_filter + Ksi*positions ; //PID控制器
- if(Turn_Need == 0)
- {
- speed_output_R = int(Pwm_out - diff_speeds_all);
- speed_output_L = int(Pwm_out + diff_speeds_all);
- }
- speed_output_R = int(Pwm_out + Turn_Need);
- speed_output_L = int(Pwm_out - Turn_Need);
- Motor(speed_output_L , speed_output_R);
- count_left = 0;
- count_right = 0;
- Serial.print(Angle[0]);Serial.print(" ");
- Serial.print(w[0]);Serial.print(" ");
- Serial.println(Pwm_out);Serial.print(" ");
- }
- void serialEvent() {
- while (Serial.available()) {
-
- //char inChar = (char)Serial.read(); Serial.print(inChar); //Output Original Data, use this code
-
- Re_buf[counter]=(unsigned char)Serial.read();
- if(counter==0&&Re_buf[0]!=0x55) return; //第0号数据不是帧头
- counter++;
- if(counter==11) //接收到11个数据
- {
- counter=0; //重新赋值,准备下一帧数据的接收
- switch(Re_buf [1])
- {
- case 0x51:
- a[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*16;
- a[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*16;
- a[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*16;
- break;
- case 0x52:
- w[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*250;
- w[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*250;
- w[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*250;
- break;
- case 0x53:
- Angle[0] = float(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
- Angle[1] = float(short(Re_buf [5]<<8| Re_buf [4]))/32768*180;
- Angle[2] = float(short(Re_buf [7]<<8| Re_buf [6]))/32768*180;
- T = float(short(Re_buf [9]<<8| Re_buf [8]));///340.0+36.25
- sign=1;
- break;
- }
- }
- }
- }
复制代码
這我是用你的PID改 幫我看看有冇甚麼問題
因為呢我寫進arduino 走不動 和讀不到資料 |
|