Chen Huan / Mbed 2 deprecated MotorPID

闭环步进电机

Dependencies:   mbed

main.cpp@1:cbd6a3232d5b, 2018-04-05 (annotated)

Committer:
heroistired
Date:
Thu Apr 05 01:37:17 2018 +0000
Revision:
1:cbd6a3232d5b
Parent:
0:5b4f19f8cd85 
New

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heroistired 0:5b4f19f8cd85 1 // Send a byte to a SPI slave, and record the response
heroistired 0:5b4f19f8cd85 2
heroistired 0:5b4f19f8cd85 3 #include "mbed.h"
heroistired 0:5b4f19f8cd85 4 #include "as5047.h"
heroistired 1:cbd6a3232d5b 5 #include "GUI.h"
heroistired 0:5b4f19f8cd85 6 #include <string>
heroistired 0:5b4f19f8cd85 7 #include <sstream>
heroistired 0:5b4f19f8cd85 8
heroistired 0:5b4f19f8cd85 9 // software ssel
heroistired 1:cbd6a3232d5b 10 SPI motor_encoder(PB_15, PB_14, PB_13); // mosi, miso, sclk
heroistired 1:cbd6a3232d5b 11 DigitalOut motor_encoder_cs(PB_12); // ssel
heroistired 1:cbd6a3232d5b 12 DigitalOut motor_dir(PA_11); // ssel
heroistired 1:cbd6a3232d5b 13 Serial pc(PA_2, PA_3,9600);
heroistired 1:cbd6a3232d5b 14 Serial screen(PB_10, PB_11,115200);
heroistired 0:5b4f19f8cd85 15 MotorType Motor;
heroistired 0:5b4f19f8cd85 16 Ticker PIDInterrupt; //PID中断 500Hz
heroistired 0:5b4f19f8cd85 17
heroistired 1:cbd6a3232d5b 18 int counter = 0;
heroistired 1:cbd6a3232d5b 19
heroistired 1:cbd6a3232d5b 20 DigitalIn Mode(PA_12);
heroistired 1:cbd6a3232d5b 21
heroistired 0:5b4f19f8cd85 22 void PIDInterruptIRQ();
heroistired 0:5b4f19f8cd85 23
heroistired 0:5b4f19f8cd85 24
heroistired 0:5b4f19f8cd85 25 int main()
heroistired 0:5b4f19f8cd85 26 {
heroistired 0:5b4f19f8cd85 27 string UartBuf = "";
heroistired 0:5b4f19f8cd85 28 char temp = 0;
heroistired 1:cbd6a3232d5b 29 InitGUI(&screen);
heroistired 0:5b4f19f8cd85 30 StepMotorInit(&Motor, &motor_dir, &motor_encoder_cs, &motor_encoder);
heroistired 0:5b4f19f8cd85 31 Encoder_Init(&Motor);
heroistired 0:5b4f19f8cd85 32 PIDInterrupt.attach(&PIDInterruptIRQ, 0.002);
heroistired 0:5b4f19f8cd85 33 Motor.Goal = 5000;
heroistired 0:5b4f19f8cd85 34 while(1)
heroistired 0:5b4f19f8cd85 35 {
heroistired 1:cbd6a3232d5b 36 if(pc.readable())// && (Mode.read() != 0))
heroistired 0:5b4f19f8cd85 37 {
heroistired 0:5b4f19f8cd85 38 temp = pc.getc();
heroistired 1:cbd6a3232d5b 39 if((temp != 'p') && (temp != 'i') && (temp != 'x') && (temp != ';') && ((temp < '0') || (temp > '9'))) //收到的非法字符
heroistired 0:5b4f19f8cd85 40 continue;
heroistired 1:cbd6a3232d5b 41 else if(temp == 'x') //请求信息
heroistired 1:cbd6a3232d5b 42 pc.printf("Position : %d, PWM Freq : %d, Goal : %d Kp : %.2f Ki : %.2f\r\n", Motor.Encoder, Motor.PIDControl, Motor.Goal, Motor.Kp, Motor.Ki);
heroistired 0:5b4f19f8cd85 43 else if((temp >= '0') && (temp <= '9')) //收到0-9
heroistired 0:5b4f19f8cd85 44 UartBuf = UartBuf + temp;
heroistired 1:cbd6a3232d5b 45 else if (temp == ';') //收到 ;
heroistired 0:5b4f19f8cd85 46 {
heroistired 0:5b4f19f8cd85 47 Motor.Goal = atoi(UartBuf.c_str());
heroistired 0:5b4f19f8cd85 48 pc.printf("Goal is set at %d\r\n", Motor.Goal);
heroistired 0:5b4f19f8cd85 49 UartBuf = "";
heroistired 1:cbd6a3232d5b 50 counter = 0;
heroistired 1:cbd6a3232d5b 51 }
heroistired 1:cbd6a3232d5b 52 else if (temp == 'p') //收到 ;
heroistired 1:cbd6a3232d5b 53 {
heroistired 1:cbd6a3232d5b 54 Motor.Kp = (float)atoi(UartBuf.c_str())/10;
heroistired 1:cbd6a3232d5b 55 pc.printf("Kp is set at %.2f\r\n", Motor.Kp);
heroistired 1:cbd6a3232d5b 56 UartBuf = "";
heroistired 1:cbd6a3232d5b 57 }
heroistired 1:cbd6a3232d5b 58 else if (temp == 'i') //收到 ;
heroistired 1:cbd6a3232d5b 59 {
heroistired 1:cbd6a3232d5b 60 Motor.Ki = (float)atoi(UartBuf.c_str())/10;
heroistired 1:cbd6a3232d5b 61 pc.printf("Ki is set at %.2f\r\n", Motor.Ki);
heroistired 1:cbd6a3232d5b 62 UartBuf = "";
heroistired 0:5b4f19f8cd85 63 }
heroistired 0:5b4f19f8cd85 64 }
heroistired 1:cbd6a3232d5b 65 if(counter < 1500)
heroistired 1:cbd6a3232d5b 66 RefreshGUI(&screen, Motor.PIDControl, Motor.Kp, Motor.Ki, Motor.Goal, Motor.Encoder);
heroistired 0:5b4f19f8cd85 67 //pc.printf("Angle : %d, PID Value : %d, Difference : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Difference);
heroistired 0:5b4f19f8cd85 68 }
heroistired 0:5b4f19f8cd85 69 }
heroistired 0:5b4f19f8cd85 70
heroistired 0:5b4f19f8cd85 71 void PIDInterruptIRQ() //PID中断服务函数
heroistired 0:5b4f19f8cd85 72 {
heroistired 0:5b4f19f8cd85 73 //缓存上一次的编码器读数
heroistired 0:5b4f19f8cd85 74 Motor.EncoderPre = Motor.Encoder;
heroistired 0:5b4f19f8cd85 75 //更新编码器读数
heroistired 0:5b4f19f8cd85 76 Motor.Encoder = Encoder_ReadData(&Motor);
heroistired 0:5b4f19f8cd85 77 //对编码器读数低通滤波 当快过零点的时候不能滤波，过零点时的编码器数据突变会干扰滤波值
heroistired 0:5b4f19f8cd85 78 if((Motor.Encoder <= 16200) && (Motor.Encoder >= 100))
heroistired 0:5b4f19f8cd85 79 Motor.Encoder = Fliter_1(Motor.Encoder, Motor.EncoderPre);
heroistired 0:5b4f19f8cd85 80 //速度-位置环PID控制
heroistired 0:5b4f19f8cd85 81 MotorPIDController(&Motor);
heroistired 0:5b4f19f8cd85 82 //更新脉冲频率
heroistired 0:5b4f19f8cd85 83 StepMotorSetFreq(&Motor);
heroistired 1:cbd6a3232d5b 84 /*if(Mode.read() == 0)
heroistired 1:cbd6a3232d5b 85 {
heroistired 1:cbd6a3232d5b 86 MotorPIDController(&Motor);
heroistired 1:cbd6a3232d5b 87 //更新脉冲频率
heroistired 1:cbd6a3232d5b 88 StepMotorSetFreq(&Motor);
heroistired 1:cbd6a3232d5b 89 }*/
heroistired 1:cbd6a3232d5b 90 counter++;
heroistired 1:cbd6a3232d5b 91 if(counter > 60000)
heroistired 1:cbd6a3232d5b 92 counter = 2001;
heroistired 0:5b4f19f8cd85 93 }
heroistired 0:5b4f19f8cd85 94
heroistired 0:5b4f19f8cd85 95