Numero Uno / Mbed 2 deprecated PID_VelocityExample

motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

main.cpp@13:40141b362092, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 11:30:38 2015 +0000
Revision:
13:40141b362092
Parent:
12:d7bb475bb82d
Child:
14:102a2b4f5c86
Child:
20:8064435d21da
added start and stop buttons

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:9bca35ae9c6b 1 //****************************************************************************/
aberk 0:9bca35ae9c6b 2 // Includes
aberk 0:9bca35ae9c6b 3 #include "PID.h"
aberk 0:9bca35ae9c6b 4 #include "QEI.h"
ewoud 9:07189a75e979 5 #include "HIDScope.h"
ewoud 9:07189a75e979 6 #include "inits.h" // all globals, pin and variable initialization
aberk 0:9bca35ae9c6b 7
ewoud 9:07189a75e979 8 void setGoFlag(){
ewoud 13:40141b362092 9 if (goFlag==true){
ewoud 13:40141b362092 10 // flag is already set true: code too slow or frequency too high
ewoud 13:40141b362092 11 }
ewoud 9:07189a75e979 12 goFlag=true;
ewoud 9:07189a75e979 13 }
aberk 0:9bca35ae9c6b 14
ewoud 13:40141b362092 15 void systemStart(){
ewoud 13:40141b362092 16 systemOn=true;
ewoud 13:40141b362092 17 }
ewoud 13:40141b362092 18 void systemStop(){
ewoud 13:40141b362092 19 systemOn=false;
ewoud 13:40141b362092 20 leftMotor=0;
ewoud 13:40141b362092 21 rightMotor=0;
ewoud 13:40141b362092 22 }
ewoud 7:e14e28d8cae3 23 int main() {
ewoud 7:e14e28d8cae3 24
ewoud 9:07189a75e979 25 // initialize (defined in inits.h)
ewoud 9:07189a75e979 26 initMotors();
ewoud 9:07189a75e979 27 initPIDs();
ewoud 9:07189a75e979 28
ewoud 9:07189a75e979 29 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 9:07189a75e979 30
ewoud 13:40141b362092 31 startButton.rise(&systemStart);
ewoud 13:40141b362092 32 stopButton.rise(&systemStop);
ewoud 13:40141b362092 33
ewoud 8:55ca92c0e39d 34 endTimer.start(); //Run for 100 seconds.
ewoud 4:be465e9a12cb 35 while (endTimer.read() < 100){
ewoud 13:40141b362092 36 if (goFlag==true && systemOn==true){
ewoud 9:07189a75e979 37 // get 'emg' signal
ewoud 9:07189a75e979 38 request_pos = pot1.read();
ewoud 9:07189a75e979 39 request_neg = pot2.read();
ewoud 9:07189a75e979 40
ewoud 9:07189a75e979 41 // determine if forward or backward signal is stronger and set reference
ewoud 9:07189a75e979 42 if (request_pos > request_neg){
ewoud 9:07189a75e979 43 request = request_pos;
ewoud 9:07189a75e979 44 }
ewoud 9:07189a75e979 45 else {
ewoud 9:07189a75e979 46 request = -request_neg;
ewoud 9:07189a75e979 47 }
ewoud 9:07189a75e979 48 leftController.setSetPoint(request);
ewoud 12:d7bb475bb82d 49 rightController.setSetPoint(request);
ewoud 9:07189a75e979 50
ewoud 12:d7bb475bb82d 51 // *******************
ewoud 9:07189a75e979 52 // Velocity calculation
ewoud 12:d7bb475bb82d 53 // left
ewoud 9:07189a75e979 54 leftPulses = leftQei.getPulses();
ewoud 9:07189a75e979 55 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
ewoud 9:07189a75e979 56 leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 9:07189a75e979 57 leftPrevPulses = leftPulses;
ewoud 12:d7bb475bb82d 58
ewoud 12:d7bb475bb82d 59 // right
ewoud 12:d7bb475bb82d 60 rightPulses = rightQei.getPulses();
ewoud 12:d7bb475bb82d 61 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
ewoud 12:d7bb475bb82d 62 rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 12:d7bb475bb82d 63 rightPrevPulses = rightPulses;
ewoud 12:d7bb475bb82d 64
ewoud 12:d7bb475bb82d 65 // ***********
ewoud 9:07189a75e979 66 // PID control
ewoud 12:d7bb475bb82d 67 // left
ewoud 9:07189a75e979 68 leftController.setProcessValue(leftVelocity);
ewoud 9:07189a75e979 69 leftPwmDuty = leftController.compute();
ewoud 9:07189a75e979 70 if (leftPwmDuty < 0){
ewoud 9:07189a75e979 71 leftDirection = 0;
ewoud 9:07189a75e979 72 leftMotor = -leftPwmDuty;
ewoud 9:07189a75e979 73 }
ewoud 9:07189a75e979 74 else {
ewoud 9:07189a75e979 75 leftDirection = 1;
ewoud 9:07189a75e979 76 leftMotor = leftPwmDuty;
ewoud 9:07189a75e979 77 }
ewoud 9:07189a75e979 78
ewoud 12:d7bb475bb82d 79 // right
ewoud 12:d7bb475bb82d 80 rightController.setProcessValue(rightVelocity);
ewoud 12:d7bb475bb82d 81 rightPwmDuty = rightController.compute();
ewoud 12:d7bb475bb82d 82 if (rightPwmDuty < 0){
ewoud 12:d7bb475bb82d 83 rightDirection = 0;
ewoud 12:d7bb475bb82d 84 rightMotor = -rightPwmDuty;
ewoud 12:d7bb475bb82d 85 }
ewoud 12:d7bb475bb82d 86 else {
ewoud 12:d7bb475bb82d 87 rightDirection = 1;
ewoud 12:d7bb475bb82d 88 rightMotor = rightPwmDuty;
ewoud 12:d7bb475bb82d 89 }
ewoud 9:07189a75e979 90
ewoud 12:d7bb475bb82d 91 // User feedback
ewoud 9:07189a75e979 92 scope.set(0, request);
ewoud 12:d7bb475bb82d 93 scope.set(1, rightPwmDuty);
ewoud 12:d7bb475bb82d 94 scope.set(2, rightVelocity);
ewoud 9:07189a75e979 95 scope.send();
ewoud 9:07189a75e979 96 pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 8:55ca92c0e39d 97
ewoud 9:07189a75e979 98 goFlag=false;
ewoud 7:e14e28d8cae3 99 }
aberk 0:9bca35ae9c6b 100 }
aberk 0:9bca35ae9c6b 101
aberk 0:9bca35ae9c6b 102 //Stop motors.
ewoud 2:b2ccd9f044bb 103 leftMotor = 0;
ewoud 12:d7bb475bb82d 104 rightMotor = 0;
aberk 0:9bca35ae9c6b 105
aberk 0:9bca35ae9c6b 106 }