Ryan Greiner / Mbed 2 deprecated Robot

Programming for Shadow Robot control for smart phone mapping application.

Dependencies:   Motor LSM9DS1_Library_cal mbed Servo

main.cpp@5:48c8fb81288e, 2016-12-09 (annotated)

Committer:
greiner218
Date:
Fri Dec 09 02:39:10 2016 +0000
Revision:
5:48c8fb81288e
Parent:
4:bc55afdd43de 
Tuned slam.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
greiner218 0:616fbf21a20e 1 #include "mbed.h"
greiner218 0:616fbf21a20e 2 #include "Motor.h"
greiner218 1:8638bdaf172b 3 #include "Servo.h"
greiner218 1:8638bdaf172b 4 #include "LSM9DS1.h"
greiner218 3:ce743dbbd4a5 5 #include <math.h>
greiner218 3:ce743dbbd4a5 6
greiner218 4:bc55afdd43de 7 #define TIMEOUT_VAL_MS 15
greiner218 3:ce743dbbd4a5 8 #define PI 3.14159
greiner218 3:ce743dbbd4a5 9 #define DECLINATION -4.94
greiner218 3:ce743dbbd4a5 10 #define DIST_FACTOR 0.107475 //cm/tick Conversion between a tick from Hall effect sensor and linear distance.
greiner218 3:ce743dbbd4a5 11 //Approximately 190 ticks for a full wheel rotation.
greiner218 3:ce743dbbd4a5 12 //Approximately 6.5cm wheel diameter
greiner218 3:ce743dbbd4a5 13 //pi*diameter (cm) / 190(ticks)
greiner218 3:ce743dbbd4a5 14 volatile float heading;
greiner218 4:bc55afdd43de 15 float prevHeading[3];
greiner218 3:ce743dbbd4a5 16 void getHeading(float ax, float ay, float az, float mx, float my, float mz);
greiner218 3:ce743dbbd4a5 17 float PI_cont(float err);
greiner218 0:616fbf21a20e 18
greiner218 1:8638bdaf172b 19 //MOTOR PINS
greiner218 1:8638bdaf172b 20 Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
greiner218 1:8638bdaf172b 21 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev /
greiner218 1:8638bdaf172b 22 //BLUETOOTH PINS
greiner218 1:8638bdaf172b 23 Serial bt(p28,p27);
greiner218 1:8638bdaf172b 24 //LEFT SONAR
greiner218 1:8638bdaf172b 25 DigitalOut triggerL(p14);
greiner218 1:8638bdaf172b 26 DigitalIn echoL(p12);
greiner218 4:bc55afdd43de 27 DigitalOut echo1RiseWait(LED1);
greiner218 4:bc55afdd43de 28 DigitalOut echo1FallWait(LED2);
greiner218 1:8638bdaf172b 29 //RIGHT SONAR
greiner218 1:8638bdaf172b 30 DigitalOut triggerR(p13);
greiner218 1:8638bdaf172b 31 DigitalIn echoR(p11);
greiner218 4:bc55afdd43de 32 DigitalOut echo2RiseWait(LED3);
greiner218 4:bc55afdd43de 33 DigitalOut echo2FallWait(LED4);
greiner218 3:ce743dbbd4a5 34 int correction = 0; //Used to adjust software delay for sonar measurement
greiner218 3:ce743dbbd4a5 35 Timer sonar;
greiner218 4:bc55afdd43de 36 Timer timeout;
greiner218 3:ce743dbbd4a5 37
greiner218 1:8638bdaf172b 38 //SERVO PINS
greiner218 1:8638bdaf172b 39 Servo angle(p21);
greiner218 1:8638bdaf172b 40 //IMU PINS
greiner218 2:64585b8d8404 41 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
greiner218 3:ce743dbbd4a5 42 Ticker nav;
greiner218 3:ce743dbbd4a5 43 float posX = 0;
greiner218 3:ce743dbbd4a5 44 float posY = 0;
greiner218 3:ce743dbbd4a5 45 int tickDistL = 0;
greiner218 3:ce743dbbd4a5 46 int tickDistR = 0;
greiner218 1:8638bdaf172b 47
greiner218 1:8638bdaf172b 48 Serial pc(USBTX, USBRX);
greiner218 0:616fbf21a20e 49 Timer timestamp;
greiner218 3:ce743dbbd4a5 50
greiner218 1:8638bdaf172b 51 // Set up Hall-effect control
greiner218 2:64585b8d8404 52 InterruptIn EncR(p25);
greiner218 2:64585b8d8404 53 InterruptIn EncL(p24);
greiner218 1:8638bdaf172b 54 int ticksR = 0; // Encoder wheel state change counts
greiner218 1:8638bdaf172b 55 int ticksL = 0;
greiner218 1:8638bdaf172b 56 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
greiner218 1:8638bdaf172b 57 float speedR = 0; //Same for right wheel
greiner218 2:64585b8d8404 58 int dirL = 1; //1 for fwd, -1 for reverse
greiner218 2:64585b8d8404 59 int dirR = 1; //1 for fwd, -1 for reverse
greiner218 1:8638bdaf172b 60 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
greiner218 2:64585b8d8404 61 int tracking = 0; //Flag used to indicate right wheel correction
greiner218 4:bc55afdd43de 62 int sweeping = 0;
greiner218 1:8638bdaf172b 63
greiner218 3:ce743dbbd4a5 64 //PI controller
greiner218 3:ce743dbbd4a5 65 //global variables
greiner218 3:ce743dbbd4a5 66 float kp=12.0f;
greiner218 3:ce743dbbd4a5 67 float ki=4.0f;
greiner218 3:ce743dbbd4a5 68 float kd=1.0f;
greiner218 3:ce743dbbd4a5 69 float P=0;
greiner218 3:ce743dbbd4a5 70 float I=0;
greiner218 3:ce743dbbd4a5 71 float D=0;
greiner218 3:ce743dbbd4a5 72 float dt=0.1f;
greiner218 3:ce743dbbd4a5 73 float out=0.0f;
greiner218 3:ce743dbbd4a5 74 float max=0.5f;
greiner218 3:ce743dbbd4a5 75 float min=-0.5f;
greiner218 3:ce743dbbd4a5 76 float prerr=0.0f;
greiner218 3:ce743dbbd4a5 77
greiner218 4:bc55afdd43de 78 //SLAM variables
greiner218 4:bc55afdd43de 79 float angle_list[64];
greiner218 4:bc55afdd43de 80 float dist_list[64];
greiner218 4:bc55afdd43de 81 int slam_ind = 0;
greiner218 4:bc55afdd43de 82 float slam_dist = 0.0f;
greiner218 1:8638bdaf172b 83 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
greiner218 1:8638bdaf172b 84 void sampleEncoder()
greiner218 1:8638bdaf172b 85 {
greiner218 2:64585b8d8404 86 float E; // Error between the 2 wheel speeds
greiner218 2:64585b8d8404 87 if(tracking) { //Right wheel "tracks" left wheel if enabled.
greiner218 1:8638bdaf172b 88
greiner218 1:8638bdaf172b 89 E = ticksL - ticksR; //Find error
greiner218 2:64585b8d8404 90 //E = (ticksL+1)/(ticksR+1);
greiner218 2:64585b8d8404 91 if (dirL == 1)
greiner218 2:64585b8d8404 92 {
greiner218 2:64585b8d8404 93 speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 94 //speedR = E*speedR;
greiner218 2:64585b8d8404 95 if (speedR < 0) speedR = 0;
greiner218 2:64585b8d8404 96 }
greiner218 2:64585b8d8404 97 else if (dirL == -1)
greiner218 2:64585b8d8404 98 {
greiner218 2:64585b8d8404 99 speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 100 //speedR = E*speedR;
greiner218 2:64585b8d8404 101 if (speedR > 0) speedR = 0;
greiner218 2:64585b8d8404 102 }
greiner218 1:8638bdaf172b 103 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 104 }
greiner218 2:64585b8d8404 105 //pc.printf("tickL: %i tickR: %i Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
greiner218 1:8638bdaf172b 106 ticksR = 0; //Restart the counters
greiner218 1:8638bdaf172b 107 ticksL = 0;
greiner218 1:8638bdaf172b 108 }
greiner218 0:616fbf21a20e 109
greiner218 4:bc55afdd43de 110 float angleAdjust(float a)
greiner218 4:bc55afdd43de 111 {
greiner218 4:bc55afdd43de 112 while(a<0 || a>360)
greiner218 4:bc55afdd43de 113 {
greiner218 4:bc55afdd43de 114 if (a<0) a+=360;
greiner218 4:bc55afdd43de 115 if (a>360) a-=360;
greiner218 4:bc55afdd43de 116 }
greiner218 4:bc55afdd43de 117 return a;
greiner218 4:bc55afdd43de 118 }
greiner218 3:ce743dbbd4a5 119 void getHeading(float ax, float ay, float az, float mx, float my, float mz)
greiner218 3:ce743dbbd4a5 120 {
greiner218 3:ce743dbbd4a5 121 float roll = atan2(ay, az);
greiner218 3:ce743dbbd4a5 122 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
greiner218 4:bc55afdd43de 123 //Shift previous headings
greiner218 4:bc55afdd43de 124 prevHeading[2] = prevHeading[1];
greiner218 4:bc55afdd43de 125 prevHeading[1] = prevHeading[0];
greiner218 4:bc55afdd43de 126 prevHeading[0] = heading;
greiner218 3:ce743dbbd4a5 127 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
greiner218 3:ce743dbbd4a5 128 mx = -mx;
greiner218 3:ce743dbbd4a5 129 if (my == 0.0)
greiner218 3:ce743dbbd4a5 130 heading = (mx < 0.0) ? 180.0 : 0.0;
greiner218 3:ce743dbbd4a5 131 else
greiner218 3:ce743dbbd4a5 132 heading = atan2(mx, my)*360.0/(2.0*PI);
greiner218 3:ce743dbbd4a5 133 //pc.printf("heading atan=%f \n\r",heading);
greiner218 3:ce743dbbd4a5 134 heading -= DECLINATION; //correct for geo location
greiner218 4:bc55afdd43de 135 heading += 90;
greiner218 4:bc55afdd43de 136 //if(heading>180.0) heading = heading - 360.0;
greiner218 4:bc55afdd43de 137 //else if(heading<-180.0) heading = 360.0 + heading;
greiner218 4:bc55afdd43de 138 //else if(heading<0.0) heading = 360.0 + heading;
greiner218 4:bc55afdd43de 139 heading = angleAdjust(heading);
greiner218 4:bc55afdd43de 140 heading = (heading + prevHeading[0] + prevHeading[1] + prevHeading[2])/4.0;
greiner218 4:bc55afdd43de 141
greiner218 3:ce743dbbd4a5 142 }
greiner218 3:ce743dbbd4a5 143
greiner218 3:ce743dbbd4a5 144 float PI_cont(float err)
greiner218 3:ce743dbbd4a5 145 {
greiner218 3:ce743dbbd4a5 146 float Pout=kp*err;
greiner218 3:ce743dbbd4a5 147 P=Pout;
greiner218 3:ce743dbbd4a5 148 //pc.printf("P is: %f \n\r",P);
greiner218 3:ce743dbbd4a5 149
greiner218 3:ce743dbbd4a5 150 float Iout=Iout+err*dt;
greiner218 3:ce743dbbd4a5 151 I=ki*Iout;
greiner218 3:ce743dbbd4a5 152 //pc.printf("I is: %f \n\r",I);
greiner218 3:ce743dbbd4a5 153
greiner218 3:ce743dbbd4a5 154 float Dout = (err - prerr)/dt;
greiner218 3:ce743dbbd4a5 155 D=kd*Dout;
greiner218 3:ce743dbbd4a5 156
greiner218 3:ce743dbbd4a5 157 out=P+I+D;
greiner218 3:ce743dbbd4a5 158
greiner218 3:ce743dbbd4a5 159 prerr=err;
greiner218 3:ce743dbbd4a5 160
greiner218 3:ce743dbbd4a5 161 //pc.printf("out is: %f \n\r",out); // basically pwm out
greiner218 3:ce743dbbd4a5 162 if (out>max)out=max;
greiner218 3:ce743dbbd4a5 163 else if (out<min)out=min;
greiner218 3:ce743dbbd4a5 164 return out;
greiner218 3:ce743dbbd4a5 165 }
greiner218 3:ce743dbbd4a5 166
greiner218 3:ce743dbbd4a5 167 void turn(float angle)
greiner218 3:ce743dbbd4a5 168 {
greiner218 3:ce743dbbd4a5 169 float s =0.05;//speed by which the robot should turn
greiner218 3:ce743dbbd4a5 170 while(!imu.magAvailable(X_AXIS));
greiner218 3:ce743dbbd4a5 171 imu.readMag();
greiner218 3:ce743dbbd4a5 172 while(!imu.accelAvailable());
greiner218 3:ce743dbbd4a5 173 imu.readAccel();
greiner218 3:ce743dbbd4a5 174 while(!imu.gyroAvailable());
greiner218 3:ce743dbbd4a5 175 imu.readGyro();
greiner218 3:ce743dbbd4a5 176 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 3:ce743dbbd4a5 177 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 3:ce743dbbd4a5 178 float PV = heading;//the original heading
greiner218 3:ce743dbbd4a5 179 float SP = heading - angle;//the new required heading
greiner218 3:ce743dbbd4a5 180 if(SP>=360){//reseting after 360 degrees to 0
greiner218 3:ce743dbbd4a5 181 SP = SP-360;
greiner218 3:ce743dbbd4a5 182 }
greiner218 3:ce743dbbd4a5 183 pc.printf("PV: %f\n",PV);
greiner218 3:ce743dbbd4a5 184 pc.printf("SV: %f\n",SP);
greiner218 3:ce743dbbd4a5 185 wait(2);
greiner218 3:ce743dbbd4a5 186 //pc.printf("required: %f\n",requiredHeading);
greiner218 3:ce743dbbd4a5 187 float error = angle;
greiner218 3:ce743dbbd4a5 188 pc.printf("error: %f\n",error);
greiner218 3:ce743dbbd4a5 189 int reached = 0;
greiner218 3:ce743dbbd4a5 190 while(reached ==0){
greiner218 3:ce743dbbd4a5 191
greiner218 3:ce743dbbd4a5 192 while(!imu.magAvailable(ALL_AXIS));
greiner218 3:ce743dbbd4a5 193 imu.readMag();
greiner218 3:ce743dbbd4a5 194 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 3:ce743dbbd4a5 195 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 3:ce743dbbd4a5 196 PV = heading;//calculate the current heading
greiner218 3:ce743dbbd4a5 197
greiner218 3:ce743dbbd4a5 198 error=(PV-SP)/360;//find error between current heading and required heading
greiner218 3:ce743dbbd4a5 199 float correction=PI_cont(error);//correction to motor speed
greiner218 3:ce743dbbd4a5 200 if (error> 1.5/360 || error<-1.5/360) { //an error limit of 1.5 degrees allowed.. checking if the robot needs to turn more
greiner218 3:ce743dbbd4a5 201 rwheel.speed((s+correction)/2);
greiner218 3:ce743dbbd4a5 202 lwheel.speed((s-correction)/2);
greiner218 3:ce743dbbd4a5 203 pc.printf("error in 1st if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 204 pc.printf("error in 1st if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 205 }
greiner218 3:ce743dbbd4a5 206 else if (error>=-1.5/360 && error<=1.5/360) { //within within satisfying angular range
greiner218 3:ce743dbbd4a5 207 rwheel.speed(0);
greiner218 3:ce743dbbd4a5 208 lwheel.speed(0);//stop moving and exit the loop
greiner218 3:ce743dbbd4a5 209 pc.printf("error in 2nd if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 210 break;
greiner218 3:ce743dbbd4a5 211 }
greiner218 3:ce743dbbd4a5 212 wait(0.01);
greiner218 3:ce743dbbd4a5 213 }
greiner218 3:ce743dbbd4a5 214
greiner218 3:ce743dbbd4a5 215 rwheel.speed(0);
greiner218 3:ce743dbbd4a5 216 lwheel.speed(0);
greiner218 3:ce743dbbd4a5 217 }
greiner218 3:ce743dbbd4a5 218
greiner218 3:ce743dbbd4a5 219
greiner218 3:ce743dbbd4a5 220 void sendCmd(char cmd, float arg)
greiner218 3:ce743dbbd4a5 221 {
greiner218 4:bc55afdd43de 222 bt.printf("!%c%.2f",cmd, arg);
greiner218 4:bc55afdd43de 223 //unsigned char c[sizeof arg];
greiner218 4:bc55afdd43de 224 //memcpy(c, &arg, sizeof arg);
greiner218 4:bc55afdd43de 225 //bt.putc('!');
greiner218 4:bc55afdd43de 226 //bt.putc(cmd);
greiner218 4:bc55afdd43de 227 //bt.putc(c[0]);
greiner218 4:bc55afdd43de 228 //bt.putc(c[1]);
greiner218 4:bc55afdd43de 229 //bt.putc(c[2]);
greiner218 4:bc55afdd43de 230 //bt.putc(c[3]);
greiner218 3:ce743dbbd4a5 231 }
greiner218 3:ce743dbbd4a5 232
greiner218 1:8638bdaf172b 233 int ping(int i)
greiner218 1:8638bdaf172b 234 {
greiner218 3:ce743dbbd4a5 235 float distance = 0;
greiner218 1:8638bdaf172b 236 switch(i)
greiner218 1:8638bdaf172b 237 {
greiner218 1:8638bdaf172b 238
greiner218 1:8638bdaf172b 239 case(1): //Ping Left Sonar
greiner218 1:8638bdaf172b 240 // trigger sonar to send a ping
greiner218 5:48c8fb81288e 241 //pc.printf("Pinging sonar 1...\n\r");
greiner218 1:8638bdaf172b 242 triggerL = 1;
greiner218 1:8638bdaf172b 243 sonar.reset();
greiner218 3:ce743dbbd4a5 244 //wait_us(10.0);
greiner218 1:8638bdaf172b 245 //wait for echo high
greiner218 4:bc55afdd43de 246 echo1RiseWait = 1;
greiner218 4:bc55afdd43de 247 timeout.reset();
greiner218 4:bc55afdd43de 248 timeout.start();
greiner218 4:bc55afdd43de 249 while (echoL==0){
greiner218 4:bc55afdd43de 250 if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
greiner218 4:bc55afdd43de 251 };
greiner218 4:bc55afdd43de 252 echo1RiseWait = 0;
greiner218 1:8638bdaf172b 253 //echo high, so start timer
greiner218 1:8638bdaf172b 254 sonar.start();
greiner218 3:ce743dbbd4a5 255 triggerL = 0;
greiner218 1:8638bdaf172b 256 //wait for echo low
greiner218 4:bc55afdd43de 257 echo1FallWait = 1;
greiner218 4:bc55afdd43de 258 timeout.reset();
greiner218 4:bc55afdd43de 259 timeout.start();
greiner218 4:bc55afdd43de 260 while (echoL==1) {
greiner218 5:48c8fb81288e 261 if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
greiner218 4:bc55afdd43de 262 };
greiner218 4:bc55afdd43de 263 echo1FallWait = 0;
greiner218 1:8638bdaf172b 264 //stop timer and read value
greiner218 1:8638bdaf172b 265 sonar.stop();
greiner218 1:8638bdaf172b 266 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 267 distance = (sonar.read_us()-correction)/58.0;
greiner218 5:48c8fb81288e 268 //pc.printf("Got distance %f cm back.\n\r", distance);
greiner218 1:8638bdaf172b 269 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 270 wait(0.015);
greiner218 1:8638bdaf172b 271 break;
greiner218 1:8638bdaf172b 272 case(2): //Ping Right Sonar
greiner218 1:8638bdaf172b 273 // trigger sonar to send a ping
greiner218 5:48c8fb81288e 274 //pc.printf("Pinging sonar 2...\n\r");
greiner218 1:8638bdaf172b 275 triggerR = 1;
greiner218 1:8638bdaf172b 276 sonar.reset();
greiner218 3:ce743dbbd4a5 277 //wait_us(10.0);
greiner218 1:8638bdaf172b 278 //wait for echo high
greiner218 4:bc55afdd43de 279 echo2RiseWait = 1;
greiner218 4:bc55afdd43de 280 timeout.reset();
greiner218 4:bc55afdd43de 281 timeout.start();
greiner218 4:bc55afdd43de 282 while (echoR==0) {
greiner218 4:bc55afdd43de 283 if (timeout.read_ms() > TIMEOUT_VAL_MS) return 0;
greiner218 4:bc55afdd43de 284 };
greiner218 4:bc55afdd43de 285 echo2RiseWait = 0;
greiner218 1:8638bdaf172b 286 //echo high, so start timer
greiner218 1:8638bdaf172b 287 sonar.start();
greiner218 3:ce743dbbd4a5 288 triggerR = 0;
greiner218 1:8638bdaf172b 289 //wait for echo low
greiner218 4:bc55afdd43de 290 echo2FallWait = 1;
greiner218 4:bc55afdd43de 291 timeout.reset();
greiner218 4:bc55afdd43de 292 timeout.start();
greiner218 4:bc55afdd43de 293 while (echoR==1) {
greiner218 5:48c8fb81288e 294 if (timeout.read_ms() > TIMEOUT_VAL_MS*5) return 0;
greiner218 4:bc55afdd43de 295 };
greiner218 4:bc55afdd43de 296 echo2FallWait = 0;
greiner218 1:8638bdaf172b 297 //stop timer and read value
greiner218 1:8638bdaf172b 298 sonar.stop();
greiner218 1:8638bdaf172b 299 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 300 distance = (sonar.read_us()-correction)/58.0;
greiner218 5:48c8fb81288e 301 //pc.printf("Got distance %f cm back.\n\r", distance);
greiner218 1:8638bdaf172b 302 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 303 wait(0.015);
greiner218 1:8638bdaf172b 304 break;
greiner218 1:8638bdaf172b 305 default:
greiner218 1:8638bdaf172b 306 break;
greiner218 3:ce743dbbd4a5 307 }
greiner218 3:ce743dbbd4a5 308 return distance;
greiner218 3:ce743dbbd4a5 309 }
greiner218 3:ce743dbbd4a5 310
greiner218 3:ce743dbbd4a5 311 void sweep()
greiner218 3:ce743dbbd4a5 312 {
greiner218 4:bc55afdd43de 313 __disable_irq();
greiner218 4:bc55afdd43de 314 float AL = 0.0;
greiner218 4:bc55afdd43de 315 float AR = 0.0;
greiner218 3:ce743dbbd4a5 316 int dL = 0;
greiner218 3:ce743dbbd4a5 317 int dR = 0;
greiner218 4:bc55afdd43de 318 sweeping = 1;
greiner218 3:ce743dbbd4a5 319 if (angle <=0.5) //Set to min angle and execute forward sweep.
greiner218 3:ce743dbbd4a5 320 {
greiner218 3:ce743dbbd4a5 321 angle = 0;
greiner218 4:bc55afdd43de 322 wait(.2);
greiner218 3:ce743dbbd4a5 323 while( angle >= 0 && angle < 1)
greiner218 3:ce743dbbd4a5 324 {
greiner218 3:ce743dbbd4a5 325 dL = ping(1);
greiner218 3:ce743dbbd4a5 326 dR = ping(2);
greiner218 4:bc55afdd43de 327 AL = angleAdjust(heading + (angle-0.5)*55 - 45);
greiner218 4:bc55afdd43de 328 AR = angleAdjust(heading + (angle-0.5)*55 + 45);
greiner218 4:bc55afdd43de 329 if (dL > 20 && dL < 200)
greiner218 4:bc55afdd43de 330 {
greiner218 4:bc55afdd43de 331 sendCmd('A',AL);
greiner218 4:bc55afdd43de 332 sendCmd('D',dL);
greiner218 4:bc55afdd43de 333 dist_list[slam_ind] = dL;
greiner218 5:48c8fb81288e 334 angle_list[slam_ind] = AL-heading;
greiner218 4:bc55afdd43de 335 if (slam_ind < 63) slam_ind++;
greiner218 5:48c8fb81288e 336 pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
greiner218 4:bc55afdd43de 337 }
greiner218 4:bc55afdd43de 338 if (dR > 20 && dR < 200)
greiner218 4:bc55afdd43de 339 {
greiner218 4:bc55afdd43de 340 sendCmd('A',AR);
greiner218 4:bc55afdd43de 341 sendCmd('D',dR);
greiner218 4:bc55afdd43de 342 dist_list[slam_ind] = dL;
greiner218 5:48c8fb81288e 343 angle_list[slam_ind] = AR-heading;
greiner218 4:bc55afdd43de 344 if (slam_ind < 63) slam_ind++;
greiner218 5:48c8fb81288e 345 pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
greiner218 4:bc55afdd43de 346 }
greiner218 5:48c8fb81288e 347 angle = angle + 0.05;
greiner218 3:ce743dbbd4a5 348 }
greiner218 3:ce743dbbd4a5 349 angle = 1;
greiner218 1:8638bdaf172b 350 }
greiner218 3:ce743dbbd4a5 351 else if (angle > 0.5) //Set to max angle and execute backward sweep.
greiner218 3:ce743dbbd4a5 352 {
greiner218 3:ce743dbbd4a5 353 angle = 1;
greiner218 4:bc55afdd43de 354 wait(.2);
greiner218 3:ce743dbbd4a5 355 while( angle > 0 && angle <= 1)
greiner218 3:ce743dbbd4a5 356 {
greiner218 3:ce743dbbd4a5 357 dL = ping(1);
greiner218 3:ce743dbbd4a5 358 dR = ping(2);
greiner218 4:bc55afdd43de 359 AL = angleAdjust(heading + (angle-0.5)*55 - 45);
greiner218 4:bc55afdd43de 360 AR = angleAdjust(heading + (angle-0.5)*55 + 45);
greiner218 4:bc55afdd43de 361 if (dL > 20 && dL < 200)
greiner218 4:bc55afdd43de 362 {
greiner218 4:bc55afdd43de 363 sendCmd('A',AL);
greiner218 4:bc55afdd43de 364 sendCmd('D',dL);
greiner218 4:bc55afdd43de 365 dist_list[slam_ind] = dL;
greiner218 5:48c8fb81288e 366 angle_list[slam_ind] = AL-heading;
greiner218 4:bc55afdd43de 367 if (slam_ind < 63) slam_ind++;
greiner218 5:48c8fb81288e 368 pc.printf("(%.1f,%.1f)\n\r",AL-heading,dL);
greiner218 4:bc55afdd43de 369 }
greiner218 4:bc55afdd43de 370 if (dR > 20 && dR < 200)
greiner218 4:bc55afdd43de 371 {
greiner218 4:bc55afdd43de 372 sendCmd('A',AR);
greiner218 4:bc55afdd43de 373 sendCmd('D',dR);
greiner218 4:bc55afdd43de 374 dist_list[slam_ind] = dL;
greiner218 5:48c8fb81288e 375 angle_list[slam_ind] = AR-heading;
greiner218 4:bc55afdd43de 376 if (slam_ind < 63) slam_ind++;
greiner218 5:48c8fb81288e 377 pc.printf("(%.1f,%.1f)\n\r",AR-heading,dR);
greiner218 4:bc55afdd43de 378 }
greiner218 5:48c8fb81288e 379 angle = angle - 0.05;
greiner218 3:ce743dbbd4a5 380 }
greiner218 3:ce743dbbd4a5 381 angle = 0;
greiner218 3:ce743dbbd4a5 382 }
greiner218 4:bc55afdd43de 383 sweeping = 0;
greiner218 4:bc55afdd43de 384 __enable_irq();
greiner218 3:ce743dbbd4a5 385 }
greiner218 3:ce743dbbd4a5 386
greiner218 3:ce743dbbd4a5 387 void updatePos()
greiner218 3:ce743dbbd4a5 388 {
greiner218 4:bc55afdd43de 389 if(!sweeping)
greiner218 4:bc55afdd43de 390 {
greiner218 4:bc55afdd43de 391 float deltaX;
greiner218 4:bc55afdd43de 392 float deltaY;
greiner218 4:bc55afdd43de 393 float dist;
greiner218 4:bc55afdd43de 394 while(!imu.magAvailable(X_AXIS));
greiner218 4:bc55afdd43de 395 imu.readMag();
greiner218 4:bc55afdd43de 396 while(!imu.accelAvailable());
greiner218 4:bc55afdd43de 397 imu.readAccel();
greiner218 4:bc55afdd43de 398 while(!imu.gyroAvailable());
greiner218 4:bc55afdd43de 399 imu.readGyro();
greiner218 4:bc55afdd43de 400 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 4:bc55afdd43de 401 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 4:bc55afdd43de 402 dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR;
greiner218 4:bc55afdd43de 403 dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance.
greiner218 4:bc55afdd43de 404 slam_dist += dist;
greiner218 5:48c8fb81288e 405 // pc.printf("Magnetic Heading: %f degress ",heading);
greiner218 5:48c8fb81288e 406 //pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR);
greiner218 4:bc55afdd43de 407 tickDistR = 0;//Reset the ticks.
greiner218 4:bc55afdd43de 408 tickDistL = 0;
greiner218 4:bc55afdd43de 409 deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig.
greiner218 4:bc55afdd43de 410 deltaY = dist*(float)cos((double)heading*PI/180.0f);
greiner218 4:bc55afdd43de 411 posX = posX + deltaX;
greiner218 4:bc55afdd43de 412 posY = posY + deltaY;
greiner218 5:48c8fb81288e 413 //pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY);
greiner218 5:48c8fb81288e 414 //pc.printf("posX: %f posY:%f \n\r",posX,posY);
greiner218 4:bc55afdd43de 415 sendCmd('X',posX);
greiner218 4:bc55afdd43de 416 sendCmd('Y',posY);
greiner218 4:bc55afdd43de 417 sendCmd('H',heading);
greiner218 4:bc55afdd43de 418 }
greiner218 1:8638bdaf172b 419 }
greiner218 1:8638bdaf172b 420
greiner218 2:64585b8d8404 421 void incTicksR()
greiner218 2:64585b8d8404 422 {
greiner218 2:64585b8d8404 423 ticksR++;
greiner218 3:ce743dbbd4a5 424 tickDistR++;
greiner218 2:64585b8d8404 425 }
greiner218 2:64585b8d8404 426
greiner218 2:64585b8d8404 427 void incTicksL()
greiner218 2:64585b8d8404 428 {
greiner218 2:64585b8d8404 429 ticksL++;
greiner218 3:ce743dbbd4a5 430 tickDistL++;
greiner218 2:64585b8d8404 431 }
greiner218 2:64585b8d8404 432
greiner218 4:bc55afdd43de 433 void moveFwd()
greiner218 4:bc55afdd43de 434 {
greiner218 4:bc55afdd43de 435 dirL = 1;
greiner218 4:bc55afdd43de 436 dirR = 1;
greiner218 4:bc55afdd43de 437 speedL = 0.9f;
greiner218 4:bc55afdd43de 438 speedR = 0.9f;
greiner218 4:bc55afdd43de 439 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 440 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 441 tracking = 1; //Turn on wheel feedback updates
greiner218 4:bc55afdd43de 442 tickDistL = 0; //Reset the distance ticks.
greiner218 4:bc55afdd43de 443 tickDistR = 0;
greiner218 4:bc55afdd43de 444 }
greiner218 4:bc55afdd43de 445
greiner218 4:bc55afdd43de 446 void moveBwd()
greiner218 4:bc55afdd43de 447 {
greiner218 4:bc55afdd43de 448 dirL = -1;
greiner218 4:bc55afdd43de 449 dirR = -1;
greiner218 4:bc55afdd43de 450 speedL = -0.9f;
greiner218 4:bc55afdd43de 451 speedR = -0.9f;
greiner218 4:bc55afdd43de 452 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 453 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 454 tracking = 1;
greiner218 4:bc55afdd43de 455 tickDistL = 0; //Reset the distance ticks.
greiner218 4:bc55afdd43de 456 tickDistR = 0;
greiner218 4:bc55afdd43de 457 }
greiner218 4:bc55afdd43de 458
greiner218 4:bc55afdd43de 459 void moveStp()
greiner218 4:bc55afdd43de 460 {
greiner218 4:bc55afdd43de 461 tracking = 0; //Turn off wheel feedback updates
greiner218 4:bc55afdd43de 462 speedL = 0;
greiner218 4:bc55afdd43de 463 speedR = 0;
greiner218 4:bc55afdd43de 464 lwheel.speed(0);
greiner218 4:bc55afdd43de 465 rwheel.speed(0);
greiner218 4:bc55afdd43de 466 }
greiner218 4:bc55afdd43de 467
greiner218 4:bc55afdd43de 468 void turnL()
greiner218 4:bc55afdd43de 469 {
greiner218 4:bc55afdd43de 470 tracking = 0;
greiner218 4:bc55afdd43de 471 dirL = -1;
greiner218 4:bc55afdd43de 472 dirR = 1;
greiner218 4:bc55afdd43de 473 speedL = -0.9;
greiner218 4:bc55afdd43de 474 speedR = .9;
greiner218 4:bc55afdd43de 475 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 476 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 477 }
greiner218 4:bc55afdd43de 478
greiner218 4:bc55afdd43de 479 void turnR()
greiner218 4:bc55afdd43de 480 {
greiner218 4:bc55afdd43de 481 tracking = 0;
greiner218 4:bc55afdd43de 482 dirL = 1;
greiner218 4:bc55afdd43de 483 dirR = -1;
greiner218 4:bc55afdd43de 484 speedL = 0.9;
greiner218 4:bc55afdd43de 485 speedR = -0.9;
greiner218 4:bc55afdd43de 486 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 487 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 488 }
greiner218 4:bc55afdd43de 489
greiner218 4:bc55afdd43de 490 int slam_sweep()
greiner218 4:bc55afdd43de 491 {
greiner218 4:bc55afdd43de 492 //Reset the SLAM variables;
greiner218 4:bc55afdd43de 493 memset(angle_list,0,sizeof(angle_list));
greiner218 4:bc55afdd43de 494 memset(dist_list,0,sizeof(dist_list));
greiner218 4:bc55afdd43de 495 slam_ind = 0;
greiner218 4:bc55afdd43de 496 //And sweep
greiner218 4:bc55afdd43de 497 sweep();
greiner218 4:bc55afdd43de 498 //Iterate to determine if detection in "danger zone"
greiner218 4:bc55afdd43de 499 for (int i = 0; i<64; i++)
greiner218 4:bc55afdd43de 500 {
greiner218 5:48c8fb81288e 501 if ((angle_list[i]) < 80 && (angle_list[i]) > -80)
greiner218 4:bc55afdd43de 502 {
greiner218 5:48c8fb81288e 503 if (dist_list[i] < 50 && dist_list[i] > 1)
greiner218 4:bc55afdd43de 504 {
greiner218 4:bc55afdd43de 505 return 1;
greiner218 4:bc55afdd43de 506
greiner218 4:bc55afdd43de 507 }
greiner218 4:bc55afdd43de 508 }
greiner218 4:bc55afdd43de 509 }
greiner218 4:bc55afdd43de 510 return 0;
greiner218 4:bc55afdd43de 511 }
greiner218 5:48c8fb81288e 512
greiner218 4:bc55afdd43de 513
greiner218 4:bc55afdd43de 514 void slam()
greiner218 4:bc55afdd43de 515 {
greiner218 4:bc55afdd43de 516 //Initialize arrays
greiner218 4:bc55afdd43de 517 int detect = 0;
greiner218 4:bc55afdd43de 518 wait(1);
greiner218 4:bc55afdd43de 519 while(1)
greiner218 4:bc55afdd43de 520 {
greiner218 4:bc55afdd43de 521 while(!detect)//while an object is not within 30cm of front 60 degrees of robot...
greiner218 4:bc55afdd43de 522 {
greiner218 4:bc55afdd43de 523 //move forward 20cm and scan
greiner218 4:bc55afdd43de 524 moveFwd();
greiner218 5:48c8fb81288e 525 //while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist);
greiner218 5:48c8fb81288e 526 wait(.8);
greiner218 4:bc55afdd43de 527 moveStp();
greiner218 4:bc55afdd43de 528 slam_dist = 0;
greiner218 4:bc55afdd43de 529 detect = slam_sweep();
greiner218 4:bc55afdd43de 530 }
greiner218 4:bc55afdd43de 531 //Else, turn left slightly.
greiner218 4:bc55afdd43de 532 detect = 0;
greiner218 4:bc55afdd43de 533 turnL();
greiner218 5:48c8fb81288e 534 wait(.8);
greiner218 4:bc55afdd43de 535 moveStp();
greiner218 4:bc55afdd43de 536 //and scan
greiner218 4:bc55afdd43de 537 detect = slam_sweep();
greiner218 4:bc55afdd43de 538 }
greiner218 4:bc55afdd43de 539
greiner218 4:bc55afdd43de 540 }
greiner218 4:bc55afdd43de 541
greiner218 0:616fbf21a20e 542 int main()
greiner218 0:616fbf21a20e 543 {
greiner218 2:64585b8d8404 544 char cmd;
greiner218 0:616fbf21a20e 545 timestamp.start();
greiner218 2:64585b8d8404 546 angle = 0.0f;
greiner218 3:ce743dbbd4a5 547 //Calibrate the IMU
greiner218 3:ce743dbbd4a5 548 if (!imu.begin()) {
greiner218 3:ce743dbbd4a5 549 pc.printf("Failed to communicate with LSM9DS1.\n");
greiner218 3:ce743dbbd4a5 550 }
greiner218 3:ce743dbbd4a5 551 imu.calibrate();
greiner218 4:bc55afdd43de 552 turnR();
greiner218 3:ce743dbbd4a5 553 imu.calibrateMag(0);
greiner218 4:bc55afdd43de 554 moveStp();
greiner218 1:8638bdaf172b 555 // Initialize hall-effect control
greiner218 2:64585b8d8404 556 Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
greiner218 1:8638bdaf172b 557 EncL.mode(PullUp); // Use internal pullups
greiner218 1:8638bdaf172b 558 EncR.mode(PullUp);
greiner218 2:64585b8d8404 559 EncR.rise(&incTicksR);
greiner218 2:64585b8d8404 560 EncL.rise(&incTicksL);
greiner218 4:bc55afdd43de 561 memset(prevHeading, 0, sizeof(prevHeading));
greiner218 3:ce743dbbd4a5 562 nav.attach(&updatePos,1);//Update X,Y,H every second
greiner218 2:64585b8d8404 563 while(1)
greiner218 2:64585b8d8404 564 {
greiner218 2:64585b8d8404 565 //Check if char is ready to be read and put into command buffer;
greiner218 2:64585b8d8404 566 if(bt.getc() =='!')
greiner218 2:64585b8d8404 567 {
greiner218 2:64585b8d8404 568 cmd = bt.getc();
greiner218 2:64585b8d8404 569 switch (cmd)
greiner218 2:64585b8d8404 570 {
greiner218 2:64585b8d8404 571 case 'B':
greiner218 5:48c8fb81288e 572 //pc.printf("A button was pressed!\n\r");
greiner218 4:bc55afdd43de 573 cmd = bt.getc();
greiner218 4:bc55afdd43de 574 switch(cmd)
greiner218 4:bc55afdd43de 575 {
greiner218 4:bc55afdd43de 576 case '5': //Up
greiner218 4:bc55afdd43de 577 cmd = bt.getc();
greiner218 4:bc55afdd43de 578 switch(cmd)
greiner218 4:bc55afdd43de 579 {
greiner218 4:bc55afdd43de 580 case '1': //Pressed
greiner218 4:bc55afdd43de 581 moveFwd();
greiner218 4:bc55afdd43de 582 break;
greiner218 4:bc55afdd43de 583
greiner218 4:bc55afdd43de 584 case '0': //Released
greiner218 4:bc55afdd43de 585 moveStp();
greiner218 4:bc55afdd43de 586 sweep();
greiner218 4:bc55afdd43de 587 break;
greiner218 4:bc55afdd43de 588 }
greiner218 4:bc55afdd43de 589 break;
greiner218 4:bc55afdd43de 590
greiner218 4:bc55afdd43de 591 case '6': //Down
greiner218 4:bc55afdd43de 592 cmd = bt.getc();
greiner218 4:bc55afdd43de 593 switch(cmd)
greiner218 4:bc55afdd43de 594 {
greiner218 4:bc55afdd43de 595 case '1': //Pressed
greiner218 4:bc55afdd43de 596 moveBwd();
greiner218 4:bc55afdd43de 597 break;
greiner218 4:bc55afdd43de 598
greiner218 4:bc55afdd43de 599 case '0': //Released
greiner218 4:bc55afdd43de 600 moveStp();
greiner218 4:bc55afdd43de 601 sweep();
greiner218 4:bc55afdd43de 602 break;
greiner218 4:bc55afdd43de 603 }
greiner218 4:bc55afdd43de 604 break;
greiner218 4:bc55afdd43de 605
greiner218 4:bc55afdd43de 606 case '7': //Left
greiner218 4:bc55afdd43de 607 cmd = bt.getc();
greiner218 4:bc55afdd43de 608 switch(cmd)
greiner218 4:bc55afdd43de 609 {
greiner218 4:bc55afdd43de 610 case '1': //Pressed
greiner218 4:bc55afdd43de 611 turnL();
greiner218 4:bc55afdd43de 612 break;
greiner218 4:bc55afdd43de 613
greiner218 4:bc55afdd43de 614 case '0': //Released
greiner218 4:bc55afdd43de 615 moveStp();
greiner218 4:bc55afdd43de 616 sweep();
greiner218 4:bc55afdd43de 617 break;
greiner218 4:bc55afdd43de 618 }
greiner218 4:bc55afdd43de 619 break;
greiner218 4:bc55afdd43de 620
greiner218 4:bc55afdd43de 621 case '8': //Right
greiner218 4:bc55afdd43de 622 cmd = bt.getc();
greiner218 4:bc55afdd43de 623 switch(cmd)
greiner218 4:bc55afdd43de 624 {
greiner218 4:bc55afdd43de 625 case '1': //Pressed
greiner218 4:bc55afdd43de 626 turnR();
greiner218 4:bc55afdd43de 627 break;
greiner218 4:bc55afdd43de 628
greiner218 4:bc55afdd43de 629 case '0': //Released
greiner218 4:bc55afdd43de 630 moveStp();
greiner218 4:bc55afdd43de 631 sweep();
greiner218 4:bc55afdd43de 632 break;
greiner218 4:bc55afdd43de 633 }
greiner218 4:bc55afdd43de 634 break;
greiner218 4:bc55afdd43de 635
greiner218 4:bc55afdd43de 636 case '9': //Right
greiner218 4:bc55afdd43de 637 cmd = bt.getc();
greiner218 4:bc55afdd43de 638 switch(cmd)
greiner218 4:bc55afdd43de 639 {
greiner218 4:bc55afdd43de 640 case '1': //Pressed
greiner218 4:bc55afdd43de 641 slam();
greiner218 4:bc55afdd43de 642 break;
greiner218 4:bc55afdd43de 643
greiner218 4:bc55afdd43de 644 case '0': //Released
greiner218 4:bc55afdd43de 645 break;
greiner218 4:bc55afdd43de 646 }
greiner218 4:bc55afdd43de 647 break;
greiner218 4:bc55afdd43de 648 }
greiner218 4:bc55afdd43de 649
greiner218 2:64585b8d8404 650 break;
greiner218 2:64585b8d8404 651
greiner218 2:64585b8d8404 652 case 'S': //Stop moving
greiner218 2:64585b8d8404 653 pc.printf("Got Command STOP!\n\r");
greiner218 2:64585b8d8404 654 tracking = 0; //Turn off wheel feedback updates
greiner218 2:64585b8d8404 655 speedL = 0;
greiner218 2:64585b8d8404 656 speedR = 0;
greiner218 2:64585b8d8404 657 lwheel.speed(0);
greiner218 2:64585b8d8404 658 rwheel.speed(0);
greiner218 2:64585b8d8404 659 break;
greiner218 2:64585b8d8404 660
greiner218 2:64585b8d8404 661 case 'F': //Forward
greiner218 2:64585b8d8404 662 pc.printf("Got Command FORWARD!\n\r");
greiner218 2:64585b8d8404 663 dirL = 1;
greiner218 2:64585b8d8404 664 dirR = 1;
greiner218 2:64585b8d8404 665 speedL = 0.9f;
greiner218 2:64585b8d8404 666 speedR = 0.9f;
greiner218 2:64585b8d8404 667 rwheel.speed(speedR);
greiner218 2:64585b8d8404 668 lwheel.speed(speedL);
greiner218 2:64585b8d8404 669 tracking = 1; //Turn on wheel feedback updates
greiner218 3:ce743dbbd4a5 670 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 671 tickDistR = 0;
greiner218 2:64585b8d8404 672 break;
greiner218 2:64585b8d8404 673
greiner218 2:64585b8d8404 674 case 'R': //Reverse
greiner218 2:64585b8d8404 675 pc.printf("Got Command REVERSE!\n\r");
greiner218 2:64585b8d8404 676 dirL = -1;
greiner218 2:64585b8d8404 677 dirR = -1;
greiner218 2:64585b8d8404 678 speedL = -0.9f;
greiner218 2:64585b8d8404 679 speedR = -0.9f;
greiner218 2:64585b8d8404 680 rwheel.speed(speedR);
greiner218 2:64585b8d8404 681 lwheel.speed(speedL);
greiner218 2:64585b8d8404 682 tracking = 1;
greiner218 3:ce743dbbd4a5 683 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 684 tickDistR = 0;
greiner218 3:ce743dbbd4a5 685 break;
greiner218 3:ce743dbbd4a5 686
greiner218 3:ce743dbbd4a5 687 case 'P': //Sweep
greiner218 3:ce743dbbd4a5 688 pc.printf("Got Command SWEEP!\n\r");
greiner218 3:ce743dbbd4a5 689 sweep();
greiner218 2:64585b8d8404 690 break;
greiner218 2:64585b8d8404 691
greiner218 2:64585b8d8404 692 default:
greiner218 2:64585b8d8404 693 pc.printf("Unknown Command!\n\r");
greiner218 2:64585b8d8404 694 break;
greiner218 2:64585b8d8404 695
greiner218 1:8638bdaf172b 696 }
greiner218 1:8638bdaf172b 697 }
greiner218 0:616fbf21a20e 698 }
greiner218 4:bc55afdd43de 699 }