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?
User | Revision | Line number | New 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 | } |