liang brain / Mbed 2 deprecated EX_encoder_PID_QianYuyangV4

nRF24L01, encoder, pca9685, pid

Dependencies:   mbed QEI-1 LibPN532 nRF24L01P xiugai

main.cpp@6:7db9b13ece76, 2019-11-03 (annotated)

Committer:
brainliang
Date:
Sun Nov 03 08:26:46 2019 +0000
Revision:
6:7db9b13ece76
Parent:
4:652d2be11b35
Child:
7:6f8157c58241
encoder+pid+pca9685+nrf24l01

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 6:7db9b13ece76 8 #include "converters.h"
brainliang 2:af377291f3ae 9
AlexQian 3:ee5e434e047e 10 int Position;
AlexQian 3:ee5e434e047e 11 float Error;
AlexQian 3:ee5e434e047e 12 int Output;
AlexQian 3:ee5e434e047e 13 float Error_Last;
AlexQian 3:ee5e434e047e 14 float P;
AlexQian 3:ee5e434e047e 15 float Error_int;
AlexQian 3:ee5e434e047e 16 float I;
AlexQian 3:ee5e434e047e 17 float Error_diff;
AlexQian 3:ee5e434e047e 18 float D;
brainliang 2:af377291f3ae 19
AlexQian 3:ee5e434e047e 20 QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
AlexQian 3:ee5e434e047e 21 Serial Serial_2(PA_2,PA_3);
AlexQian 3:ee5e434e047e 22 PwmOut myServoPB_0(PB_0);
AlexQian 3:ee5e434e047e 23 PwmOut myServoPB_1(PB_1);
brainliang 6:7db9b13ece76 24 Ticker tick241376;
brainliang 2:af377291f3ae 25
AlexQian 3:ee5e434e047e 26 void PID_Caculation() {
AlexQian 3:ee5e434e047e 27 Error = 0 - Position;
AlexQian 3:ee5e434e047e 28 Error_diff = Error - Error_Last;
AlexQian 3:ee5e434e047e 29 Error_Last = Error;
AlexQian 3:ee5e434e047e 30 Error_int = Error_int + Error;
AlexQian 3:ee5e434e047e 31 if (Error_int > 100) {
AlexQian 3:ee5e434e047e 32 Error_int = 100;
AlexQian 3:ee5e434e047e 33 } else if (Error_int < -100) {
AlexQian 3:ee5e434e047e 34 Error_int = -100;
AlexQian 3:ee5e434e047e 35 }
AlexQian 3:ee5e434e047e 36 if (Error > -10 && Error < 10) {
AlexQian 3:ee5e434e047e 37 Output = 0;
AlexQian 3:ee5e434e047e 38 } else {
AlexQian 3:ee5e434e047e 39 Output = P * Error + (I * Error_int + D * Error_diff);
AlexQian 3:ee5e434e047e 40 }
AlexQian 3:ee5e434e047e 41 if (Output > 100) {
AlexQian 3:ee5e434e047e 42 Output = 100;
AlexQian 3:ee5e434e047e 43 } else if (Output < -100) {
AlexQian 3:ee5e434e047e 44 Output = -100;
AlexQian 3:ee5e434e047e 45 }
glintligo 0:e63858fec119 46 }
glintligo 1:1e3eb2d1496b 47
AlexQian 3:ee5e434e047e 48 void Set_speed() {
AlexQian 3:ee5e434e047e 49 if (Output >= 0) {
AlexQian 3:ee5e434e047e 50 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 51 myServoPB_0.pulsewidth_us((200 * Output));
AlexQian 3:ee5e434e047e 52 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 53 myServoPB_1.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 54 } else if (Output < 0) {
AlexQian 3:ee5e434e047e 55 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 56 myServoPB_0.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 57 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 58 myServoPB_1.pulsewidth_us((-200 * Output));
AlexQian 3:ee5e434e047e 59 }
glintligo 1:1e3eb2d1496b 60 }
glintligo 1:1e3eb2d1496b 61
brainliang 6:7db9b13ece76 62 void tick241376_handle() {
AlexQian 3:ee5e434e047e 63 Position = Position + qei_PA_0.getPulses();
AlexQian 3:ee5e434e047e 64 qei_PA_0.reset();
brainliang 6:7db9b13ece76 65 Serial_2.printf("%d\n",_p(Position));
AlexQian 3:ee5e434e047e 66 PID_Caculation();
AlexQian 3:ee5e434e047e 67 Set_speed();
AlexQian 3:ee5e434e047e 68 }
AlexQian 3:ee5e434e047e 69
AlexQian 3:ee5e434e047e 70
AlexQian 3:ee5e434e047e 71 int main() {
brainliang 6:7db9b13ece76 72
brainliang 6:7db9b13ece76 73
AlexQian 3:ee5e434e047e 74 Serial_2.baud(9600);
AlexQian 3:ee5e434e047e 75
brainliang 6:7db9b13ece76 76 tick241376.attach(&tick241376_handle,0.05);
AlexQian 3:ee5e434e047e 77 Position = 0;
AlexQian 3:ee5e434e047e 78 qei_PA_0.reset();
brainliang 4:652d2be11b35 79 P = 1;
brainliang 4:652d2be11b35 80 I = 0;
brainliang 6:7db9b13ece76 81 D = 1;
AlexQian 3:ee5e434e047e 82 while (true) {
AlexQian 3:ee5e434e047e 83 }
AlexQian 3:ee5e434e047e 84
glintligo 1:1e3eb2d1496b 85 }