![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
闭环步进电机
Diff: main.cpp
- Revision:
- 0:5b4f19f8cd85
- Child:
- 1:cbd6a3232d5b
diff -r 000000000000 -r 5b4f19f8cd85 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 30 12:03:12 2018 +0000 @@ -0,0 +1,64 @@ +// Send a byte to a SPI slave, and record the response + +#include "mbed.h" +#include "as5047.h" +#include <string> +#include <sstream> + +// software ssel +SPI motor_encoder(D11, D12, D13); // mosi, miso, sclk +DigitalOut motor_encoder_cs(D3); // ssel +DigitalOut motor_dir(D2); // ssel +Serial pc(SERIAL_TX, SERIAL_RX); +MotorType Motor; +Ticker PIDInterrupt; //PID中断 500Hz + +void PIDInterruptIRQ(); + + +int main() +{ + string UartBuf = ""; + char temp = 0; + 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()) + { + temp = pc.getc(); + if((temp != 'i') && (temp != ';') && ((temp < '0') || (temp > '9'))) //收到的非法字符 + continue; + else if(temp == 'i') //请求信息 + pc.printf("Position : %d, PWM Freq : %d, Goal : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Goal); + else if((temp >= '0') && (temp <= '9')) //收到0-9 + UartBuf = UartBuf + temp; + else //收到 ; + { + Motor.Goal = atoi(UartBuf.c_str()); + pc.printf("Goal is set at %d\r\n", Motor.Goal); + UartBuf = ""; + } + } + //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); +} + +