Chen Huan
/
MotorPID
闭环步进电机
main.cpp@0:5b4f19f8cd85, 2018-03-30 (annotated)
- Committer:
- heroistired
- Date:
- Fri Mar 30 12:03:12 2018 +0000
- Revision:
- 0:5b4f19f8cd85
- Child:
- 1:cbd6a3232d5b
??????
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:5b4f19f8cd85 | 5 | #include <string> |
heroistired | 0:5b4f19f8cd85 | 6 | #include <sstream> |
heroistired | 0:5b4f19f8cd85 | 7 | |
heroistired | 0:5b4f19f8cd85 | 8 | // software ssel |
heroistired | 0:5b4f19f8cd85 | 9 | SPI motor_encoder(D11, D12, D13); // mosi, miso, sclk |
heroistired | 0:5b4f19f8cd85 | 10 | DigitalOut motor_encoder_cs(D3); // ssel |
heroistired | 0:5b4f19f8cd85 | 11 | DigitalOut motor_dir(D2); // ssel |
heroistired | 0:5b4f19f8cd85 | 12 | Serial pc(SERIAL_TX, SERIAL_RX); |
heroistired | 0:5b4f19f8cd85 | 13 | MotorType Motor; |
heroistired | 0:5b4f19f8cd85 | 14 | Ticker PIDInterrupt; //PID中断 500Hz |
heroistired | 0:5b4f19f8cd85 | 15 | |
heroistired | 0:5b4f19f8cd85 | 16 | void PIDInterruptIRQ(); |
heroistired | 0:5b4f19f8cd85 | 17 | |
heroistired | 0:5b4f19f8cd85 | 18 | |
heroistired | 0:5b4f19f8cd85 | 19 | int main() |
heroistired | 0:5b4f19f8cd85 | 20 | { |
heroistired | 0:5b4f19f8cd85 | 21 | string UartBuf = ""; |
heroistired | 0:5b4f19f8cd85 | 22 | char temp = 0; |
heroistired | 0:5b4f19f8cd85 | 23 | StepMotorInit(&Motor, &motor_dir, &motor_encoder_cs, &motor_encoder); |
heroistired | 0:5b4f19f8cd85 | 24 | Encoder_Init(&Motor); |
heroistired | 0:5b4f19f8cd85 | 25 | PIDInterrupt.attach(&PIDInterruptIRQ, 0.002); |
heroistired | 0:5b4f19f8cd85 | 26 | Motor.Goal = 5000; |
heroistired | 0:5b4f19f8cd85 | 27 | while(1) |
heroistired | 0:5b4f19f8cd85 | 28 | { |
heroistired | 0:5b4f19f8cd85 | 29 | if(pc.readable()) |
heroistired | 0:5b4f19f8cd85 | 30 | { |
heroistired | 0:5b4f19f8cd85 | 31 | temp = pc.getc(); |
heroistired | 0:5b4f19f8cd85 | 32 | if((temp != 'i') && (temp != ';') && ((temp < '0') || (temp > '9'))) //收到的非法字符 |
heroistired | 0:5b4f19f8cd85 | 33 | continue; |
heroistired | 0:5b4f19f8cd85 | 34 | else if(temp == 'i') //请求信息 |
heroistired | 0:5b4f19f8cd85 | 35 | pc.printf("Position : %d, PWM Freq : %d, Goal : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Goal); |
heroistired | 0:5b4f19f8cd85 | 36 | else if((temp >= '0') && (temp <= '9')) //收到0-9 |
heroistired | 0:5b4f19f8cd85 | 37 | UartBuf = UartBuf + temp; |
heroistired | 0:5b4f19f8cd85 | 38 | else //收到 ; |
heroistired | 0:5b4f19f8cd85 | 39 | { |
heroistired | 0:5b4f19f8cd85 | 40 | Motor.Goal = atoi(UartBuf.c_str()); |
heroistired | 0:5b4f19f8cd85 | 41 | pc.printf("Goal is set at %d\r\n", Motor.Goal); |
heroistired | 0:5b4f19f8cd85 | 42 | UartBuf = ""; |
heroistired | 0:5b4f19f8cd85 | 43 | } |
heroistired | 0:5b4f19f8cd85 | 44 | } |
heroistired | 0:5b4f19f8cd85 | 45 | //pc.printf("Angle : %d, PID Value : %d, Difference : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Difference); |
heroistired | 0:5b4f19f8cd85 | 46 | } |
heroistired | 0:5b4f19f8cd85 | 47 | } |
heroistired | 0:5b4f19f8cd85 | 48 | |
heroistired | 0:5b4f19f8cd85 | 49 | void PIDInterruptIRQ() //PID中断服务函数 |
heroistired | 0:5b4f19f8cd85 | 50 | { |
heroistired | 0:5b4f19f8cd85 | 51 | //缓存上一次的编码器读数 |
heroistired | 0:5b4f19f8cd85 | 52 | Motor.EncoderPre = Motor.Encoder; |
heroistired | 0:5b4f19f8cd85 | 53 | //更新编码器读数 |
heroistired | 0:5b4f19f8cd85 | 54 | Motor.Encoder = Encoder_ReadData(&Motor); |
heroistired | 0:5b4f19f8cd85 | 55 | //对编码器读数低通滤波 当快过零点的时候不能滤波,过零点时的编码器数据突变会干扰滤波值 |
heroistired | 0:5b4f19f8cd85 | 56 | if((Motor.Encoder <= 16200) && (Motor.Encoder >= 100)) |
heroistired | 0:5b4f19f8cd85 | 57 | Motor.Encoder = Fliter_1(Motor.Encoder, Motor.EncoderPre); |
heroistired | 0:5b4f19f8cd85 | 58 | //速度-位置环PID控制 |
heroistired | 0:5b4f19f8cd85 | 59 | MotorPIDController(&Motor); |
heroistired | 0:5b4f19f8cd85 | 60 | //更新脉冲频率 |
heroistired | 0:5b4f19f8cd85 | 61 | StepMotorSetFreq(&Motor); |
heroistired | 0:5b4f19f8cd85 | 62 | } |
heroistired | 0:5b4f19f8cd85 | 63 | |
heroistired | 0:5b4f19f8cd85 | 64 |