Chen Huan / Mbed 2 deprecated MotorPID

闭环步进电机

Dependencies:   mbed

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?

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 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