求助，求助自平衡小车的问题

immortal

1. #include <PID_v1.h>
   
2. /*陀螺仪使用的定义变量*/
   
3. unsigned char Re_buf[11],counter=0;
   
4. unsigned char sign=0;
   
5. /*电机使用定义的变量*/
   
6. int motor_a1=4;
   
7. int motor_a2=5;
   
8. int motor_b1=6;
   
9. int motor_b2=7;
   
10. int ena=10;
    
11. int enb=11;
    
12. /*定义PID库使用的参数*/
    
13. double Setpoint, Input, Output;
    
14. double kp,ki,kd;
    
15. 
    
16. /*定义PID程序*/
    
17. PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);
    
18. /*初始化*/
    
19. void setup() {
    
20.   Serial.begin(115200); //初始化串口速率
    
21.   /*配置电机针脚为输出*/
    
22.   pinMode(motor_a1,OUTPUT);
    
23.   pinMode(motor_a2,OUTPUT);
    
24.   pinMode(motor_b1,OUTPUT);
    
25.   pinMode(motor_b2,OUTPUT);
    
26.   myPID.SetMode(AUTOMATIC); //配置PID为自动模式
    
27.   myPID.SetSampleTime(100); //配置PID采样率为100毫秒
    
28.   Setpoint = -1; //设置小车平衡点角度
    
29. }
    
30. /*主函数*/
    
31. void loop() {
    
32.   if (sign==0)
    
33.     return; //sign为数据更新标志，每隔10ms更新一次，也就是说以下代码每隔10ms控制一次
    
34.   sign=0;
    
35.   //  getkpkikd(&kp,&ki,&kd); //获取kp,ki,kd的值
    
36.   kp=(double)analogRead(0)/1024*200;
    
37.   ki=(double)analogRead(1)/10240;
    
38.   kd=(double)analogRead(2)/1024*400;
    
39. 
    
40.   myPID.Compute(); //PID运算
    
41.   SetMotor(Output); //控制电机
    
42. //  cout2serial() ; //串口输出需要查看的数据
    
43.   Serial.print("angle:");
    
44.   Serial.print(Input);Serial.print(" ");
    
45.   Serial.print("output:");
    
46.   Serial.print(Output);Serial.print(" ");
    
47.   Serial.print("kp:");
    
48.   Serial.print(kp);Serial.print(" ");
    
49.   Serial.print("ki:");
    
50.   Serial.print(ki);Serial.print(" ");
    
51.   Serial.print("kd:");
    
52.   Serial.print(kd);Serial.println(" ");
    
53. }
    
54. 
    
55. /*获取kp,ki,kd的值的函数*/
    
56. /*void getkpkikd(&kp,&ki,&kd){
    
57.   kp=(double)analogRead(0)/1024*200;
    
58.   ki=(double)analogRead(1)/10240;
    
59.   kd=(double)analogRead(2)/1024*400;
    
60. }*/
    
61. 
    
62. /*控制电机运行的函数*/
    
63. void SetMotor(float Output)
    
64. {
    
65. if (Output>=0){
    
66.   pinMode(motor_a1,HIGH);
    
67.   pinMode(motor_a2,LOW);
    
68.   pinMode(motor_b1,HIGH);
    
69.   pinMode(motor_b2,LOW);
    
70.   analogWrite(ena,Output);
    
71.   analogWrite(enb,Output);
    
72.   }
    
73. else if (Output<0) {
    
74.   pinMode(motor_a1,LOW);
    
75.   pinMode(motor_a2,HIGH);
    
76.   pinMode(motor_b1,LOW);
    
77.   pinMode(motor_b2,HIGH);
    
78.   analogWrite(ena,abs(Output));
    
79.   analogWrite(enb,abs(Output));
    
80.   }
    
81. }
    
82. 
    
83. /*输出串口数据的函数*/
    
84. /*void cout2serial() {
    
85.   Serial.print("angle:");
    
86.   Serial.print(Input);Serial.print(" ");
    
87.   Serial.print("output:");
    
88.   Serial.print(Output);Serial.print(" ");
    
89.   Serial.print("kp:");
    
90.   Serial.print(kp);Serial.print(" ");
    
91.   Serial.print("ki:");
    
92.   Serial.print(ki);Serial.print(" ");
    
93.   Serial.print("kd:");
    
94.   Serial.print(kd);Serial.println(" ");
    
95. }*/
    
96. 
    
97. void serialEvent()
    
