Dhiren Lad / PID_Test

code to go straight with pid

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@0:99dd5499928a, 2015-11-19 (annotated)

Committer:
mochaMan
Date:
Thu Nov 19 03:01:07 2015 +0000
Revision:
0:99dd5499928a
Child:
1:b9485bb1ca69
Because he said so

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mochaMan 0:99dd5499928a 1 #include "mbed.h"
mochaMan 0:99dd5499928a 2 #include "QEI.h"
mochaMan 0:99dd5499928a 3 #include "AVEncoder.h"
mochaMan 0:99dd5499928a 4
mochaMan 0:99dd5499928a 5 DigitalOut myled(LED1);
mochaMan 0:99dd5499928a 6 DigitalIn mybutton(USER_BUTTON);
mochaMan 0:99dd5499928a 7
mochaMan 0:99dd5499928a 8 PwmOut LMotorForward(PA_7);
mochaMan 0:99dd5499928a 9 PwmOut LMotorReverse(PB_6);
mochaMan 0:99dd5499928a 10
mochaMan 0:99dd5499928a 11 PwmOut RMotorForward(PB_10);
mochaMan 0:99dd5499928a 12 PwmOut RMotorReverse(PB_4);
mochaMan 0:99dd5499928a 13
mochaMan 0:99dd5499928a 14 QEI LeftEncoder(PA_15, PB_3, NC, 12);
mochaMan 0:99dd5499928a 15 QEI RightEncoder(PA_1, PA_2, NC, 12);
mochaMan 0:99dd5499928a 16
mochaMan 0:99dd5499928a 17 Serial pc(USBTX, USBRX);
mochaMan 0:99dd5499928a 18
mochaMan 0:99dd5499928a 19 void pid();
mochaMan 0:99dd5499928a 20 double P_controller(int error);
mochaMan 0:99dd5499928a 21 double D_controller(int error);
mochaMan 0:99dd5499928a 22 double I_controller(int error);
mochaMan 0:99dd5499928a 23 void setRightPwm(double speed);
mochaMan 0:99dd5499928a 24 void setLeftPwm(double speed);
mochaMan 0:99dd5499928a 25 void stop();
mochaMan 0:99dd5499928a 26
mochaMan 0:99dd5499928a 27 double speed = 0.5;
mochaMan 0:99dd5499928a 28 Ticker interrupt;
mochaMan 0:99dd5499928a 29 int STOP = 0;
mochaMan 0:99dd5499928a 30
mochaMan 0:99dd5499928a 31 int main()
mochaMan 0:99dd5499928a 32 {
mochaMan 0:99dd5499928a 33 myled = 1;
mochaMan 0:99dd5499928a 34 interrupt.attach_us(&pid, 1000);
mochaMan 0:99dd5499928a 35 while(mybutton == 1);
mochaMan 0:99dd5499928a 36
mochaMan 0:99dd5499928a 37 speed = 0.5;
mochaMan 0:99dd5499928a 38
mochaMan 0:99dd5499928a 39 while(true)
mochaMan 0:99dd5499928a 40 {
mochaMan 0:99dd5499928a 41 setLeftPwm(speed);
mochaMan 0:99dd5499928a 42 setRightPwm(speed);
mochaMan 0:99dd5499928a 43 }
mochaMan 0:99dd5499928a 44 }
mochaMan 0:99dd5499928a 45
mochaMan 0:99dd5499928a 46 void pid()
mochaMan 0:99dd5499928a 47 {
mochaMan 0:99dd5499928a 48 speed += P_controller(0) + D_controller(0) + I_controller(0);
mochaMan 0:99dd5499928a 49
mochaMan 0:99dd5499928a 50 if(speed < 0)
mochaMan 0:99dd5499928a 51 {
mochaMan 0:99dd5499928a 52 speed = 0;
mochaMan 0:99dd5499928a 53 }
mochaMan 0:99dd5499928a 54 else if(speed > 1)
mochaMan 0:99dd5499928a 55 {
mochaMan 0:99dd5499928a 56 speed = 1;
mochaMan 0:99dd5499928a 57 }
mochaMan 0:99dd5499928a 58 }
mochaMan 0:99dd5499928a 59
mochaMan 0:99dd5499928a 60 double P_controller(int error)
mochaMan 0:99dd5499928a 61 {
mochaMan 0:99dd5499928a 62 return -1;
mochaMan 0:99dd5499928a 63 }
mochaMan 0:99dd5499928a 64
mochaMan 0:99dd5499928a 65 double D_controller(int error)
mochaMan 0:99dd5499928a 66 {
mochaMan 0:99dd5499928a 67 return -1;
mochaMan 0:99dd5499928a 68 }
mochaMan 0:99dd5499928a 69
mochaMan 0:99dd5499928a 70 double I_controller(int error)
mochaMan 0:99dd5499928a 71 {
mochaMan 0:99dd5499928a 72 return -1;
mochaMan 0:99dd5499928a 73 }
mochaMan 0:99dd5499928a 74
mochaMan 0:99dd5499928a 75 void setLeftPwm(double speed)
mochaMan 0:99dd5499928a 76 {
mochaMan 0:99dd5499928a 77 if(speed == 0)
mochaMan 0:99dd5499928a 78 {
mochaMan 0:99dd5499928a 79 LMotorForward = 1.0;
mochaMan 0:99dd5499928a 80 LMotorReverse = 1.0;
mochaMan 0:99dd5499928a 81 }
mochaMan 0:99dd5499928a 82 if(speed > 0)
mochaMan 0:99dd5499928a 83 {
mochaMan 0:99dd5499928a 84 LMotorForward = speed;
mochaMan 0:99dd5499928a 85 LMotorReverse = 0;
mochaMan 0:99dd5499928a 86 }
mochaMan 0:99dd5499928a 87 if(speed < 0)
mochaMan 0:99dd5499928a 88 {
mochaMan 0:99dd5499928a 89 LMotorForward = 0;
mochaMan 0:99dd5499928a 90 LMotorReverse = -speed;
mochaMan 0:99dd5499928a 91 }
mochaMan 0:99dd5499928a 92 }
mochaMan 0:99dd5499928a 93
mochaMan 0:99dd5499928a 94 //todo: make this have ramping
mochaMan 0:99dd5499928a 95 void setRightPwm(double speed)
mochaMan 0:99dd5499928a 96 {
mochaMan 0:99dd5499928a 97 if(speed > 0)
mochaMan 0:99dd5499928a 98 {
mochaMan 0:99dd5499928a 99 RMotorForward = speed;
mochaMan 0:99dd5499928a 100 RMotorReverse = STOP;
mochaMan 0:99dd5499928a 101 }
mochaMan 0:99dd5499928a 102 if(speed < 0)
mochaMan 0:99dd5499928a 103 {
mochaMan 0:99dd5499928a 104 RMotorForward = STOP;
mochaMan 0:99dd5499928a 105 RMotorReverse = -speed;
mochaMan 0:99dd5499928a 106 }
mochaMan 0:99dd5499928a 107 }
mochaMan 0:99dd5499928a 108
mochaMan 0:99dd5499928a 109 void stop()
mochaMan 0:99dd5499928a 110 {
mochaMan 0:99dd5499928a 111 setLeftPwm(0);
mochaMan 0:99dd5499928a 112 setRightPwm(0);
mochaMan 0:99dd5499928a 113 }