QAQ ==!
Dependencies: mbed QEI-1 nRF24L01P xiugai
main.cpp
- Committer:
- AlexQian
- Date:
- 2019-10-16
- Revision:
- 3:ee5e434e047e
- Parent:
- 2:af377291f3ae
- Child:
- 4:652d2be11b35
File content as of revision 3:ee5e434e047e:
#define HIGH 1 #define LOW 0 #include "mbed.h" #include <string> typedef bool boolean; typedef std::string String; #include "QEI.h" int Position; float Error; int Output; float Error_Last; float P; float Error_int; float I; float Error_diff; float D; 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); 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 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 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) { } }