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@4:bc55afdd43de, 2016-12-09 (annotated)

Committer:
greiner218
Date:
Fri Dec 09 01:10:31 2016 +0000
Revision:
4:bc55afdd43de
Parent:
3:ce743dbbd4a5
Child:
5:48c8fb81288e
Added 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 3:ce743dbbd4a5 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 4:bc55afdd43de 261 if (timeout.read_ms() > TIMEOUT_VAL_MS*3) 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 3:ce743dbbd4a5 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 3:ce743dbbd4a5 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 4:bc55afdd43de 294 if (timeout.read_ms() > TIMEOUT_VAL_MS*3) 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 3:ce743dbbd4a5 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 4:bc55afdd43de 334 angle_list[slam_ind] = AL;
greiner218 4:bc55afdd43de 335 if (slam_ind < 63) slam_ind++;
greiner218 4:bc55afdd43de 336 }
greiner218 4:bc55afdd43de 337 if (dR > 20 && dR < 200)
greiner218 4:bc55afdd43de 338 {
greiner218 4:bc55afdd43de 339 sendCmd('A',AR);
greiner218 4:bc55afdd43de 340 sendCmd('D',dR);
greiner218 4:bc55afdd43de 341 dist_list[slam_ind] = dL;
greiner218 4:bc55afdd43de 342 angle_list[slam_ind] = AR;
greiner218 4:bc55afdd43de 343 if (slam_ind < 63) slam_ind++;
greiner218 4:bc55afdd43de 344 }
greiner218 4:bc55afdd43de 345 angle = angle + 0.1;
greiner218 3:ce743dbbd4a5 346 }
greiner218 3:ce743dbbd4a5 347 angle = 1;
greiner218 1:8638bdaf172b 348 }
greiner218 3:ce743dbbd4a5 349 else if (angle > 0.5) //Set to max angle and execute backward sweep.
greiner218 3:ce743dbbd4a5 350 {
greiner218 3:ce743dbbd4a5 351 angle = 1;
greiner218 4:bc55afdd43de 352 wait(.2);
greiner218 3:ce743dbbd4a5 353 while( angle > 0 && angle <= 1)
greiner218 3:ce743dbbd4a5 354 {
greiner218 3:ce743dbbd4a5 355 dL = ping(1);
greiner218 3:ce743dbbd4a5 356 dR = ping(2);
greiner218 4:bc55afdd43de 357 AL = angleAdjust(heading + (angle-0.5)*55 - 45);
greiner218 4:bc55afdd43de 358 AR = angleAdjust(heading + (angle-0.5)*55 + 45);
greiner218 4:bc55afdd43de 359 if (dL > 20 && dL < 200)
greiner218 4:bc55afdd43de 360 {
greiner218 4:bc55afdd43de 361 sendCmd('A',AL);
greiner218 4:bc55afdd43de 362 sendCmd('D',dL);
greiner218 4:bc55afdd43de 363 dist_list[slam_ind] = dL;
greiner218 4:bc55afdd43de 364 angle_list[slam_ind] = AL;
greiner218 4:bc55afdd43de 365 if (slam_ind < 63) slam_ind++;
greiner218 4:bc55afdd43de 366 }
greiner218 4:bc55afdd43de 367 if (dR > 20 && dR < 200)
greiner218 4:bc55afdd43de 368 {
greiner218 4:bc55afdd43de 369 sendCmd('A',AR);
greiner218 4:bc55afdd43de 370 sendCmd('D',dR);
greiner218 4:bc55afdd43de 371 dist_list[slam_ind] = dL;
greiner218 4:bc55afdd43de 372 angle_list[slam_ind] = AR;
greiner218 4:bc55afdd43de 373 if (slam_ind < 63) slam_ind++;
greiner218 4:bc55afdd43de 374 }
greiner218 4:bc55afdd43de 375 angle = angle - 0.1;
greiner218 3:ce743dbbd4a5 376 }
greiner218 3:ce743dbbd4a5 377 angle = 0;
greiner218 3:ce743dbbd4a5 378 }
greiner218 4:bc55afdd43de 379 sweeping = 0;
greiner218 4:bc55afdd43de 380 __enable_irq();
greiner218 3:ce743dbbd4a5 381 }
greiner218 3:ce743dbbd4a5 382
greiner218 3:ce743dbbd4a5 383 void updatePos()
greiner218 3:ce743dbbd4a5 384 {
greiner218 4:bc55afdd43de 385 if(!sweeping)
greiner218 4:bc55afdd43de 386 {
greiner218 4:bc55afdd43de 387 float deltaX;
greiner218 4:bc55afdd43de 388 float deltaY;
greiner218 4:bc55afdd43de 389 float dist;
greiner218 4:bc55afdd43de 390 while(!imu.magAvailable(X_AXIS));
greiner218 4:bc55afdd43de 391 imu.readMag();
greiner218 4:bc55afdd43de 392 while(!imu.accelAvailable());
greiner218 4:bc55afdd43de 393 imu.readAccel();
greiner218 4:bc55afdd43de 394 while(!imu.gyroAvailable());
greiner218 4:bc55afdd43de 395 imu.readGyro();
greiner218 4:bc55afdd43de 396 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 4:bc55afdd43de 397 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 4:bc55afdd43de 398 dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR;
greiner218 4:bc55afdd43de 399 dist = (dirL + dirR)*dist/2.0; //Average the ticks and convert to linear distance.
greiner218 4:bc55afdd43de 400 slam_dist += dist;
greiner218 4:bc55afdd43de 401 pc.printf("Magnetic Heading: %f degress ",heading);
greiner218 4:bc55afdd43de 402 pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR);
greiner218 4:bc55afdd43de 403 tickDistR = 0;//Reset the ticks.
greiner218 4:bc55afdd43de 404 tickDistL = 0;
greiner218 4:bc55afdd43de 405 deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig.
greiner218 4:bc55afdd43de 406 deltaY = dist*(float)cos((double)heading*PI/180.0f);
greiner218 4:bc55afdd43de 407 posX = posX + deltaX;
greiner218 4:bc55afdd43de 408 posY = posY + deltaY;
greiner218 4:bc55afdd43de 409 pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY);
greiner218 4:bc55afdd43de 410 pc.printf("posX: %f posY:%f \n\r",posX,posY);
greiner218 4:bc55afdd43de 411 sendCmd('X',posX);
greiner218 4:bc55afdd43de 412 sendCmd('Y',posY);
greiner218 4:bc55afdd43de 413 sendCmd('H',heading);
greiner218 4:bc55afdd43de 414 }
greiner218 1:8638bdaf172b 415 }
greiner218 1:8638bdaf172b 416
greiner218 2:64585b8d8404 417 void incTicksR()
greiner218 2:64585b8d8404 418 {
greiner218 2:64585b8d8404 419 ticksR++;
greiner218 3:ce743dbbd4a5 420 tickDistR++;
greiner218 2:64585b8d8404 421 }
greiner218 2:64585b8d8404 422
greiner218 2:64585b8d8404 423 void incTicksL()
greiner218 2:64585b8d8404 424 {
greiner218 2:64585b8d8404 425 ticksL++;
greiner218 3:ce743dbbd4a5 426 tickDistL++;
greiner218 2:64585b8d8404 427 }
greiner218 2:64585b8d8404 428
greiner218 4:bc55afdd43de 429 void moveFwd()
greiner218 4:bc55afdd43de 430 {
greiner218 4:bc55afdd43de 431 dirL = 1;
greiner218 4:bc55afdd43de 432 dirR = 1;
greiner218 4:bc55afdd43de 433 speedL = 0.9f;
greiner218 4:bc55afdd43de 434 speedR = 0.9f;
greiner218 4:bc55afdd43de 435 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 436 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 437 tracking = 1; //Turn on wheel feedback updates
greiner218 4:bc55afdd43de 438 tickDistL = 0; //Reset the distance ticks.
greiner218 4:bc55afdd43de 439 tickDistR = 0;
greiner218 4:bc55afdd43de 440 }
greiner218 4:bc55afdd43de 441
greiner218 4:bc55afdd43de 442 void moveBwd()
greiner218 4:bc55afdd43de 443 {
greiner218 4:bc55afdd43de 444 dirL = -1;
greiner218 4:bc55afdd43de 445 dirR = -1;
greiner218 4:bc55afdd43de 446 speedL = -0.9f;
greiner218 4:bc55afdd43de 447 speedR = -0.9f;
greiner218 4:bc55afdd43de 448 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 449 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 450 tracking = 1;
greiner218 4:bc55afdd43de 451 tickDistL = 0; //Reset the distance ticks.
greiner218 4:bc55afdd43de 452 tickDistR = 0;
greiner218 4:bc55afdd43de 453 }
greiner218 4:bc55afdd43de 454
greiner218 4:bc55afdd43de 455 void moveStp()
greiner218 4:bc55afdd43de 456 {
greiner218 4:bc55afdd43de 457 tracking = 0; //Turn off wheel feedback updates
greiner218 4:bc55afdd43de 458 speedL = 0;
greiner218 4:bc55afdd43de 459 speedR = 0;
greiner218 4:bc55afdd43de 460 lwheel.speed(0);
greiner218 4:bc55afdd43de 461 rwheel.speed(0);
greiner218 4:bc55afdd43de 462 }
greiner218 4:bc55afdd43de 463
greiner218 4:bc55afdd43de 464 void turnL()
greiner218 4:bc55afdd43de 465 {
greiner218 4:bc55afdd43de 466 tracking = 0;
greiner218 4:bc55afdd43de 467 dirL = -1;
greiner218 4:bc55afdd43de 468 dirR = 1;
greiner218 4:bc55afdd43de 469 speedL = -0.9;
greiner218 4:bc55afdd43de 470 speedR = .9;
greiner218 4:bc55afdd43de 471 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 472 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 473 }
greiner218 4:bc55afdd43de 474
greiner218 4:bc55afdd43de 475 void turnR()
greiner218 4:bc55afdd43de 476 {
greiner218 4:bc55afdd43de 477 tracking = 0;
greiner218 4:bc55afdd43de 478 dirL = 1;
greiner218 4:bc55afdd43de 479 dirR = -1;
greiner218 4:bc55afdd43de 480 speedL = 0.9;
greiner218 4:bc55afdd43de 481 speedR = -0.9;
greiner218 4:bc55afdd43de 482 lwheel.speed(speedL);
greiner218 4:bc55afdd43de 483 rwheel.speed(speedR);
greiner218 4:bc55afdd43de 484 }
greiner218 4:bc55afdd43de 485
greiner218 4:bc55afdd43de 486 int slam_sweep()
greiner218 4:bc55afdd43de 487 {
greiner218 4:bc55afdd43de 488 //Reset the SLAM variables;
greiner218 4:bc55afdd43de 489 memset(angle_list,0,sizeof(angle_list));
greiner218 4:bc55afdd43de 490 memset(dist_list,0,sizeof(dist_list));
greiner218 4:bc55afdd43de 491 slam_ind = 0;
greiner218 4:bc55afdd43de 492 //And sweep
greiner218 4:bc55afdd43de 493 sweep();
greiner218 4:bc55afdd43de 494 //Iterate to determine if detection in "danger zone"
greiner218 4:bc55afdd43de 495 for (int i = 0; i<64; i++)
greiner218 4:bc55afdd43de 496 {
greiner218 4:bc55afdd43de 497 if ((angle_list[i] - heading) < 60 && (angle_list[i] - heading) > -60)
greiner218 4:bc55afdd43de 498 {
greiner218 4:bc55afdd43de 499 if (dist_list[i] < 30)
greiner218 4:bc55afdd43de 500 {
greiner218 4:bc55afdd43de 501 return 1;
greiner218 4:bc55afdd43de 502
greiner218 4:bc55afdd43de 503 }
greiner218 4:bc55afdd43de 504 }
greiner218 4:bc55afdd43de 505 }
greiner218 4:bc55afdd43de 506 return 0;
greiner218 4:bc55afdd43de 507 }
greiner218 4:bc55afdd43de 508
greiner218 4:bc55afdd43de 509 void slam()
greiner218 4:bc55afdd43de 510 {
greiner218 4:bc55afdd43de 511 //Initialize arrays
greiner218 4:bc55afdd43de 512 int detect = 0;
greiner218 4:bc55afdd43de 513 wait(1);
greiner218 4:bc55afdd43de 514 while(1)
greiner218 4:bc55afdd43de 515 {
greiner218 4:bc55afdd43de 516 while(!detect)//while an object is not within 30cm of front 60 degrees of robot...
greiner218 4:bc55afdd43de 517 {
greiner218 4:bc55afdd43de 518 //move forward 20cm and scan
greiner218 4:bc55afdd43de 519 moveFwd();
greiner218 4:bc55afdd43de 520 while(slam_dist < 20)pc.printf("SlamDist: %f\n\r",slam_dist);
greiner218 4:bc55afdd43de 521 moveStp();
greiner218 4:bc55afdd43de 522 slam_dist = 0;
greiner218 4:bc55afdd43de 523 detect = slam_sweep();
greiner218 4:bc55afdd43de 524 }
greiner218 4:bc55afdd43de 525 //Else, turn left slightly.
greiner218 4:bc55afdd43de 526 detect = 0;
greiner218 4:bc55afdd43de 527 turnL();
greiner218 4:bc55afdd43de 528 wait(1);
greiner218 4:bc55afdd43de 529 moveStp();
greiner218 4:bc55afdd43de 530 //and scan
greiner218 4:bc55afdd43de 531 detect = slam_sweep();
greiner218 4:bc55afdd43de 532 }
greiner218 4:bc55afdd43de 533
greiner218 4:bc55afdd43de 534 }
greiner218 4:bc55afdd43de 535
greiner218 0:616fbf21a20e 536 int main()
greiner218 0:616fbf21a20e 537 {
greiner218 2:64585b8d8404 538 char cmd;
greiner218 0:616fbf21a20e 539 timestamp.start();
greiner218 2:64585b8d8404 540 angle = 0.0f;
greiner218 3:ce743dbbd4a5 541 //Calibrate the IMU
greiner218 3:ce743dbbd4a5 542 if (!imu.begin()) {
greiner218 3:ce743dbbd4a5 543 pc.printf("Failed to communicate with LSM9DS1.\n");
greiner218 3:ce743dbbd4a5 544 }
greiner218 3:ce743dbbd4a5 545 imu.calibrate();
greiner218 4:bc55afdd43de 546 turnR();
greiner218 3:ce743dbbd4a5 547 imu.calibrateMag(0);
greiner218 4:bc55afdd43de 548 moveStp();
greiner218 1:8638bdaf172b 549 // Initialize hall-effect control
greiner218 2:64585b8d8404 550 Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
greiner218 1:8638bdaf172b 551 EncL.mode(PullUp); // Use internal pullups
greiner218 1:8638bdaf172b 552 EncR.mode(PullUp);
greiner218 2:64585b8d8404 553 EncR.rise(&incTicksR);
greiner218 2:64585b8d8404 554 EncL.rise(&incTicksL);
greiner218 4:bc55afdd43de 555 memset(prevHeading, 0, sizeof(prevHeading));
greiner218 3:ce743dbbd4a5 556 nav.attach(&updatePos,1);//Update X,Y,H every second
greiner218 2:64585b8d8404 557 while(1)
greiner218 2:64585b8d8404 558 {
greiner218 2:64585b8d8404 559 //Check if char is ready to be read and put into command buffer;
greiner218 2:64585b8d8404 560 if(bt.getc() =='!')
greiner218 2:64585b8d8404 561 {
greiner218 2:64585b8d8404 562 cmd = bt.getc();
greiner218 2:64585b8d8404 563 switch (cmd)
greiner218 2:64585b8d8404 564 {
greiner218 2:64585b8d8404 565 case 'B':
greiner218 4:bc55afdd43de 566 pc.printf("A button was pressed!\n\r");
greiner218 4:bc55afdd43de 567 cmd = bt.getc();
greiner218 4:bc55afdd43de 568 switch(cmd)
greiner218 4:bc55afdd43de 569 {
greiner218 4:bc55afdd43de 570 case '5': //Up
greiner218 4:bc55afdd43de 571 cmd = bt.getc();
greiner218 4:bc55afdd43de 572 switch(cmd)
greiner218 4:bc55afdd43de 573 {
greiner218 4:bc55afdd43de 574 case '1': //Pressed
greiner218 4:bc55afdd43de 575 moveFwd();
greiner218 4:bc55afdd43de 576 break;
greiner218 4:bc55afdd43de 577
greiner218 4:bc55afdd43de 578 case '0': //Released
greiner218 4:bc55afdd43de 579 moveStp();
greiner218 4:bc55afdd43de 580 sweep();
greiner218 4:bc55afdd43de 581 break;
greiner218 4:bc55afdd43de 582 }
greiner218 4:bc55afdd43de 583 break;
greiner218 4:bc55afdd43de 584
greiner218 4:bc55afdd43de 585 case '6': //Down
greiner218 4:bc55afdd43de 586 cmd = bt.getc();
greiner218 4:bc55afdd43de 587 switch(cmd)
greiner218 4:bc55afdd43de 588 {
greiner218 4:bc55afdd43de 589 case '1': //Pressed
greiner218 4:bc55afdd43de 590 moveBwd();
greiner218 4:bc55afdd43de 591 break;
greiner218 4:bc55afdd43de 592
greiner218 4:bc55afdd43de 593 case '0': //Released
greiner218 4:bc55afdd43de 594 moveStp();
greiner218 4:bc55afdd43de 595 sweep();
greiner218 4:bc55afdd43de 596 break;
greiner218 4:bc55afdd43de 597 }
greiner218 4:bc55afdd43de 598 break;
greiner218 4:bc55afdd43de 599
greiner218 4:bc55afdd43de 600 case '7': //Left
greiner218 4:bc55afdd43de 601 cmd = bt.getc();
greiner218 4:bc55afdd43de 602 switch(cmd)
greiner218 4:bc55afdd43de 603 {
greiner218 4:bc55afdd43de 604 case '1': //Pressed
greiner218 4:bc55afdd43de 605 turnL();
greiner218 4:bc55afdd43de 606 break;
greiner218 4:bc55afdd43de 607
greiner218 4:bc55afdd43de 608 case '0': //Released
greiner218 4:bc55afdd43de 609 moveStp();
greiner218 4:bc55afdd43de 610 sweep();
greiner218 4:bc55afdd43de 611 break;
greiner218 4:bc55afdd43de 612 }
greiner218 4:bc55afdd43de 613 break;
greiner218 4:bc55afdd43de 614
greiner218 4:bc55afdd43de 615 case '8': //Right
greiner218 4:bc55afdd43de 616 cmd = bt.getc();
greiner218 4:bc55afdd43de 617 switch(cmd)
greiner218 4:bc55afdd43de 618 {
greiner218 4:bc55afdd43de 619 case '1': //Pressed
greiner218 4:bc55afdd43de 620 turnR();
greiner218 4:bc55afdd43de 621 break;
greiner218 4:bc55afdd43de 622
greiner218 4:bc55afdd43de 623 case '0': //Released
greiner218 4:bc55afdd43de 624 moveStp();
greiner218 4:bc55afdd43de 625 sweep();
greiner218 4:bc55afdd43de 626 break;
greiner218 4:bc55afdd43de 627 }
greiner218 4:bc55afdd43de 628 break;
greiner218 4:bc55afdd43de 629
greiner218 4:bc55afdd43de 630 case '9': //Right
greiner218 4:bc55afdd43de 631 cmd = bt.getc();
greiner218 4:bc55afdd43de 632 switch(cmd)
greiner218 4:bc55afdd43de 633 {
greiner218 4:bc55afdd43de 634 case '1': //Pressed
greiner218 4:bc55afdd43de 635 slam();
greiner218 4:bc55afdd43de 636 break;
greiner218 4:bc55afdd43de 637
greiner218 4:bc55afdd43de 638 case '0': //Released
greiner218 4:bc55afdd43de 639 break;
greiner218 4:bc55afdd43de 640 }
greiner218 4:bc55afdd43de 641 break;
greiner218 4:bc55afdd43de 642 }
greiner218 4:bc55afdd43de 643
greiner218 2:64585b8d8404 644 break;
greiner218 2:64585b8d8404 645
greiner218 2:64585b8d8404 646 case 'S': //Stop moving
greiner218 2:64585b8d8404 647 pc.printf("Got Command STOP!\n\r");
greiner218 2:64585b8d8404 648 tracking = 0; //Turn off wheel feedback updates
greiner218 2:64585b8d8404 649 speedL = 0;
greiner218 2:64585b8d8404 650 speedR = 0;
greiner218 2:64585b8d8404 651 lwheel.speed(0);
greiner218 2:64585b8d8404 652 rwheel.speed(0);
greiner218 2:64585b8d8404 653 break;
greiner218 2:64585b8d8404 654
greiner218 2:64585b8d8404 655 case 'F': //Forward
greiner218 2:64585b8d8404 656 pc.printf("Got Command FORWARD!\n\r");
greiner218 2:64585b8d8404 657 dirL = 1;
greiner218 2:64585b8d8404 658 dirR = 1;
greiner218 2:64585b8d8404 659 speedL = 0.9f;
greiner218 2:64585b8d8404 660 speedR = 0.9f;
greiner218 2:64585b8d8404 661 rwheel.speed(speedR);
greiner218 2:64585b8d8404 662 lwheel.speed(speedL);
greiner218 2:64585b8d8404 663 tracking = 1; //Turn on wheel feedback updates
greiner218 3:ce743dbbd4a5 664 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 665 tickDistR = 0;
greiner218 2:64585b8d8404 666 break;
greiner218 2:64585b8d8404 667
greiner218 2:64585b8d8404 668 case 'R': //Reverse
greiner218 2:64585b8d8404 669 pc.printf("Got Command REVERSE!\n\r");
greiner218 2:64585b8d8404 670 dirL = -1;
greiner218 2:64585b8d8404 671 dirR = -1;
greiner218 2:64585b8d8404 672 speedL = -0.9f;
greiner218 2:64585b8d8404 673 speedR = -0.9f;
greiner218 2:64585b8d8404 674 rwheel.speed(speedR);
greiner218 2:64585b8d8404 675 lwheel.speed(speedL);
greiner218 2:64585b8d8404 676 tracking = 1;
greiner218 3:ce743dbbd4a5 677 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 678 tickDistR = 0;
greiner218 3:ce743dbbd4a5 679 break;
greiner218 3:ce743dbbd4a5 680
greiner218 3:ce743dbbd4a5 681 case 'P': //Sweep
greiner218 3:ce743dbbd4a5 682 pc.printf("Got Command SWEEP!\n\r");
greiner218 3:ce743dbbd4a5 683 sweep();
greiner218 2:64585b8d8404 684 break;
greiner218 2:64585b8d8404 685
greiner218 2:64585b8d8404 686 default:
greiner218 2:64585b8d8404 687 pc.printf("Unknown Command!\n\r");
greiner218 2:64585b8d8404 688 break;
greiner218 2:64585b8d8404 689
greiner218 1:8638bdaf172b 690 }
greiner218 1:8638bdaf172b 691 }
greiner218 0:616fbf21a20e 692 }
greiner218 4:bc55afdd43de 693 }