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@10:819fb5288aa0, 2015-10-27 (annotated)

Committer:
ewoud
Date:
Tue Oct 27 17:18:17 2015 +0000
Revision:
10:819fb5288aa0
Parent:
9:3e19a344c025
Child:
11:c5042e19a096
working very smooth. fixed float->int problem, correct lcd working

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