QAQ ==!
Dependencies: mbed QEI-1 nRF24L01P xiugai
Diff: main.cpp
- Revision:
- 3:ee5e434e047e
- Parent:
- 2:af377291f3ae
- Child:
- 4:652d2be11b35
--- a/main.cpp Tue Oct 15 05:03:34 2019 +0000 +++ b/main.cpp Wed Oct 16 13:59:33 2019 +0000 @@ -1,75 +1,82 @@ +#define HIGH 1 +#define LOW 0 #include "mbed.h" +#include <string> +typedef bool boolean; +typedef std::string String; #include "QEI.h" -//两个轮子L298N -DigitalOut w1(PB_6); -DigitalOut w2(PB_7); -DigitalOut w3(PB_8); -DigitalOut w4(PB_9); -PwmOut mypwm1(PA_9); -PwmOut mypwm2(PA_8); -//调试串口 -Serial pc(PB_10, PB_11); +int Position; +float Error; +int Output; +float Error_Last; +float P; +float Error_int; +float I; +float Error_diff; +float D; -Ticker toggle_time_ticker; -//编码器1-4 -QEI wheel(PA_2, PA_3, NC, 11, QEI::X4_ENCODING); //买来的电机自带编码器,说明书上有电机每转一圈编码器发生11个信号 -QEI wheel2(PA_6, PA_7, NC, 11, QEI::X4_ENCODING); +QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING); +Serial Serial_2(PA_2,PA_3); +Ticker tick622103; +PwmOut myServoPB_0(PB_0); +PwmOut myServoPB_1(PB_1); -int x,y; -float a= 0.01, b = 0.00860454; -void time_ticker(); -void serialread(); + -int main() -{ - mypwm1.period_ms(10); - mypwm2.period_ms(10); - - toggle_time_ticker.attach(&time_ticker, 0.2); //使能ticker中断,0.2s产生一次.中断函数用于获取每个轮子的计数值 - - pc.attach(&serialread); - - mypwm1.pulsewidth(a); - mypwm2.pulsewidth(b); - wheel.reset(); //reset函数,将编码器计数值赋0(初始化编码器,开始计数) - wheel2.reset(); - w1 = 0; - w2 = 0; - w3 = 0; - w4 = 0; - while(1) - { - - } +void PID_Caculation() { +Error = 0 - Position; +Error_diff = Error - Error_Last; +Error_Last = Error; +Error_int = Error_int + Error; +if (Error_int > 100) { +Error_int = 100; +} else if (Error_int < -100) { +Error_int = -100; +} +if (Error > -10 && Error < 10) { +Output = 0; +} else { +Output = P * Error + (I * Error_int + D * Error_diff); +} +if (Output > 100) { +Output = 100; +} else if (Output < -100) { +Output = -100; +} } -void time_ticker(){ - //获取一个轮子的编码器读到的数和旋转方向 - x = wheel.getPulses(); - x = x/44; //将编码器计数值转化成轮子旋转的圈数 - wheel.getState(); //获取旋转方向,返回值为1--顺时针, -1--逆时针 - y = wheel2.getPulses(); - y = y/44; //将编码器计数值转化成轮子旋转的圈数 - wheel2.getState(); //获取旋转方向,返回值为1--顺时针, -1--逆时针 - - wheel.reset(); - wheel2.reset(); - pc.printf("x:%d y:%d\n",x,y); +void Set_speed() { +if (Output >= 0) { +myServoPB_0.period_ms(20); +myServoPB_0.pulsewidth_us((200 * Output)); +myServoPB_1.period_ms(20); +myServoPB_1.pulsewidth_us(0); +} else if (Output < 0) { +myServoPB_0.period_ms(20); +myServoPB_0.pulsewidth_us(0); +myServoPB_1.period_ms(20); +myServoPB_1.pulsewidth_us((-200 * Output)); +} } -void serialread(){ - switch (pc.getc()) - { - case '1': - w1=0; - w4=0; - break; - case '2': - w1=1; - w4=1; - break; - default: - break; - } +void tick622103_handle() { +Position = Position + qei_PA_0.getPulses(); +qei_PA_0.reset(); +Serial_2.printf("%d\n",Position); +PID_Caculation(); +Set_speed(); +} + + +int main() { +Serial_2.baud(9600); + +tick622103.attach(&tick622103_handle,0.05); + +Position = 0; +qei_PA_0.reset(); +while (true) { +} + } \ No newline at end of file