|
发表于 2014-4-15 12:45:36
|
显示全部楼层
電機安裝
厰家給的代碼 - unsigned char Re_buf[11],counter=0;
- unsigned char sign=0;
- int M11=5;
- int M12=6;
- int M21=9;
- int M22=10;
- float a[3],w[3],Angle[3],T;
- short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
- void setup() {
- // initialize serial:
- Serial.begin(115200);
- pinMode(M11,OUTPUT);analogWrite(M11,0);
- pinMode(M12,OUTPUT);analogWrite(M12,0);
- pinMode(M21,OUTPUT);analogWrite(M21,0);
- pinMode(M22,OUTPUT);analogWrite(M22,0);
- }
- void SetMotor(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);}
- }
- float PID1(float e,float kp,float ki,float kd)
- {
- static float es=0,sum=0;
- float r;
- sum+=e;
- r = kp*e+ki*sum+kd*(e-es);
- es=e;
- return r;
- }
- float PID2(float e,float kp,float ki,float kd)
- {
- static float es=0,sum=0;
- float r;
- sum+=e;
- r = kp*e+ki*sum+kd*(e-es);
- es=e;
- return r;
- }
- void loop() {
- float kp=30,ki=0.0,kd=10,r1,r2;
- if (sign==0) return;//sign为数据更新标志,每隔10ms更新一次,也就是说以下代码每隔10ms控制一次。
- sign=0;
- kd = (float)analogRead(0)/1024*200;
- r1=PID1(Angle[0],kp,ki,kd);//PID1、PID2函数就是第四节的PID函数,为了区分左右轮,所以分成两个。
- r2=PID2(Angle[0],kp,ki,kd);
- SetMotor(r1,r2);//设置电机转速。
- Serial.print("angle:");
- Serial.print(Angle[0]);Serial.print(" ");
- Serial.print(r1);Serial.print(" ");
- Serial.println(kd);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 library 寫出來的 - #include <PID_v1.h>
- //Define Variables we'll be connecting to
- double Setpoint, Input, Output;
- //Specify the links and initial tuning parameters
- PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
- unsigned char Re_buf[11],counter=0;
- unsigned char sign=0;
- int M11=5;
- int M12=6;
- int M21=9;
- int M22=10;
- float a[3],w[3],Angle[3],T;
- short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
- void setup() {
- // initialize serial:
- Serial.begin(115200);
- pinMode(M11,OUTPUT);
- pinMode(M12,OUTPUT);
- pinMode(M21,OUTPUT);
- pinMode(M22,OUTPUT);
- Input = Angle[0];
- Setpoint = 0;
- myPID.SetMode(AUTOMATIC);
- myPID.SetOutputLimits(-255,255);
- }
- void loop(){
- Input = Angle[0];
- myPID.Compute();
- if(Angle[0]>0){analogWrite(M11,Output);analogWrite(M12,0);analogWrite(M21,Output);analogWrite(M22,0);}
- else if(Angle[0]<0){analogWrite(M11,0);analogWrite(M12,-Output);analogWrite(M21,0);analogWrite(M22,-Output);}
-
-
- Serial.print("angle:");
- Serial.print(Angle[0]);Serial.print(" "); Serial.print(Output);Serial.println(" ");
- }
- 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;
- }
- }
- }
- }
复制代码 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|