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@22:ae8012b12890, 2015-10-12 (annotated)

Committer:
ewoud
Date:
Mon Oct 12 09:33:34 2015 +0000
Revision:
22:ae8012b12890
Parent:
20:8064435d21da 
integration issues

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