QAQ ==!
Dependencies: mbed QEI-1 nRF24L01P xiugai
main.cpp@3:ee5e434e047e, 2019-10-16 (annotated)
- Committer:
- AlexQian
- Date:
- Wed Oct 16 13:59:33 2019 +0000
- Revision:
- 3:ee5e434e047e
- Parent:
- 2:af377291f3ae
- Child:
- 4:652d2be11b35
Encoder+PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AlexQian | 3:ee5e434e047e | 1 | #define HIGH 1 |
AlexQian | 3:ee5e434e047e | 2 | #define LOW 0 |
glintligo | 0:e63858fec119 | 3 | #include "mbed.h" |
AlexQian | 3:ee5e434e047e | 4 | #include <string> |
AlexQian | 3:ee5e434e047e | 5 | typedef bool boolean; |
AlexQian | 3:ee5e434e047e | 6 | typedef std::string String; |
glintligo | 0:e63858fec119 | 7 | #include "QEI.h" |
brainliang | 2:af377291f3ae | 8 | |
AlexQian | 3:ee5e434e047e | 9 | int Position; |
AlexQian | 3:ee5e434e047e | 10 | float Error; |
AlexQian | 3:ee5e434e047e | 11 | int Output; |
AlexQian | 3:ee5e434e047e | 12 | float Error_Last; |
AlexQian | 3:ee5e434e047e | 13 | float P; |
AlexQian | 3:ee5e434e047e | 14 | float Error_int; |
AlexQian | 3:ee5e434e047e | 15 | float I; |
AlexQian | 3:ee5e434e047e | 16 | float Error_diff; |
AlexQian | 3:ee5e434e047e | 17 | float D; |
brainliang | 2:af377291f3ae | 18 | |
AlexQian | 3:ee5e434e047e | 19 | QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING); |
AlexQian | 3:ee5e434e047e | 20 | Serial Serial_2(PA_2,PA_3); |
AlexQian | 3:ee5e434e047e | 21 | Ticker tick622103; |
AlexQian | 3:ee5e434e047e | 22 | PwmOut myServoPB_0(PB_0); |
AlexQian | 3:ee5e434e047e | 23 | PwmOut myServoPB_1(PB_1); |
brainliang | 2:af377291f3ae | 24 | |
AlexQian | 3:ee5e434e047e | 25 | |
brainliang | 2:af377291f3ae | 26 | |
AlexQian | 3:ee5e434e047e | 27 | void PID_Caculation() { |
AlexQian | 3:ee5e434e047e | 28 | Error = 0 - Position; |
AlexQian | 3:ee5e434e047e | 29 | Error_diff = Error - Error_Last; |
AlexQian | 3:ee5e434e047e | 30 | Error_Last = Error; |
AlexQian | 3:ee5e434e047e | 31 | Error_int = Error_int + Error; |
AlexQian | 3:ee5e434e047e | 32 | if (Error_int > 100) { |
AlexQian | 3:ee5e434e047e | 33 | Error_int = 100; |
AlexQian | 3:ee5e434e047e | 34 | } else if (Error_int < -100) { |
AlexQian | 3:ee5e434e047e | 35 | Error_int = -100; |
AlexQian | 3:ee5e434e047e | 36 | } |
AlexQian | 3:ee5e434e047e | 37 | if (Error > -10 && Error < 10) { |
AlexQian | 3:ee5e434e047e | 38 | Output = 0; |
AlexQian | 3:ee5e434e047e | 39 | } else { |
AlexQian | 3:ee5e434e047e | 40 | Output = P * Error + (I * Error_int + D * Error_diff); |
AlexQian | 3:ee5e434e047e | 41 | } |
AlexQian | 3:ee5e434e047e | 42 | if (Output > 100) { |
AlexQian | 3:ee5e434e047e | 43 | Output = 100; |
AlexQian | 3:ee5e434e047e | 44 | } else if (Output < -100) { |
AlexQian | 3:ee5e434e047e | 45 | Output = -100; |
AlexQian | 3:ee5e434e047e | 46 | } |
glintligo | 0:e63858fec119 | 47 | } |
glintligo | 1:1e3eb2d1496b | 48 | |
AlexQian | 3:ee5e434e047e | 49 | void Set_speed() { |
AlexQian | 3:ee5e434e047e | 50 | if (Output >= 0) { |
AlexQian | 3:ee5e434e047e | 51 | myServoPB_0.period_ms(20); |
AlexQian | 3:ee5e434e047e | 52 | myServoPB_0.pulsewidth_us((200 * Output)); |
AlexQian | 3:ee5e434e047e | 53 | myServoPB_1.period_ms(20); |
AlexQian | 3:ee5e434e047e | 54 | myServoPB_1.pulsewidth_us(0); |
AlexQian | 3:ee5e434e047e | 55 | } else if (Output < 0) { |
AlexQian | 3:ee5e434e047e | 56 | myServoPB_0.period_ms(20); |
AlexQian | 3:ee5e434e047e | 57 | myServoPB_0.pulsewidth_us(0); |
AlexQian | 3:ee5e434e047e | 58 | myServoPB_1.period_ms(20); |
AlexQian | 3:ee5e434e047e | 59 | myServoPB_1.pulsewidth_us((-200 * Output)); |
AlexQian | 3:ee5e434e047e | 60 | } |
glintligo | 1:1e3eb2d1496b | 61 | } |
glintligo | 1:1e3eb2d1496b | 62 | |
AlexQian | 3:ee5e434e047e | 63 | void tick622103_handle() { |
AlexQian | 3:ee5e434e047e | 64 | Position = Position + qei_PA_0.getPulses(); |
AlexQian | 3:ee5e434e047e | 65 | qei_PA_0.reset(); |
AlexQian | 3:ee5e434e047e | 66 | Serial_2.printf("%d\n",Position); |
AlexQian | 3:ee5e434e047e | 67 | PID_Caculation(); |
AlexQian | 3:ee5e434e047e | 68 | Set_speed(); |
AlexQian | 3:ee5e434e047e | 69 | } |
AlexQian | 3:ee5e434e047e | 70 | |
AlexQian | 3:ee5e434e047e | 71 | |
AlexQian | 3:ee5e434e047e | 72 | int main() { |
AlexQian | 3:ee5e434e047e | 73 | Serial_2.baud(9600); |
AlexQian | 3:ee5e434e047e | 74 | |
AlexQian | 3:ee5e434e047e | 75 | tick622103.attach(&tick622103_handle,0.05); |
AlexQian | 3:ee5e434e047e | 76 | |
AlexQian | 3:ee5e434e047e | 77 | Position = 0; |
AlexQian | 3:ee5e434e047e | 78 | qei_PA_0.reset(); |
AlexQian | 3:ee5e434e047e | 79 | while (true) { |
AlexQian | 3:ee5e434e047e | 80 | } |
AlexQian | 3:ee5e434e047e | 81 | |
glintligo | 1:1e3eb2d1496b | 82 | } |