Diff: main.cpp

Revision:

0:5b4f19f8cd85

Child:

1:cbd6a3232d5b

--- /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);
+}
+
+