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@8:b0971033dc41, 2015-10-26 (annotated)

Committer:
ewoud
Date:
Mon Oct 26 11:15:31 2015 +0000
Revision:
8:b0971033dc41
Parent:
7:572e7f3e184a
Child:
9:3e19a344c025
working a bit stuttery

Who changed what in which revision?

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