main.cpp

00001 // Send a byte to a SPI slave, and record the response
00002 
00003 #include "mbed.h"
00004 #include "as5047.h"
00005 #include "GUI.h"
00006 #include <string> 
00007 #include <sstream>
00008 
00009 // software ssel
00010 SPI motor_encoder(PB_15, PB_14, PB_13); // mosi, miso, sclk
00011 DigitalOut motor_encoder_cs(PB_12); // ssel
00012 DigitalOut motor_dir(PA_11); // ssel
00013 Serial pc(PA_2, PA_3,9600);
00014 Serial screen(PB_10, PB_11,115200);
00015 MotorType Motor;
00016 Ticker PIDInterrupt;  //PID中断 500Hz
00017 
00018 int counter = 0;
00019 
00020 DigitalIn Mode(PA_12);
00021 
00022 void PIDInterruptIRQ(); 
00023 
00024 
00025 int main() 
00026 {
00027     string UartBuf = "";
00028     char temp = 0;
00029     InitGUI(&screen);
00030     StepMotorInit(&Motor, &motor_dir, &motor_encoder_cs, &motor_encoder);
00031     Encoder_Init(&Motor);
00032     PIDInterrupt.attach(&PIDInterruptIRQ, 0.002);
00033     Motor.Goal = 5000;
00034     while(1)
00035     {
00036         if(pc.readable())// && (Mode.read() != 0))
00037         {
00038             temp = pc.getc();
00039             if((temp != 'p') && (temp != 'i') && (temp != 'x') && (temp != ';') && ((temp < '0') || (temp > '9')))   //收到的非法字符
00040                 continue;
00041             else if(temp == 'x')  //请求信息
00042                 pc.printf("Position : %d, PWM Freq : %d, Goal : %d Kp : %.2f Ki : %.2f\r\n", Motor.Encoder, Motor.PIDControl, Motor.Goal, Motor.Kp, Motor.Ki);
00043             else if((temp >= '0') && (temp <= '9'))  //收到0-9
00044                 UartBuf = UartBuf + temp;
00045             else if (temp == ';') //收到 ;
00046             {
00047                 Motor.Goal = atoi(UartBuf.c_str());
00048                 pc.printf("Goal is set at %d\r\n", Motor.Goal);
00049                 UartBuf = "";
00050                 counter = 0;
00051             }
00052             else if (temp == 'p') //收到 ;
00053             {
00054                 Motor.Kp = (float)atoi(UartBuf.c_str())/10;
00055                 pc.printf("Kp is set at %.2f\r\n", Motor.Kp);
00056                 UartBuf = "";
00057             }
00058             else if (temp == 'i') //收到 ;
00059             {
00060                 Motor.Ki = (float)atoi(UartBuf.c_str())/10;
00061                 pc.printf("Ki is set at %.2f\r\n", Motor.Ki);
00062                 UartBuf = "";
00063             }
00064         }
00065         if(counter < 1500)
00066             RefreshGUI(&screen, Motor.PIDControl, Motor.Kp, Motor.Ki, Motor.Goal, Motor.Encoder);
00067         //pc.printf("Angle : %d, PID Value : %d, Difference : %d\r\n", Motor.Encoder, Motor.PIDControl, Motor.Difference);
00068     }
00069 }
00070 
00071 void PIDInterruptIRQ()  //PID中断服务函数
00072 {   
00073     //缓存上一次的编码器读数
00074     Motor.EncoderPre = Motor.Encoder;
00075     //更新编码器读数
00076     Motor.Encoder = Encoder_ReadData(&Motor);
00077     //对编码器读数低通滤波 当快过零点的时候不能滤波，过零点时的编码器数据突变会干扰滤波值
00078     if((Motor.Encoder <= 16200) && (Motor.Encoder >= 100))
00079         Motor.Encoder = Fliter_1(Motor.Encoder, Motor.EncoderPre);
00080     //速度-位置环PID控制
00081     MotorPIDController(&Motor);
00082     //更新脉冲频率
00083     StepMotorSetFreq(&Motor);
00084     /*if(Mode.read() == 0)
00085     {
00086         MotorPIDController(&Motor);
00087         //更新脉冲频率
00088         StepMotorSetFreq(&Motor);
00089     }*/
00090     counter++;
00091     if(counter > 60000)
00092         counter = 2001;
00093 }
00094 
00095