liang brain / Mbed 2 deprecated EX_encoder_PID_QianYuyang

nRF24L01, encoder, pca9685, pid

Dependencies:   mbed QEI-1 nRF24L01P xiugai

main.cpp@4:652d2be11b35, 2019-10-25 (annotated)

Committer:
brainliang
Date:
Fri Oct 25 02:58:18 2019 +0000
Revision:
4:652d2be11b35
Parent:
3:ee5e434e047e 
nRF24L01,encoder,pid,pca9685

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 PwmOut myServoPB_0(PB_0);
AlexQian 3:ee5e434e047e 22 PwmOut myServoPB_1(PB_1);
brainliang 4:652d2be11b35 23 Ticker tick561436;
brainliang 2:af377291f3ae 24
AlexQian 3:ee5e434e047e 25 void PID_Caculation() {
AlexQian 3:ee5e434e047e 26 Error = 0 - Position;
AlexQian 3:ee5e434e047e 27 Error_diff = Error - Error_Last;
AlexQian 3:ee5e434e047e 28 Error_Last = Error;
AlexQian 3:ee5e434e047e 29 Error_int = Error_int + Error;
AlexQian 3:ee5e434e047e 30 if (Error_int > 100) {
AlexQian 3:ee5e434e047e 31 Error_int = 100;
AlexQian 3:ee5e434e047e 32 } else if (Error_int < -100) {
AlexQian 3:ee5e434e047e 33 Error_int = -100;
AlexQian 3:ee5e434e047e 34 }
AlexQian 3:ee5e434e047e 35 if (Error > -10 && Error < 10) {
AlexQian 3:ee5e434e047e 36 Output = 0;
AlexQian 3:ee5e434e047e 37 } else {
AlexQian 3:ee5e434e047e 38 Output = P * Error + (I * Error_int + D * Error_diff);
AlexQian 3:ee5e434e047e 39 }
AlexQian 3:ee5e434e047e 40 if (Output > 100) {
AlexQian 3:ee5e434e047e 41 Output = 100;
AlexQian 3:ee5e434e047e 42 } else if (Output < -100) {
AlexQian 3:ee5e434e047e 43 Output = -100;
AlexQian 3:ee5e434e047e 44 }
glintligo 0:e63858fec119 45 }
glintligo 1:1e3eb2d1496b 46
AlexQian 3:ee5e434e047e 47 void Set_speed() {
AlexQian 3:ee5e434e047e 48 if (Output >= 0) {
AlexQian 3:ee5e434e047e 49 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 50 myServoPB_0.pulsewidth_us((200 * Output));
AlexQian 3:ee5e434e047e 51 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 52 myServoPB_1.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 53 } else if (Output < 0) {
AlexQian 3:ee5e434e047e 54 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 55 myServoPB_0.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 56 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 57 myServoPB_1.pulsewidth_us((-200 * Output));
AlexQian 3:ee5e434e047e 58 }
glintligo 1:1e3eb2d1496b 59 }
glintligo 1:1e3eb2d1496b 60
brainliang 4:652d2be11b35 61 void tick561436_handle() {
AlexQian 3:ee5e434e047e 62 Position = Position + qei_PA_0.getPulses();
AlexQian 3:ee5e434e047e 63 qei_PA_0.reset();
AlexQian 3:ee5e434e047e 64 Serial_2.printf("%d\n",Position);
AlexQian 3:ee5e434e047e 65 PID_Caculation();
AlexQian 3:ee5e434e047e 66 Set_speed();
AlexQian 3:ee5e434e047e 67 }
AlexQian 3:ee5e434e047e 68
AlexQian 3:ee5e434e047e 69
AlexQian 3:ee5e434e047e 70 int main() {
AlexQian 3:ee5e434e047e 71 Serial_2.baud(9600);
AlexQian 3:ee5e434e047e 72
brainliang 4:652d2be11b35 73 tick561436.attach(&tick561436_handle,0.05);
AlexQian 3:ee5e434e047e 74 Position = 0;
AlexQian 3:ee5e434e047e 75 qei_PA_0.reset();
brainliang 4:652d2be11b35 76 P = 1;
brainliang 4:652d2be11b35 77 I = 0;
brainliang 4:652d2be11b35 78 D = 10;
AlexQian 3:ee5e434e047e 79 while (true) {
AlexQian 3:ee5e434e047e 80 }
AlexQian 3:ee5e434e047e 81
glintligo 1:1e3eb2d1496b 82 }