Chen Huan
/
MotorPID
闭环步进电机
main.cpp
- Committer:
- heroistired
- Date:
- 2018-03-30
- Revision:
- 0:5b4f19f8cd85
- Child:
- 1:cbd6a3232d5b
File content as of revision 0:5b4f19f8cd85:
// 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); }