Diff: main.cpp

Revision:

3:ee5e434e047e

Parent:

2:af377291f3ae

Child:

4:652d2be11b35

--- a/main.cpp	Tue Oct 15 05:03:34 2019 +0000
+++ b/main.cpp	Wed Oct 16 13:59:33 2019 +0000
@@ -1,75 +1,82 @@
+#define HIGH 1
+#define LOW 0
 #include "mbed.h"
+#include <string>
+typedef bool boolean;
+typedef std::string String;
 #include "QEI.h"
 
-//两个轮子L298N
-DigitalOut w1(PB_6);
-DigitalOut w2(PB_7);
-DigitalOut w3(PB_8);
-DigitalOut w4(PB_9);
-PwmOut mypwm1(PA_9);
-PwmOut mypwm2(PA_8);
-//调试串口
-Serial pc(PB_10, PB_11);
+int Position;
+float Error;
+int Output;
+float Error_Last;
+float P;
+float Error_int;
+float I;
+float Error_diff;
+float D;
 
-Ticker toggle_time_ticker;
-//编码器1-4
-QEI wheel(PA_2, PA_3, NC, 11, QEI::X4_ENCODING);                //买来的电机自带编码器,说明书上有电机每转一圈编码器发生11个信号
-QEI wheel2(PA_6, PA_7, NC, 11, QEI::X4_ENCODING);
+QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
+Serial Serial_2(PA_2,PA_3);
+Ticker tick622103;
+PwmOut myServoPB_0(PB_0);
+PwmOut myServoPB_1(PB_1);
 
-int x,y;
-float a= 0.01, b = 0.00860454;
-void time_ticker();
-void serialread();
+
 
-int main()
-{
-    mypwm1.period_ms(10);
-    mypwm2.period_ms(10);
-    
-    toggle_time_ticker.attach(&time_ticker, 0.2);       //使能ticker中断,0.2s产生一次.中断函数用于获取每个轮子的计数值
-    
-    pc.attach(&serialread);
-    
-    mypwm1.pulsewidth(a);
-    mypwm2.pulsewidth(b);
-    wheel.reset();      //reset函数,将编码器计数值赋0(初始化编码器,开始计数)
-    wheel2.reset();
-    w1 = 0;
-    w2 = 0;
-    w3 = 0;
-    w4 = 0; 
-    while(1)
-    {
-        
-    }   
+void PID_Caculation() {
+Error = 0 - Position;
+Error_diff = Error - Error_Last;
+Error_Last = Error;
+Error_int = Error_int + Error;
+if (Error_int > 100) {
+Error_int = 100;
+} else if (Error_int < -100) {
+Error_int = -100;
+}
+if (Error > -10 && Error < 10) {
+Output = 0;
+} else {
+Output = P * Error + (I * Error_int + D * Error_diff);
+}
+if (Output > 100) {
+Output = 100;
+} else if (Output < -100) {
+Output = -100;
+}
 }
 
-void time_ticker(){
-    //获取一个轮子的编码器读到的数和旋转方向
-    x = wheel.getPulses();      
-    x = x/44;       //将编码器计数值转化成轮子旋转的圈数
-    wheel.getState();   //获取旋转方向,返回值为1--顺时针, -1--逆时针         
-    y = wheel2.getPulses();
-    y = y/44;       //将编码器计数值转化成轮子旋转的圈数
-    wheel2.getState();  //获取旋转方向,返回值为1--顺时针, -1--逆时针
-    
-    wheel.reset();
-    wheel2.reset();
-    pc.printf("x:%d y:%d\n",x,y);
+void Set_speed() {
+if (Output >= 0) {
+myServoPB_0.period_ms(20);
+myServoPB_0.pulsewidth_us((200 * Output));
+myServoPB_1.period_ms(20);
+myServoPB_1.pulsewidth_us(0);
+} else if (Output < 0) {
+myServoPB_0.period_ms(20);
+myServoPB_0.pulsewidth_us(0);
+myServoPB_1.period_ms(20);
+myServoPB_1.pulsewidth_us((-200 * Output));
+}
 }
 
-void serialread(){
-    switch (pc.getc())
-  {
-    case '1':
-          w1=0;
-          w4=0;
-        break;
-    case '2':
-          w1=1;
-          w4=1;
-        break;
-    default:
-        break;
-  }
+void tick622103_handle() {
+Position = Position + qei_PA_0.getPulses();
+qei_PA_0.reset();
+Serial_2.printf("%d\n",Position);
+PID_Caculation();
+Set_speed();
+}
+
+
+int main() {
+Serial_2.baud(9600);
+
+tick622103.attach(&tick622103_handle,0.05);
+
+Position = 0;
+qei_PA_0.reset();
+while (true) {
+}
+
 }
\ No newline at end of file