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@11:c5042e19a096, 2015-11-02 (annotated)

Committer:
ewoud
Date:
Mon Nov 02 11:12:46 2015 +0000
Revision:
11:c5042e19a096
Parent:
10:819fb5288aa0 
The final thing

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 11:c5042e19a096 4 #include <stdio.h>
ewoud 11:c5042e19a096 5 #include <stdlib.h>
ewoud 11:c5042e19a096 6 #include <sstream>
ewoud 8:b0971033dc41 7 #include "TextLCD.h"
ewoud 0:ca94aa9bf823 8 #include "mbed.h"
ewoud 0:ca94aa9bf823 9 #include "PID.h"
ewoud 0:ca94aa9bf823 10 #include "QEI.h"
ewoud 0:ca94aa9bf823 11 #include "HIDScope.h"
ewoud 0:ca94aa9bf823 12 #include "biquadFilter.h"
ewoud 0:ca94aa9bf823 13 #include "inits.h" // all globals, pin and variable initialization
ewoud 0:ca94aa9bf823 14 #include "EMG.h"
ewoud 11:c5042e19a096 15 string intToString(int i)
ewoud 11:c5042e19a096 16 {
ewoud 11:c5042e19a096 17 std::stringstream ss;
ewoud 11:c5042e19a096 18 std::string s;
ewoud 11:c5042e19a096 19 ss << i;
ewoud 11:c5042e19a096 20 s = ss.str();
ewoud 0:ca94aa9bf823 21
ewoud 11:c5042e19a096 22 return s;
ewoud 11:c5042e19a096 23 }
ewoud 9:3e19a344c025 24 void setLcdFlag(){
ewoud 9:3e19a344c025 25 lcdGoFlag=true;
ewoud 9:3e19a344c025 26
ewoud 9:3e19a344c025 27 }
ewoud 0:ca94aa9bf823 28 void setGoFlag(){
ewoud 0:ca94aa9bf823 29 if (goFlag==true){
ewoud 0:ca94aa9bf823 30 //pc.printf("rate too high, error in setGoFlag \n\r");
ewoud 0:ca94aa9bf823 31 // flag is already set true: code too slow or frequency too high
ewoud 0:ca94aa9bf823 32 }
ewoud 0:ca94aa9bf823 33 goFlag=true;
ewoud 0:ca94aa9bf823 34 }
ewoud 6:ae2ce89dd695 35 void initpositiongo(){
ewoud 10:819fb5288aa0 36 if(initposition_go){initposition_go=false;
ewoud 10:819fb5288aa0 37 pc.printf("ended init process\n\r");}
ewoud 10:819fb5288aa0 38 else {initposition_go=true;
ewoud 10:819fb5288aa0 39 pc.printf("started init process\n\rleft arm: QAZ right arm: OKM");
ewoud 10:819fb5288aa0 40
ewoud 10:819fb5288aa0 41 }
ewoud 6:ae2ce89dd695 42 }
ewoud 0:ca94aa9bf823 43
ewoud 11:c5042e19a096 44 void showFinishScreen(){
ewoud 11:c5042e19a096 45 lcd.cls();
ewoud 11:c5042e19a096 46 lcd.printf("FINISH!\n\rSCORE: %d",playTime, hitCount, playTime+5*hitCount);
ewoud 11:c5042e19a096 47 pc.printf("enter name (3 characters): ");
ewoud 11:c5042e19a096 48 for (int i=0; i<=3; i++){
ewoud 11:c5042e19a096 49 char input= pc.getc();
ewoud 11:c5042e19a096 50 pc.putc(input);
ewoud 11:c5042e19a096 51 }
ewoud 11:c5042e19a096 52 lcd.cls();
ewoud 11:c5042e19a096 53 lcd.printf("press button to\n\rstart again");
ewoud 11:c5042e19a096 54
ewoud 11:c5042e19a096 55 }
ewoud 11:c5042e19a096 56
ewoud 0:ca94aa9bf823 57 void systemStart(){
ewoud 10:819fb5288aa0 58 //if (calibrated==false){
ewoud 10:819fb5288aa0 59 // calibrate_go=true;
ewoud 10:819fb5288aa0 60 // }
ewoud 8:b0971033dc41 61 if (systemOn==true){
ewoud 8:b0971033dc41 62 systemOn=false;
ewoud 8:b0971033dc41 63 lcd.cls();
ewoud 10:819fb5288aa0 64 lcd.printf("stopped :(\n\r");
ewoud 10:819fb5288aa0 65 pc.printf("stopped :(\n\r");
ewoud 10:819fb5288aa0 66 statusled=1;
ewoud 11:c5042e19a096 67 playTimer.reset();
ewoud 8:b0971033dc41 68 }
ewoud 8:b0971033dc41 69 else {
ewoud 8:b0971033dc41 70 systemOn=true;
ewoud 8:b0971033dc41 71 lcd.cls();
ewoud 10:819fb5288aa0 72 lcd.printf("GO GO GO!!\n\r");
ewoud 10:819fb5288aa0 73 pc.printf("started\n\r");
ewoud 10:819fb5288aa0 74 statusled=0;
ewoud 8:b0971033dc41 75 }
ewoud 0:ca94aa9bf823 76 }
ewoud 0:ca94aa9bf823 77 void systemStop(){
ewoud 0:ca94aa9bf823 78 systemOn=false;
ewoud 0:ca94aa9bf823 79 pc.printf("system stopped\n\r");
ewoud 8:b0971033dc41 80 lcd.cls();
ewoud 10:819fb5288aa0 81 lcd.printf("stopped :(\n\r");
ewoud 10:819fb5288aa0 82 statusled=1;
ewoud 0:ca94aa9bf823 83 }
ewoud 1:dafb63606b66 84
ewoud 0:ca94aa9bf823 85 int main() {
ewoud 10:819fb5288aa0 86 pc.printf("system start\n\r");
ewoud 8:b0971033dc41 87 lcd.printf("A-MAZE-ING\nMARIO"); // print a test message to the display.
ewoud 0:ca94aa9bf823 88 // initialize (defined in inits.h)
ewoud 0:ca94aa9bf823 89 initMotors();
ewoud 0:ca94aa9bf823 90 initPIDs();
ewoud 0:ca94aa9bf823 91
ewoud 0:ca94aa9bf823 92 edgeleft.mode(PullUp);
ewoud 0:ca94aa9bf823 93 edgeright.mode(PullUp);
ewoud 8:b0971033dc41 94 edgetop.mode(PullUp);
ewoud 8:b0971033dc41 95 edgebottom.mode(PullUp);
ewoud 10:819fb5288aa0 96 edgeStart.mode(PullUp);
ewoud 10:819fb5288aa0 97 edgeFinish.mode(PullUp);
ewoud 1:dafb63606b66 98
ewoud 1:dafb63606b66 99 // interrupts
ewoud 0:ca94aa9bf823 100 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 9:3e19a344c025 101 lcdTicker.attach(&setLcdFlag,0.2);
ewoud 3:70f78cfc0f25 102 cali_button.rise(&calibratego);
ewoud 6:ae2ce89dd695 103 initpositionButton.rise(&initpositiongo);
ewoud 10:819fb5288aa0 104 //startButton.fall(&buttonpressTimer); could be used to calculate the button press time
ewoud 0:ca94aa9bf823 105 startButton.rise(&systemStart);
ewoud 0:ca94aa9bf823 106 stopButton.rise(&systemStop);
ewoud 0:ca94aa9bf823 107
ewoud 11:c5042e19a096 108
ewoud 11:c5042e19a096 109
ewoud 11:c5042e19a096 110 bool endchar=false;
ewoud 11:c5042e19a096 111 string input;
ewoud 11:c5042e19a096 112 float leftOffset;
ewoud 11:c5042e19a096 113 float rightOffset;
ewoud 11:c5042e19a096 114 pc.printf("read angle from left arm (end with d): ");
ewoud 11:c5042e19a096 115 while(endchar==false){
ewoud 11:c5042e19a096 116 char input= pc.getc();
ewoud 11:c5042e19a096 117
ewoud 11:c5042e19a096 118 if(input=='d'){endchar=true;}
ewoud 11:c5042e19a096 119 else {text+=input;}
ewoud 11:c5042e19a096 120 pc.putc(input);
ewoud 11:c5042e19a096 121 }
ewoud 11:c5042e19a096 122 char chartext[1024];
ewoud 11:c5042e19a096 123 strcpy(chartext, text.c_str());
ewoud 11:c5042e19a096 124
ewoud 11:c5042e19a096 125 leftOffset=180-atof(chartext);
ewoud 11:c5042e19a096 126
ewoud 9:3e19a344c025 127
ewoud 11:c5042e19a096 128 pc.printf("\n\rread angle from right arm (end with d): ");
ewoud 11:c5042e19a096 129 text="";
ewoud 11:c5042e19a096 130 endchar=false;
ewoud 11:c5042e19a096 131 while(endchar==false){
ewoud 11:c5042e19a096 132 char input= pc.getc();
ewoud 11:c5042e19a096 133
ewoud 11:c5042e19a096 134 if(input=='d'){endchar=true;}
ewoud 11:c5042e19a096 135 else {text+=input;}
ewoud 11:c5042e19a096 136 pc.putc(input);
ewoud 11:c5042e19a096 137 }
ewoud 11:c5042e19a096 138
ewoud 11:c5042e19a096 139 strcpy(chartext, text.c_str());
ewoud 11:c5042e19a096 140
ewoud 11:c5042e19a096 141 rightOffset=atof(chartext);
ewoud 11:c5042e19a096 142 pc.printf("\n\rcontrol by pc? (y/n): ");
ewoud 11:c5042e19a096 143 char userinput=pc.getc();
ewoud 11:c5042e19a096 144 if(userinput == 'y'){controlbypc=true;}
ewoud 11:c5042e19a096 145 else {controlbypc=false;}
ewoud 11:c5042e19a096 146
ewoud 11:c5042e19a096 147 pc.printf("\n\rvalues set, leftangle:%f, rightangle:%f\n\r",leftOffset,rightOffset);
ewoud 11:c5042e19a096 148
ewoud 0:ca94aa9bf823 149 while (true){
ewoud 3:70f78cfc0f25 150 if (calibrate_go==true){
ewoud 8:b0971033dc41 151 lcd.cls();
ewoud 8:b0971033dc41 152 lcd.printf("calibrating...");
ewoud 10:819fb5288aa0 153 calibrate();
ewoud 8:b0971033dc41 154 lcd.cls();
ewoud 8:b0971033dc41 155 lcd.printf("ready to start!\nPress the Button");
ewoud 3:70f78cfc0f25 156
ewoud 3:70f78cfc0f25 157 }
ewoud 0:ca94aa9bf823 158 if (goFlag==true && systemOn==true){
ewoud 0:ca94aa9bf823 159 /*********** Contents *************/
ewoud 0:ca94aa9bf823 160 // 1) get emg signal
ewoud 0:ca94aa9bf823 161 // 2) calculate desired angle velocities
ewoud 0:ca94aa9bf823 162 // 3) calculate current angle velocities
ewoud 1:dafb63606b66 163 // 4) pid controller output
ewoud 0:ca94aa9bf823 164 // 5) user output
ewoud 0:ca94aa9bf823 165
ewoud 1:dafb63606b66 166 // **************
ewoud 0:ca94aa9bf823 167 // get emg signal
ewoud 6:ae2ce89dd695 168 if(initposition_go){
ewoud 10:819fb5288aa0 169 if (pc.readable()){
ewoud 10:819fb5288aa0 170 char input= pc.getc();
ewoud 10:819fb5288aa0 171 if(input=='q'){leftRequest=0.5;}
ewoud 10:819fb5288aa0 172 else if(input=='a'){leftRequest=0;}
ewoud 10:819fb5288aa0 173 else if(input=='z'){leftRequest=-0.5;}
ewoud 10:819fb5288aa0 174
ewoud 10:819fb5288aa0 175 if(input=='o'){rightRequest=0.5;}
ewoud 10:819fb5288aa0 176 else if(input=='k'){rightRequest=0;}
ewoud 10:819fb5288aa0 177 else if(input=='m'){rightRequest=-0.5;}
ewoud 10:819fb5288aa0 178 pc.putc(input);
ewoud 11:c5042e19a096 179
ewoud 10:819fb5288aa0 180 }
ewoud 10:819fb5288aa0 181
ewoud 3:70f78cfc0f25 182 }
ewoud 0:ca94aa9bf823 183 else {
ewoud 10:819fb5288aa0 184 if(controlbypc){
ewoud 10:819fb5288aa0 185 if (pc.readable()){
ewoud 10:819fb5288aa0 186 char input= pc.getc();
ewoud 11:c5042e19a096 187 if(input=='8'){verrequest=1;}
ewoud 11:c5042e19a096 188 else if(input=='2'){verrequest=-1;}
ewoud 11:c5042e19a096 189 else if(input=='4'){horrequest=-1;}
ewoud 11:c5042e19a096 190 else if(input=='6'){horrequest=1;}
ewoud 10:819fb5288aa0 191 else {horrequest=0; verrequest=0;}
ewoud 10:819fb5288aa0 192 pc.putc(input);
ewoud 10:819fb5288aa0 193 }
ewoud 10:819fb5288aa0 194
ewoud 10:819fb5288aa0 195
ewoud 10:819fb5288aa0 196 }
ewoud 11:c5042e19a096 197 else { // control by emg
ewoud 10:819fb5288aa0 198 sample_filter();
ewoud 11:c5042e19a096 199 if (y3 > y2){
ewoud 11:c5042e19a096 200 horrequest = y3;
ewoud 10:819fb5288aa0 201 }
ewoud 10:819fb5288aa0 202 else {
ewoud 10:819fb5288aa0 203 horrequest = -y2;
ewoud 10:819fb5288aa0 204 }
ewoud 11:c5042e19a096 205 if (y4 > y1){
ewoud 11:c5042e19a096 206 verrequest = y4;
ewoud 10:819fb5288aa0 207 }
ewoud 10:819fb5288aa0 208 else {
ewoud 11:c5042e19a096 209 verrequest = -y1;
ewoud 10:819fb5288aa0 210 }
ewoud 10:819fb5288aa0 211 // perform stepping between boundries
ewoud 10:819fb5288aa0 212 if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
ewoud 10:819fb5288aa0 213 else if(horrequest > grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;}
ewoud 10:819fb5288aa0 214 else {horrequest=0;}
ewoud 10:819fb5288aa0 215
ewoud 10:819fb5288aa0 216 if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
ewoud 10:819fb5288aa0 217 else if(verrequest > grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;}
ewoud 10:819fb5288aa0 218 else {verrequest=0;}
ewoud 10:819fb5288aa0 219
ewoud 6:ae2ce89dd695 220 }
ewoud 11:c5042e19a096 221
ewoud 6:ae2ce89dd695 222
ewoud 6:ae2ce89dd695 223 // ***************
ewoud 6:ae2ce89dd695 224 // calculate required rotational velocity from the requested horizontal velocity
ewoud 6:ae2ce89dd695 225 // first get the current position from the motor encoders and make them start at 45 degree.
ewoud 11:c5042e19a096 226 leftAngle=-(leftQei.getPulses()/round)*360+leftOffset;
ewoud 11:c5042e19a096 227 rightAngle=-(rightQei.getPulses()/round)*360+rightOffset;
ewoud 6:ae2ce89dd695 228
ewoud 6:ae2ce89dd695 229 // trigonometry to get xy position from angles (cm)
ewoud 6:ae2ce89dd695 230 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 6:ae2ce89dd695 231 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 6:ae2ce89dd695 232
ewoud 6:ae2ce89dd695 233 // restrict motion if edges are touched
ewoud 11:c5042e19a096 234 if (edgeleft==0 or edgeStart==0){
ewoud 8:b0971033dc41 235 if (horrequest < 0){horrequest=0; }
ewoud 6:ae2ce89dd695 236 }
ewoud 11:c5042e19a096 237 if (edgeright==0 or edgeFinish==0){
ewoud 8:b0971033dc41 238 if (horrequest > 0){horrequest=0; }
ewoud 6:ae2ce89dd695 239 }
ewoud 9:3e19a344c025 240 if (edgetop==0){
ewoud 8:b0971033dc41 241 if (verrequest > 0){verrequest=0; }
ewoud 6:ae2ce89dd695 242 }
ewoud 9:3e19a344c025 243 if (edgebottom==0){
ewoud 8:b0971033dc41 244 if (verrequest < 0){verrequest=0; }
ewoud 6:ae2ce89dd695 245 }
ewoud 6:ae2ce89dd695 246
ewoud 11:c5042e19a096 247 if(edgebottom==0 or edgetop==0 or edgeleft==0 or edgeright==0) {
ewoud 11:c5042e19a096 248 musicHit=1;
ewoud 11:c5042e19a096 249 if (playTimer.read() > hitTime+3){
ewoud 11:c5042e19a096 250 hitTime=playTimer.read();
ewoud 11:c5042e19a096 251 hitCount+=1;
ewoud 11:c5042e19a096 252 }
ewoud 11:c5042e19a096 253 }
ewoud 11:c5042e19a096 254 else {musicHit=0;}
ewoud 11:c5042e19a096 255
ewoud 11:c5042e19a096 256 if(edgeStart==0){
ewoud 11:c5042e19a096 257 playTimer.reset();
ewoud 11:c5042e19a096 258 playTimer.start();
ewoud 11:c5042e19a096 259 hitCount=0;
ewoud 11:c5042e19a096 260 started=true;
ewoud 11:c5042e19a096 261 }
ewoud 11:c5042e19a096 262
ewoud 11:c5042e19a096 263 if(edgeFinish==0 and started==true){
ewoud 11:c5042e19a096 264 started=false;
ewoud 11:c5042e19a096 265 playTimer.stop();
ewoud 11:c5042e19a096 266 systemOn=false;
ewoud 11:c5042e19a096 267 showFinishScreen();
ewoud 11:c5042e19a096 268 hitCount=0;
ewoud 11:c5042e19a096 269 }
ewoud 11:c5042e19a096 270
ewoud 11:c5042e19a096 271
ewoud 6:ae2ce89dd695 272 // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
ewoud 11:c5042e19a096 273 toX=currentX+horrequest*maxspeed*RATE;
ewoud 11:c5042e19a096 274 toY=currentY+verrequest*maxspeed*RATE;
ewoud 6:ae2ce89dd695 275
ewoud 6:ae2ce89dd695 276 // trigonometry to get angles from xy new position (degree)
ewoud 6:ae2ce89dd695 277 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 6:ae2ce89dd695 278 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 6:ae2ce89dd695 279
ewoud 6:ae2ce89dd695 280 // restrict motion if angles out-of-bound
ewoud 10:819fb5288aa0 281 //if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
ewoud 11:c5042e19a096 282 //if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
ewoud 10:819fb5288aa0 283 //if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
ewoud 10:819fb5288aa0 284 //if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
ewoud 6:ae2ce89dd695 285
ewoud 6:ae2ce89dd695 286 // restrict motion if position is out of reach of the arms
ewoud 6:ae2ce89dd695 287 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 6:ae2ce89dd695 288 //return 0;
ewoud 6:ae2ce89dd695 289 //}
ewoud 6:ae2ce89dd695 290 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 6:ae2ce89dd695 291 //return 0;
ewoud 6:ae2ce89dd695 292 //}
ewoud 6:ae2ce89dd695 293
ewoud 6:ae2ce89dd695 294 // calculate the neccesairy velocities to make these angles happen (degree/sec)
ewoud 6:ae2ce89dd695 295 leftRequest=(toLeftAngle-leftAngle)/RATE;
ewoud 6:ae2ce89dd695 296 rightRequest=(toRightAngle-rightAngle)/RATE;
ewoud 0:ca94aa9bf823 297 }
ewoud 8:b0971033dc41 298 //pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
ewoud 11:c5042e19a096 299
ewoud 0:ca94aa9bf823 300 // set the setpoint to the pid controller
ewoud 0:ca94aa9bf823 301 leftController.setSetPoint(leftRequest);
ewoud 0:ca94aa9bf823 302 rightController.setSetPoint(rightRequest);
ewoud 0:ca94aa9bf823 303
ewoud 1:dafb63606b66 304 // **************
ewoud 0:ca94aa9bf823 305 // Velocity calculation
ewoud 1:dafb63606b66 306 // left, differentiate from encoders
ewoud 10:819fb5288aa0 307 leftPulses = -(float)leftQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 308 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 309 leftPrevPulses = leftPulses;
ewoud 1:dafb63606b66 310 leftController.setProcessValue(leftVelocity); // set PID process value
ewoud 0:ca94aa9bf823 311
ewoud 0:ca94aa9bf823 312 // right
ewoud 10:819fb5288aa0 313 rightPulses = -(float)rightQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 314 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 315 rightPrevPulses = rightPulses;
ewoud 1:dafb63606b66 316 rightController.setProcessValue(rightVelocity); // set PID process value
ewoud 0:ca94aa9bf823 317
ewoud 0:ca94aa9bf823 318
ewoud 1:dafb63606b66 319 // **************
ewoud 0:ca94aa9bf823 320 // PID control output
ewoud 0:ca94aa9bf823 321 // left
ewoud 0:ca94aa9bf823 322
ewoud 0:ca94aa9bf823 323 leftPwmDuty = leftController.compute();
ewoud 1:dafb63606b66 324 if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 10:819fb5288aa0 325 leftDirection = 0;
ewoud 0:ca94aa9bf823 326 leftMotor = -leftPwmDuty;
ewoud 0:ca94aa9bf823 327 }
ewoud 0:ca94aa9bf823 328 else {
ewoud 10:819fb5288aa0 329 leftDirection = 1;
ewoud 0:ca94aa9bf823 330 leftMotor = leftPwmDuty;
ewoud 0:ca94aa9bf823 331 }
ewoud 0:ca94aa9bf823 332
ewoud 0:ca94aa9bf823 333 // right
ewoud 0:ca94aa9bf823 334 rightPwmDuty = rightController.compute();
ewoud 1:dafb63606b66 335 if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 10:819fb5288aa0 336 rightDirection = 0;
ewoud 0:ca94aa9bf823 337 rightMotor = -rightPwmDuty;
ewoud 0:ca94aa9bf823 338 }
ewoud 0:ca94aa9bf823 339 else {
ewoud 10:819fb5288aa0 340 rightDirection = 1;
ewoud 0:ca94aa9bf823 341 rightMotor = rightPwmDuty;
ewoud 0:ca94aa9bf823 342 }
ewoud 0:ca94aa9bf823 343
ewoud 1:dafb63606b66 344 // ***************
ewoud 0:ca94aa9bf823 345 // User feedback
ewoud 11:c5042e19a096 346 scope.set(0, y1);
ewoud 11:c5042e19a096 347 scope.set(1, y2);
ewoud 11:c5042e19a096 348 scope.set(2, y3);
ewoud 11:c5042e19a096 349 scope.set(3, y4);
ewoud 11:c5042e19a096 350 scope.set(4, currentX);
ewoud 11:c5042e19a096 351 scope.set(5, currentY);
ewoud 0:ca94aa9bf823 352 scope.send();
ewoud 1:dafb63606b66 353
ewoud 0:ca94aa9bf823 354 goFlag=false;
ewoud 0:ca94aa9bf823 355 }
ewoud 0:ca94aa9bf823 356 if(systemOn==false)
ewoud 0:ca94aa9bf823 357 {
ewoud 0:ca94aa9bf823 358 leftMotor=0;
ewoud 0:ca94aa9bf823 359 rightMotor=0;
ewoud 0:ca94aa9bf823 360 }
ewoud 10:819fb5288aa0 361
ewoud 10:819fb5288aa0 362 if(lcdGoFlag==true && systemOn==true){
ewoud 9:3e19a344c025 363 text="";
ewoud 9:3e19a344c025 364 text+="hor: ";
ewoud 9:3e19a344c025 365 if(horrequest==-1){text+="<<";}
ewoud 9:3e19a344c025 366 if(horrequest==-0.5){text+=" <";}
ewoud 9:3e19a344c025 367 if(horrequest>=0){text+=" ";}
ewoud 11:c5042e19a096 368 text+="-";
ewoud 9:3e19a344c025 369 if(horrequest<=0){text+=" ";}
ewoud 9:3e19a344c025 370 if(horrequest==0.5){text+="> ";}
ewoud 9:3e19a344c025 371 if(horrequest==1){text+=">>";}
ewoud 11:c5042e19a096 372
ewoud 11:c5042e19a096 373 playTime=(int)playTimer.read();
ewoud 11:c5042e19a096 374
ewoud 11:c5042e19a096 375 if(playTime < 10){text+=" T: ";}
ewoud 11:c5042e19a096 376 else if(playTime < 100){text+=" T: ";}
ewoud 11:c5042e19a096 377 else if(playTime < 1000){text+=" T:";}
ewoud 11:c5042e19a096 378 else {text+=" T:MAX";}
ewoud 11:c5042e19a096 379
ewoud 11:c5042e19a096 380 text+=intToString(playTime);
ewoud 10:819fb5288aa0 381 text+="ver: ";
ewoud 9:3e19a344c025 382
ewoud 10:819fb5288aa0 383 if(verrequest==-1){text+="vv";}
ewoud 10:819fb5288aa0 384 else if(verrequest==-0.5){text+=" v";}
ewoud 10:819fb5288aa0 385 else {text+=" ";}
ewoud 11:c5042e19a096 386 text+="|";
ewoud 10:819fb5288aa0 387
ewoud 11:c5042e19a096 388 if(verrequest==1){text+="^^";}
ewoud 11:c5042e19a096 389 else if(verrequest==0.5){text+="^ ";}
ewoud 9:3e19a344c025 390 if(verrequest<=0){text+=" ";}
ewoud 9:3e19a344c025 391
ewoud 11:c5042e19a096 392 if(hitCount < 10){text+=" H: ";}
ewoud 11:c5042e19a096 393 else if(hitCount < 100){text+=" H: ";}
ewoud 11:c5042e19a096 394 else if(hitCount < 1000){text+=" H:";}
ewoud 11:c5042e19a096 395 else {text+=" H:MAX";}
ewoud 9:3e19a344c025 396
ewoud 11:c5042e19a096 397 text+=intToString(hitCount);
ewoud 9:3e19a344c025 398
ewoud 9:3e19a344c025 399
ewoud 9:3e19a344c025 400 char chartext[1024];
ewoud 9:3e19a344c025 401 strcpy(chartext, text.c_str());
ewoud 9:3e19a344c025 402
ewoud 9:3e19a344c025 403 lcd.cls();
ewoud 9:3e19a344c025 404 lcd.printf(chartext);
ewoud 9:3e19a344c025 405
ewoud 9:3e19a344c025 406 lcdGoFlag=false;
ewoud 9:3e19a344c025 407
ewoud 9:3e19a344c025 408 }
ewoud 0:ca94aa9bf823 409 }
ewoud 0:ca94aa9bf823 410
ewoud 0:ca94aa9bf823 411 }