Chen Huan
/
MotorPID
闭环步进电机
main.cpp
- Committer:
- heroistired
- Date:
- 2018-04-05
- Revision:
- 1:cbd6a3232d5b
- Parent:
- 0:5b4f19f8cd85
File content as of revision 1:cbd6a3232d5b:
// Send a byte to a SPI slave, and record the response #include "mbed.h" #include "as5047.h" #include "GUI.h" #include <string> #include <sstream> // software ssel SPI motor_encoder(PB_15, PB_14, PB_13); // mosi, miso, sclk DigitalOut motor_encoder_cs(PB_12); // ssel DigitalOut motor_dir(PA_11); // ssel Serial pc(PA_2, PA_3,9600); Serial screen(PB_10, PB_11,115200); MotorType Motor; Ticker PIDInterrupt; //PID中断 500Hz int counter = 0; DigitalIn Mode(PA_12); void PIDInterruptIRQ(); int main() { string UartBuf = ""; char temp = 0; InitGUI(&screen); StepMotorInit(&Motor, &motor_dir, &motor_encoder_cs, &motor_encoder); Encoder_Init(&Motor); PIDInterrupt.attach(&PIDInterruptIRQ, 0.002); Motor.Goal = 5000; while(1) { if(pc.readable())// && (Mode.read() != 0)) { temp = pc.getc(); if((temp != 'p') && (temp != 'i') && (temp != 'x') && (temp != ';') && ((temp < '0') || (temp > '9'))) //收到的非法字符 continue; else if(temp == 'x') //请求信息 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); else if((temp >= '0') && (temp <= '9')) //收到0-9 UartBuf = UartBuf + temp; else if (temp == ';') //收到 ; { Motor.Goal = atoi(UartBuf.c_str()); pc.printf("Goal is set at %d\r\n", Motor.Goal); UartBuf = ""; counter = 0; } else if (temp == 'p') //收到 ; { Motor.Kp = (float)atoi(UartBuf.c_str())/10; pc.printf("Kp is set at %.2f\r\n", Motor.Kp); UartBuf = ""; } else if (temp == 'i') //收到 ; { Motor.Ki = (float)atoi(UartBuf.c_str())/10; pc.printf("Ki is set at %.2f\r\n", Motor.Ki); UartBuf = ""; } } if(counter < 1500) RefreshGUI(&screen, Motor.PIDControl, Motor.Kp, Motor.Ki, Motor.Goal, Motor.Encoder); //pc.printf("Angle : %d, PID Value : %d, Difference : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Difference); } } void PIDInterruptIRQ() //PID中断服务函数 { //缓存上一次的编码器读数 Motor.EncoderPre = Motor.Encoder; //更新编码器读数 Motor.Encoder = Encoder_ReadData(&Motor); //对编码器读数低通滤波 当快过零点的时候不能滤波,过零点时的编码器数据突变会干扰滤波值 if((Motor.Encoder <= 16200) && (Motor.Encoder >= 100)) Motor.Encoder = Fliter_1(Motor.Encoder, Motor.EncoderPre); //速度-位置环PID控制 MotorPIDController(&Motor); //更新脉冲频率 StepMotorSetFreq(&Motor); /*if(Mode.read() == 0) { MotorPIDController(&Motor); //更新脉冲频率 StepMotorSetFreq(&Motor); }*/ counter++; if(counter > 60000) counter = 2001; }