liang brain / Mbed 2 deprecated EX_encoder_PID_QianYuyangV4

nRF24L01, encoder, pca9685, pid

Dependencies:   mbed QEI-1 LibPN532 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?

UserRevisionLine numberNew 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 }