Numero Uno / Mbed 2 deprecated TheProgram

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EMG HIDScope PID QEI mbed TextLCD

main.cpp@7:572e7f3e184a, 2015-10-20 (annotated)

Committer:
ewoud
Date:
Tue Oct 20 08:21:50 2015 +0000
Revision:
7:572e7f3e184a
Parent:
6:ae2ce89dd695
Child:
8:b0971033dc41
pot meters to adjust speed of motors after button press

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 0:ca94aa9bf823 1 //****************************************************************************/
ewoud 0:ca94aa9bf823 2 // Includes
ewoud 0:ca94aa9bf823 3 #include "mbed.h"
ewoud 0:ca94aa9bf823 4 #include "PID.h"
ewoud 0:ca94aa9bf823 5 #include "QEI.h"
ewoud 0:ca94aa9bf823 6 #include "HIDScope.h"
ewoud 0:ca94aa9bf823 7 #include "biquadFilter.h"
ewoud 0:ca94aa9bf823 8 #include "inits.h" // all globals, pin and variable initialization
ewoud 0:ca94aa9bf823 9 #include "EMG.h"
ewoud 0:ca94aa9bf823 10
ewoud 0:ca94aa9bf823 11 void setGoFlag(){
ewoud 0:ca94aa9bf823 12 if (goFlag==true){
ewoud 0:ca94aa9bf823 13 //pc.printf("rate too high, error in setGoFlag \n\r");
ewoud 0:ca94aa9bf823 14 // flag is already set true: code too slow or frequency too high
ewoud 0:ca94aa9bf823 15 }
ewoud 0:ca94aa9bf823 16 goFlag=true;
ewoud 0:ca94aa9bf823 17 }
ewoud 6:ae2ce89dd695 18 void initpositiongo(){
ewoud 6:ae2ce89dd695 19 if(initposition_go){initposition_go=false;}
ewoud 6:ae2ce89dd695 20 else {initposition_go=true;}
ewoud 6:ae2ce89dd695 21
ewoud 6:ae2ce89dd695 22 }
ewoud 0:ca94aa9bf823 23
ewoud 0:ca94aa9bf823 24 void systemStart(){
ewoud 0:ca94aa9bf823 25 systemOn=true;
ewoud 0:ca94aa9bf823 26 }
ewoud 0:ca94aa9bf823 27 void systemStop(){
ewoud 0:ca94aa9bf823 28 systemOn=false;
ewoud 0:ca94aa9bf823 29 pc.printf("system stopped\n\r");
ewoud 0:ca94aa9bf823 30 }
ewoud 1:dafb63606b66 31
ewoud 0:ca94aa9bf823 32 int main() {
ewoud 0:ca94aa9bf823 33
ewoud 0:ca94aa9bf823 34 // initialize (defined in inits.h)
ewoud 0:ca94aa9bf823 35 initMotors();
ewoud 0:ca94aa9bf823 36 initPIDs();
ewoud 0:ca94aa9bf823 37
ewoud 0:ca94aa9bf823 38 outofboundsled=1;
ewoud 0:ca94aa9bf823 39 edgeleft.mode(PullUp);
ewoud 0:ca94aa9bf823 40 edgeright.mode(PullUp);
ewoud 1:dafb63606b66 41
ewoud 1:dafb63606b66 42 // interrupts
ewoud 0:ca94aa9bf823 43 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 3:70f78cfc0f25 44 cali_button.rise(&calibratego);
ewoud 6:ae2ce89dd695 45 initpositionButton.rise(&initpositiongo);
ewoud 0:ca94aa9bf823 46 startButton.rise(&systemStart);
ewoud 0:ca94aa9bf823 47 stopButton.rise(&systemStop);
ewoud 0:ca94aa9bf823 48
ewoud 0:ca94aa9bf823 49 while (true){
ewoud 3:70f78cfc0f25 50 if (calibrate_go==true){
ewoud 3:70f78cfc0f25 51 calibrate();
ewoud 3:70f78cfc0f25 52
ewoud 3:70f78cfc0f25 53
ewoud 3:70f78cfc0f25 54 }
ewoud 0:ca94aa9bf823 55 if (goFlag==true && systemOn==true){
ewoud 0:ca94aa9bf823 56 /*********** Contents *************/
ewoud 0:ca94aa9bf823 57 // 1) get emg signal
ewoud 0:ca94aa9bf823 58 // 2) calculate desired angle velocities
ewoud 0:ca94aa9bf823 59 // 3) calculate current angle velocities
ewoud 1:dafb63606b66 60 // 4) pid controller output
ewoud 0:ca94aa9bf823 61 // 5) user output
ewoud 0:ca94aa9bf823 62
ewoud 1:dafb63606b66 63 // **************
ewoud 0:ca94aa9bf823 64 // get emg signal
ewoud 6:ae2ce89dd695 65 if(initposition_go){
ewoud 7:572e7f3e184a 66 leftRequest=pot1.read()*20-10;
ewoud 7:572e7f3e184a 67 rightRequest=pot2.read()*20-10;
ewoud 3:70f78cfc0f25 68 }
ewoud 0:ca94aa9bf823 69 else {
ewoud 6:ae2ce89dd695 70 sample_filter();
ewoud 6:ae2ce89dd695 71
ewoud 6:ae2ce89dd695 72 // determine if forward or backward signal is stronger
ewoud 6:ae2ce89dd695 73 if (y1 > y2){
ewoud 6:ae2ce89dd695 74 horrequest = y1;
ewoud 6:ae2ce89dd695 75 }
ewoud 6:ae2ce89dd695 76 else {
ewoud 6:ae2ce89dd695 77 horrequest = -y2;
ewoud 6:ae2ce89dd695 78 }
ewoud 6:ae2ce89dd695 79 if (y3 > y4){
ewoud 6:ae2ce89dd695 80 verrequest = y3;
ewoud 6:ae2ce89dd695 81 }
ewoud 6:ae2ce89dd695 82 else {
ewoud 6:ae2ce89dd695 83 verrequest = -y4;
ewoud 6:ae2ce89dd695 84 }
ewoud 6:ae2ce89dd695 85
ewoud 6:ae2ce89dd695 86 // perform stepping between boundries
ewoud 6:ae2ce89dd695 87 if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
ewoud 6:ae2ce89dd695 88 else if(horrequest > grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;}
ewoud 6:ae2ce89dd695 89 else {horrequest=0;}
ewoud 6:ae2ce89dd695 90
ewoud 6:ae2ce89dd695 91 if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
ewoud 6:ae2ce89dd695 92 else if(verrequest > grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;}
ewoud 6:ae2ce89dd695 93 else {verrequest=0;}
ewoud 3:70f78cfc0f25 94
ewoud 1:dafb63606b66 95
ewoud 6:ae2ce89dd695 96 horrequest=horrequest*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
ewoud 6:ae2ce89dd695 97 verrequest=verrequest*maxspeed;
ewoud 6:ae2ce89dd695 98
ewoud 6:ae2ce89dd695 99
ewoud 6:ae2ce89dd695 100 // ***************
ewoud 6:ae2ce89dd695 101 // calculate required rotational velocity from the requested horizontal velocity
ewoud 6:ae2ce89dd695 102 // first get the current position from the motor encoders and make them start at 45 degree.
ewoud 6:ae2ce89dd695 103 leftAngle=(leftQei.getPulses()/round)*360+45;
ewoud 6:ae2ce89dd695 104 rightAngle=(rightQei.getPulses()/round)*360+45;
ewoud 6:ae2ce89dd695 105
ewoud 6:ae2ce89dd695 106 // trigonometry to get xy position from angles (cm)
ewoud 6:ae2ce89dd695 107 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 6:ae2ce89dd695 108 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 6:ae2ce89dd695 109
ewoud 6:ae2ce89dd695 110 // restrict motion if edges are touched
ewoud 6:ae2ce89dd695 111 if (edgeleft==0){
ewoud 6:ae2ce89dd695 112 if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
ewoud 6:ae2ce89dd695 113 }
ewoud 6:ae2ce89dd695 114 if (edgeright==0){
ewoud 6:ae2ce89dd695 115 if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
ewoud 6:ae2ce89dd695 116 }
ewoud 6:ae2ce89dd695 117 if (edgetop==0){
ewoud 6:ae2ce89dd695 118 if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
ewoud 6:ae2ce89dd695 119 }
ewoud 6:ae2ce89dd695 120 if (edgebottom==0){
ewoud 6:ae2ce89dd695 121 if (verrequest < 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
ewoud 6:ae2ce89dd695 122 }
ewoud 6:ae2ce89dd695 123
ewoud 6:ae2ce89dd695 124 // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
ewoud 6:ae2ce89dd695 125 toX=currentX+horrequest*RATE;
ewoud 6:ae2ce89dd695 126 toY=currentY+verrequest*RATE; // should be vertical request*RATE
ewoud 6:ae2ce89dd695 127
ewoud 6:ae2ce89dd695 128 // trigonometry to get angles from xy new position (degree)
ewoud 6:ae2ce89dd695 129 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 6:ae2ce89dd695 130 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 6:ae2ce89dd695 131
ewoud 6:ae2ce89dd695 132 // restrict motion if angles out-of-bound
ewoud 6:ae2ce89dd695 133 if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
ewoud 6:ae2ce89dd695 134 if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
ewoud 6:ae2ce89dd695 135 if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
ewoud 6:ae2ce89dd695 136 if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
ewoud 6:ae2ce89dd695 137
ewoud 6:ae2ce89dd695 138 // restrict motion if position is out of reach of the arms
ewoud 6:ae2ce89dd695 139 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 6:ae2ce89dd695 140 //return 0;
ewoud 6:ae2ce89dd695 141 //}
ewoud 6:ae2ce89dd695 142 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 6:ae2ce89dd695 143 //return 0;
ewoud 6:ae2ce89dd695 144 //}
ewoud 6:ae2ce89dd695 145
ewoud 6:ae2ce89dd695 146 // calculate the neccesairy velocities to make these angles happen (degree/sec)
ewoud 6:ae2ce89dd695 147 leftRequest=(toLeftAngle-leftAngle)/RATE;
ewoud 6:ae2ce89dd695 148 rightRequest=(toRightAngle-rightAngle)/RATE;
ewoud 0:ca94aa9bf823 149 }
ewoud 7:572e7f3e184a 150 pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
ewoud 7:572e7f3e184a 151
ewoud 0:ca94aa9bf823 152 // set the setpoint to the pid controller
ewoud 0:ca94aa9bf823 153 leftController.setSetPoint(leftRequest);
ewoud 0:ca94aa9bf823 154 rightController.setSetPoint(rightRequest);
ewoud 0:ca94aa9bf823 155
ewoud 1:dafb63606b66 156 // **************
ewoud 0:ca94aa9bf823 157 // Velocity calculation
ewoud 1:dafb63606b66 158 // left, differentiate from encoders
ewoud 1:dafb63606b66 159 leftPulses = leftQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 160 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 161 leftPrevPulses = leftPulses;
ewoud 1:dafb63606b66 162 leftController.setProcessValue(leftVelocity); // set PID process value
ewoud 0:ca94aa9bf823 163
ewoud 0:ca94aa9bf823 164 // right
ewoud 1:dafb63606b66 165 rightPulses = rightQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 166 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 167 rightPrevPulses = rightPulses;
ewoud 1:dafb63606b66 168 rightController.setProcessValue(rightVelocity); // set PID process value
ewoud 0:ca94aa9bf823 169
ewoud 0:ca94aa9bf823 170
ewoud 1:dafb63606b66 171 // **************
ewoud 0:ca94aa9bf823 172 // PID control output
ewoud 0:ca94aa9bf823 173 // left
ewoud 0:ca94aa9bf823 174
ewoud 0:ca94aa9bf823 175 leftPwmDuty = leftController.compute();
ewoud 1:dafb63606b66 176 if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 0:ca94aa9bf823 177 leftDirection = 0;
ewoud 0:ca94aa9bf823 178 leftMotor = -leftPwmDuty;
ewoud 0:ca94aa9bf823 179 }
ewoud 0:ca94aa9bf823 180 else {
ewoud 0:ca94aa9bf823 181 leftDirection = 1;
ewoud 0:ca94aa9bf823 182 leftMotor = leftPwmDuty;
ewoud 0:ca94aa9bf823 183 }
ewoud 0:ca94aa9bf823 184
ewoud 0:ca94aa9bf823 185 // right
ewoud 0:ca94aa9bf823 186 rightPwmDuty = rightController.compute();
ewoud 1:dafb63606b66 187 if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 0:ca94aa9bf823 188 rightDirection = 1;
ewoud 0:ca94aa9bf823 189 rightMotor = -rightPwmDuty;
ewoud 0:ca94aa9bf823 190 }
ewoud 0:ca94aa9bf823 191 else {
ewoud 0:ca94aa9bf823 192 rightDirection = 0;
ewoud 0:ca94aa9bf823 193 rightMotor = rightPwmDuty;
ewoud 0:ca94aa9bf823 194 }
ewoud 0:ca94aa9bf823 195
ewoud 1:dafb63606b66 196 // ***************
ewoud 0:ca94aa9bf823 197 // User feedback
ewoud 3:70f78cfc0f25 198 scope.set(0, horrequest);
ewoud 0:ca94aa9bf823 199 scope.set(1, leftPwmDuty);
ewoud 0:ca94aa9bf823 200 scope.set(2, leftVelocity);
ewoud 0:ca94aa9bf823 201 scope.set(3, currentX);
ewoud 0:ca94aa9bf823 202 scope.set(4, currentY);
ewoud 0:ca94aa9bf823 203 scope.send();
ewoud 1:dafb63606b66 204
ewoud 0:ca94aa9bf823 205 goFlag=false;
ewoud 0:ca94aa9bf823 206 }
ewoud 0:ca94aa9bf823 207 if(systemOn==false)
ewoud 0:ca94aa9bf823 208 {
ewoud 0:ca94aa9bf823 209 leftMotor=0;
ewoud 0:ca94aa9bf823 210 rightMotor=0;
ewoud 0:ca94aa9bf823 211 }
ewoud 0:ca94aa9bf823 212 }
ewoud 0:ca94aa9bf823 213
ewoud 0:ca94aa9bf823 214 }