QAQ ==!
Dependencies: mbed QEI-1 nRF24L01P xiugai
main.cpp
- Committer:
- brainliang
- Date:
- 2019-10-25
- Revision:
- 4:652d2be11b35
- Parent:
- 3:ee5e434e047e
- Child:
- 6:7db9b13ece76
File content as of revision 4:652d2be11b35:
#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); PwmOut myServoPB_0(PB_0); PwmOut myServoPB_1(PB_1); Ticker tick561436; 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 tick561436_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); tick561436.attach(&tick561436_handle,0.05); Position = 0; qei_PA_0.reset(); P = 1; I = 0; D = 10; while (true) { } }