98. {
    
99.   while (Serial.available()) {
    
100.     Re_buf[counter]=(unsigned char)Serial.read();
     
101.     if(counter==0&&Re_buf[0]!=0x55) return; //第0号数据不是帧头
     
102.       counter++;
     
103.     if(counter==11) //接收到11个数据
     
104.     {
     
105.       counter=0; //重新赋值，准备下一帧数据的接收
     
106.       switch(Re_buf [1])
     
107.       {
     
108.         case 0x51:
     
109.         break;
     
110.         case 0x52:
     
111.         break;
     
112.         case 0x53:
     
113.         Input= double(short(Re_buf [3]<<8| Re_buf [2]))/32768*180;
     
114.         sign=1;
     
115.         break;
     
116.       }
     
117.     }
     
118.   }
     
119. }

复制代码

不知道为什么Output 为零

immortal

immortal

1. /**********************************************************************************************
   
2. * Arduino PID Library - Version 1.0.1
   
3. * by Brett Beauregard <[email protected]> brettbeauregard.com
   
4. *
   
5. * This Library is licensed under a GPLv3 License
   
6. **********************************************************************************************/
   
7. 
   
8. #if ARDUINO >= 100
   
9.   #include "Arduino.h"
   
10. #else
    
11.   #include "WProgram.h"
    
12. #endif
    
13. 
    
14. #include <PID_v1.h>
    
15. 
    
16. /*Constructor (...)*********************************************************
    
17. *    The parameters specified here are those for for which we can't set up
    
18. *    reliable defaults, so we need to have the user set them.
    
19. ***************************************************************************/
    
20. PID::PID(double* Input, double* Output, double* Setpoint,
    
21.         double Kp, double Ki, double Kd, int ControllerDirection)
    
22. {
    
23.        
    
24.     myOutput = Output;
    
25.     myInput = Input;
    
26.     mySetpoint = Setpoint;
    
27.         inAuto = false;
    
28.        
    
29.         PID::SetOutputLimits(0, 255);                                //default output limit corresponds to
    
30.                                                                                                 //the arduino pwm limits
    
31. 
    
32.     SampleTime = 100;                                                        //default Controller Sample Time is 0.1 seconds
    
33. 
    
34.     PID::SetControllerDirection(ControllerDirection);
    
35.     PID::SetTunings(Kp, Ki, Kd);
    
36. 
    
37.     lastTime = millis()-SampleTime;                               
    
38. }
    
39. 
    
40. 
    
41. /* Compute() **********************************************************************
    
42. *     This, as they say, is where the magic happens.  this function should be called
    
43. *   every time "void loop()" executes.  the function will decide for itself whether a new
    
44. *   pid Output needs to be computed.  returns true when the output is computed,
    
45. *   false when nothing has been done.
    
46. **********************************************************************************/
    
47. bool PID::Compute()
    
48. {
    
49.    if(!inAuto) return false;
    
50.    unsigned long now = millis();
    
51.    unsigned long timeChange = (now - lastTime);
    
52.    if(timeChange>=SampleTime)
    
53.    {
    
54.       /*Compute all the working error variables*/
    
55.           double input = *myInput;
    
56.       double error = *mySetpoint - input;
    
57.       ITerm+= (ki * error);
    
58.       if(ITerm > outMax) ITerm= outMax;
    
59.       else if(ITerm < outMin) ITerm= outMin;
    
60.       double dInput = (input - lastInput);
    
61. 
    
62.       /*Compute PID Output*/
    
63.       double output = kp * error + ITerm- kd * dInput;
    
64.       
    
65.           if(output > outMax) output = outMax;
    
66.       else if(output < outMin) output = outMin;
    
67.           *myOutput = output;
    
68.           
    
69.       /*Remember some variables for next time*/
    
70.       lastInput = input;
    
71.       lastTime = now;
    
72.           return true;
    
73.    }
    
74.    else return false;
    
75. }
    
76. 
    
77. 
    
78. /* SetTunings(...)*************************************************************
    
79. * This function allows the controller's dynamic performance to be adjusted.
    
80. * it's called automatically from the constructor, but tunings can also
    
81. * be adjusted on the fly during normal operation
    
82. ******************************************************************************/
    
83. void PID::SetTunings(double Kp, double Ki, double Kd)
    
84. {
    
85.    if (Kp<0 || Ki<0 || Kd<0) return;
    
86. 
    
87.    dispKp = Kp; dispKi = Ki; dispKd = Kd;
    
88.    
    
89.    double SampleTimeInSec = ((double)SampleTime)/1000;  
    
90.    kp = Kp;
    
91.    ki = Ki * SampleTimeInSec;
    
92.    kd = Kd / SampleTimeInSec;
    
93. 
    
94.   if(controllerDirection ==REVERSE)
    
95.    {
    
96.       kp = (0 - kp);
    
97.       ki = (0 - ki);
    
98.       kd = (0 - kd);
    
99.    }
    
100. }
     
