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@9:3e19a344c025, 2015-10-26 (annotated)

Committer:
ewoud
Date:
Mon Oct 26 14:26:13 2015 +0000
Revision:
9:3e19a344c025
Parent:
8:b0971033dc41
Child:
10:819fb5288aa0
moving works quite well, started on lcd output while playing

Who changed what in which revision?

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