101.   
     
102. /* SetSampleTime(...) *********************************************************
     
103. * sets the period, in Milliseconds, at which the calculation is performed       
     
104. ******************************************************************************/
     
105. void PID::SetSampleTime(int NewSampleTime)
     
106. {
     
107.    if (NewSampleTime > 0)
     
108.    {
     
109.       double ratio  = (double)NewSampleTime
     
110.                       / (double)SampleTime;
     
111.       ki *= ratio;
     
112.       kd /= ratio;
     
113.       SampleTime = (unsigned long)NewSampleTime;
     
114.    }
     
115. }
     
116. 
     
117. /* SetOutputLimits(...)****************************************************
     
118. *     This function will be used far more often than SetInputLimits.  while
     
119. *  the input to the controller will generally be in the 0-1023 range (which is
     
120. *  the default already,)  the output will be a little different.  maybe they'll
     
121. *  be doing a time window and will need 0-8000 or something.  or maybe they'll
     
122. *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
     
123. *  here.
     
124. **************************************************************************/
     
125. void PID::SetOutputLimits(double Min, double Max)
     
126. {
     
127.    if(Min >= Max) return;
     
128.    outMin = Min;
     
129.    outMax = Max;
     
130. 
     
131.    if(inAuto)
     
132.    {
     
133.            if(*myOutput > outMax) *myOutput = outMax;
     
134.            else if(*myOutput < outMin) *myOutput = outMin;
     
135.          
     
136.            if(ITerm > outMax) ITerm= outMax;
     
137.            else if(ITerm < outMin) ITerm= outMin;
     
138.    }
     
139. }
     
140. 
     
141. /* SetMode(...)****************************************************************
     
142. * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
     
143. * when the transition from manual to auto occurs, the controller is
     
144. * automatically initialized
     
145. ******************************************************************************/
     
146. void PID::SetMode(int Mode)
     
147. {
     
148.     bool newAuto = (Mode == AUTOMATIC);
     
149.     if(newAuto == !inAuto)
     
150.     {  /*we just went from manual to auto*/
     
151.         PID::Initialize();
     
152.     }
     
153.     inAuto = newAuto;
     
154. }
     
155. 
     
156. /* Initialize()****************************************************************
     
157. *        does all the things that need to happen to ensure a bumpless transfer
     
158. *  from manual to automatic mode.
     
159. ******************************************************************************/
     
160. void PID::Initialize()
     
161. {
     
162.    ITerm = *myOutput;
     
163.    lastInput = *myInput;
     
164.    if(ITerm > outMax) ITerm = outMax;
     
165.    else if(ITerm < outMin) ITerm = outMin;
     
166. }
     
167. 
     
168. /* SetControllerDirection(...)*************************************************
     
169. * The PID will either be connected to a DIRECT acting process (+Output leads
     
170. * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
     
171. * know which one, because otherwise we may increase the output when we should
     
172. * be decreasing.  This is called from the constructor.
     
173. ******************************************************************************/
     
174. void PID::SetControllerDirection(int Direction)
     
175. {
     
176.    if(inAuto && Direction !=controllerDirection)
     
177.    {
     
178.           kp = (0 - kp);
     
179.       ki = (0 - ki);
     
180.       kd = (0 - kd);
     
181.    }   
     
182.    controllerDirection = Direction;
     
183. }
     
184. 
     
185. /* Status Funcions*************************************************************
     
186. * Just because you set the Kp=-1 doesn't mean it actually happened.  these
     
187. * functions query the internal state of the PID.  they're here for display
     
188. * purposes.  this are the functions the PID Front-end uses for example
     
189. ******************************************************************************/
     
190. double PID::GetKp(){ return  dispKp; }
     
191. double PID::GetKi(){ return  dispKi;}
     
192. double PID::GetKd(){ return  dispKd;}
     
193. int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
     
194. int PID::GetDirection(){ return controllerDirection;}
     

复制代码

immortal

immortal 发表于 2015-4-18 23:19

这是PID库函数

逝去の唯一

不懂！可能是PId问题

suoma

没看见你的PID调节值

immortal

suoma 发表于 2015-4-19 18:30
没看见你的PID调节值

我不太懂你的意思，PID调节值的相应程序呢，该怎样改呢。谢谢