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@3:ce743dbbd4a5, 2016-12-07 (annotated)

Committer:
greiner218
Date:
Wed Dec 07 22:29:35 2016 +0000
Revision:
3:ce743dbbd4a5
Parent:
2:64585b8d8404
Child:
4:bc55afdd43de
Added some position tracking

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 3:ce743dbbd4a5 7 #define PI 3.14159
greiner218 3:ce743dbbd4a5 8 #define DECLINATION -4.94
greiner218 3:ce743dbbd4a5 9 #define DIST_FACTOR 0.107475 //cm/tick Conversion between a tick from Hall effect sensor and linear distance.
greiner218 3:ce743dbbd4a5 10 //Approximately 190 ticks for a full wheel rotation.
greiner218 3:ce743dbbd4a5 11 //Approximately 6.5cm wheel diameter
greiner218 3:ce743dbbd4a5 12 //pi*diameter (cm) / 190(ticks)
greiner218 3:ce743dbbd4a5 13 volatile float heading;
greiner218 3:ce743dbbd4a5 14 void getHeading(float ax, float ay, float az, float mx, float my, float mz);
greiner218 3:ce743dbbd4a5 15 float PI_cont(float err);
greiner218 0:616fbf21a20e 16
greiner218 1:8638bdaf172b 17 //MOTOR PINS
greiner218 1:8638bdaf172b 18 Motor rwheel(p22, p18, p17); // pwmA, fwd / Ain2, rev / Ain1
greiner218 1:8638bdaf172b 19 Motor lwheel(p23, p15, p16); // pwmB, fwd / Bin2, rev /
greiner218 1:8638bdaf172b 20 //BLUETOOTH PINS
greiner218 1:8638bdaf172b 21 Serial bt(p28,p27);
greiner218 1:8638bdaf172b 22 //LEFT SONAR
greiner218 1:8638bdaf172b 23 DigitalOut triggerL(p14);
greiner218 1:8638bdaf172b 24 DigitalIn echoL(p12);
greiner218 1:8638bdaf172b 25 //RIGHT SONAR
greiner218 1:8638bdaf172b 26 DigitalOut triggerR(p13);
greiner218 1:8638bdaf172b 27 DigitalIn echoR(p11);
greiner218 3:ce743dbbd4a5 28 int correction = 0; //Used to adjust software delay for sonar measurement
greiner218 3:ce743dbbd4a5 29 Timer sonar;
greiner218 3:ce743dbbd4a5 30
greiner218 1:8638bdaf172b 31 //SERVO PINS
greiner218 1:8638bdaf172b 32 Servo angle(p21);
greiner218 1:8638bdaf172b 33 //IMU PINS
greiner218 2:64585b8d8404 34 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
greiner218 3:ce743dbbd4a5 35 Ticker nav;
greiner218 3:ce743dbbd4a5 36 float posX = 0;
greiner218 3:ce743dbbd4a5 37 float posY = 0;
greiner218 3:ce743dbbd4a5 38 int tickDistL = 0;
greiner218 3:ce743dbbd4a5 39 int tickDistR = 0;
greiner218 1:8638bdaf172b 40
greiner218 1:8638bdaf172b 41 Serial pc(USBTX, USBRX);
greiner218 0:616fbf21a20e 42 Timer timestamp;
greiner218 3:ce743dbbd4a5 43
greiner218 1:8638bdaf172b 44 // Set up Hall-effect control
greiner218 2:64585b8d8404 45 InterruptIn EncR(p25);
greiner218 2:64585b8d8404 46 InterruptIn EncL(p24);
greiner218 1:8638bdaf172b 47 int ticksR = 0; // Encoder wheel state change counts
greiner218 1:8638bdaf172b 48 int ticksL = 0;
greiner218 1:8638bdaf172b 49 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
greiner218 1:8638bdaf172b 50 float speedR = 0; //Same for right wheel
greiner218 2:64585b8d8404 51 int dirL = 1; //1 for fwd, -1 for reverse
greiner218 2:64585b8d8404 52 int dirR = 1; //1 for fwd, -1 for reverse
greiner218 1:8638bdaf172b 53 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
greiner218 2:64585b8d8404 54 int tracking = 0; //Flag used to indicate right wheel correction
greiner218 1:8638bdaf172b 55
greiner218 3:ce743dbbd4a5 56 //PI controller
greiner218 3:ce743dbbd4a5 57 //global variables
greiner218 3:ce743dbbd4a5 58 float kp=12.0f;
greiner218 3:ce743dbbd4a5 59 float ki=4.0f;
greiner218 3:ce743dbbd4a5 60 float kd=1.0f;
greiner218 3:ce743dbbd4a5 61 float P=0;
greiner218 3:ce743dbbd4a5 62 float I=0;
greiner218 3:ce743dbbd4a5 63 float D=0;
greiner218 3:ce743dbbd4a5 64 float dt=0.1f;
greiner218 3:ce743dbbd4a5 65 float out=0.0f;
greiner218 3:ce743dbbd4a5 66 float max=0.5f;
greiner218 3:ce743dbbd4a5 67 float min=-0.5f;
greiner218 3:ce743dbbd4a5 68 float prerr=0.0f;
greiner218 3:ce743dbbd4a5 69
greiner218 1:8638bdaf172b 70 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
greiner218 1:8638bdaf172b 71 void sampleEncoder()
greiner218 1:8638bdaf172b 72 {
greiner218 2:64585b8d8404 73 float E; // Error between the 2 wheel speeds
greiner218 2:64585b8d8404 74 if(tracking) { //Right wheel "tracks" left wheel if enabled.
greiner218 1:8638bdaf172b 75
greiner218 1:8638bdaf172b 76 E = ticksL - ticksR; //Find error
greiner218 2:64585b8d8404 77 //E = (ticksL+1)/(ticksR+1);
greiner218 2:64585b8d8404 78 if (dirL == 1)
greiner218 2:64585b8d8404 79 {
greiner218 2:64585b8d8404 80 speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 81 //speedR = E*speedR;
greiner218 2:64585b8d8404 82 if (speedR < 0) speedR = 0;
greiner218 2:64585b8d8404 83 }
greiner218 2:64585b8d8404 84 else if (dirL == -1)
greiner218 2:64585b8d8404 85 {
greiner218 2:64585b8d8404 86 speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
greiner218 2:64585b8d8404 87 //speedR = E*speedR;
greiner218 2:64585b8d8404 88 if (speedR > 0) speedR = 0;
greiner218 2:64585b8d8404 89 }
greiner218 1:8638bdaf172b 90 rwheel.speed(speedR);
greiner218 1:8638bdaf172b 91 }
greiner218 2:64585b8d8404 92 //pc.printf("tickL: %i tickR: %i Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
greiner218 1:8638bdaf172b 93 ticksR = 0; //Restart the counters
greiner218 1:8638bdaf172b 94 ticksL = 0;
greiner218 1:8638bdaf172b 95 }
greiner218 0:616fbf21a20e 96
greiner218 3:ce743dbbd4a5 97 void getHeading(float ax, float ay, float az, float mx, float my, float mz)
greiner218 3:ce743dbbd4a5 98 {
greiner218 3:ce743dbbd4a5 99 float roll = atan2(ay, az);
greiner218 3:ce743dbbd4a5 100 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
greiner218 3:ce743dbbd4a5 101 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
greiner218 3:ce743dbbd4a5 102 mx = -mx;
greiner218 3:ce743dbbd4a5 103 if (my == 0.0)
greiner218 3:ce743dbbd4a5 104 heading = (mx < 0.0) ? 180.0 : 0.0;
greiner218 3:ce743dbbd4a5 105 else
greiner218 3:ce743dbbd4a5 106 heading = atan2(mx, my)*360.0/(2.0*PI);
greiner218 3:ce743dbbd4a5 107 //pc.printf("heading atan=%f \n\r",heading);
greiner218 3:ce743dbbd4a5 108 heading -= DECLINATION; //correct for geo location
greiner218 3:ce743dbbd4a5 109 if(heading>180.0) heading = heading - 360.0;
greiner218 3:ce743dbbd4a5 110 else if(heading<-180.0) heading = 360.0 + heading;
greiner218 3:ce743dbbd4a5 111 else if(heading<0.0) heading = 360.0 + heading;
greiner218 3:ce743dbbd4a5 112 }
greiner218 3:ce743dbbd4a5 113
greiner218 3:ce743dbbd4a5 114 float PI_cont(float err)
greiner218 3:ce743dbbd4a5 115 {
greiner218 3:ce743dbbd4a5 116 float Pout=kp*err;
greiner218 3:ce743dbbd4a5 117 P=Pout;
greiner218 3:ce743dbbd4a5 118 //pc.printf("P is: %f \n\r",P);
greiner218 3:ce743dbbd4a5 119
greiner218 3:ce743dbbd4a5 120 float Iout=Iout+err*dt;
greiner218 3:ce743dbbd4a5 121 I=ki*Iout;
greiner218 3:ce743dbbd4a5 122 //pc.printf("I is: %f \n\r",I);
greiner218 3:ce743dbbd4a5 123
greiner218 3:ce743dbbd4a5 124 float Dout = (err - prerr)/dt;
greiner218 3:ce743dbbd4a5 125 D=kd*Dout;
greiner218 3:ce743dbbd4a5 126
greiner218 3:ce743dbbd4a5 127 out=P+I+D;
greiner218 3:ce743dbbd4a5 128
greiner218 3:ce743dbbd4a5 129 prerr=err;
greiner218 3:ce743dbbd4a5 130
greiner218 3:ce743dbbd4a5 131 //pc.printf("out is: %f \n\r",out); // basically pwm out
greiner218 3:ce743dbbd4a5 132 if (out>max)out=max;
greiner218 3:ce743dbbd4a5 133 else if (out<min)out=min;
greiner218 3:ce743dbbd4a5 134 return out;
greiner218 3:ce743dbbd4a5 135 }
greiner218 3:ce743dbbd4a5 136
greiner218 3:ce743dbbd4a5 137 void turn(float angle)
greiner218 3:ce743dbbd4a5 138 {
greiner218 3:ce743dbbd4a5 139 float s =0.05;//speed by which the robot should turn
greiner218 3:ce743dbbd4a5 140 while(!imu.magAvailable(X_AXIS));
greiner218 3:ce743dbbd4a5 141 imu.readMag();
greiner218 3:ce743dbbd4a5 142 while(!imu.accelAvailable());
greiner218 3:ce743dbbd4a5 143 imu.readAccel();
greiner218 3:ce743dbbd4a5 144 while(!imu.gyroAvailable());
greiner218 3:ce743dbbd4a5 145 imu.readGyro();
greiner218 3:ce743dbbd4a5 146 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 3:ce743dbbd4a5 147 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 3:ce743dbbd4a5 148 float PV = heading;//the original heading
greiner218 3:ce743dbbd4a5 149 float SP = heading - angle;//the new required heading
greiner218 3:ce743dbbd4a5 150 if(SP>=360){//reseting after 360 degrees to 0
greiner218 3:ce743dbbd4a5 151 SP = SP-360;
greiner218 3:ce743dbbd4a5 152 }
greiner218 3:ce743dbbd4a5 153 pc.printf("PV: %f\n",PV);
greiner218 3:ce743dbbd4a5 154 pc.printf("SV: %f\n",SP);
greiner218 3:ce743dbbd4a5 155 wait(2);
greiner218 3:ce743dbbd4a5 156 //pc.printf("required: %f\n",requiredHeading);
greiner218 3:ce743dbbd4a5 157 float error = angle;
greiner218 3:ce743dbbd4a5 158 pc.printf("error: %f\n",error);
greiner218 3:ce743dbbd4a5 159 int reached = 0;
greiner218 3:ce743dbbd4a5 160 while(reached ==0){
greiner218 3:ce743dbbd4a5 161
greiner218 3:ce743dbbd4a5 162 while(!imu.magAvailable(ALL_AXIS));
greiner218 3:ce743dbbd4a5 163 imu.readMag();
greiner218 3:ce743dbbd4a5 164 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 3:ce743dbbd4a5 165 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 3:ce743dbbd4a5 166 PV = heading;//calculate the current heading
greiner218 3:ce743dbbd4a5 167
greiner218 3:ce743dbbd4a5 168 error=(PV-SP)/360;//find error between current heading and required heading
greiner218 3:ce743dbbd4a5 169 float correction=PI_cont(error);//correction to motor speed
greiner218 3:ce743dbbd4a5 170 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 171 rwheel.speed((s+correction)/2);
greiner218 3:ce743dbbd4a5 172 lwheel.speed((s-correction)/2);
greiner218 3:ce743dbbd4a5 173 pc.printf("error in 1st if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 174 pc.printf("error in 1st if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 175 }
greiner218 3:ce743dbbd4a5 176 else if (error>=-1.5/360 && error<=1.5/360) { //within within satisfying angular range
greiner218 3:ce743dbbd4a5 177 rwheel.speed(0);
greiner218 3:ce743dbbd4a5 178 lwheel.speed(0);//stop moving and exit the loop
greiner218 3:ce743dbbd4a5 179 pc.printf("error in 2nd if: %f\n\r",error);
greiner218 3:ce743dbbd4a5 180 break;
greiner218 3:ce743dbbd4a5 181 }
greiner218 3:ce743dbbd4a5 182 wait(0.01);
greiner218 3:ce743dbbd4a5 183 }
greiner218 3:ce743dbbd4a5 184
greiner218 3:ce743dbbd4a5 185 rwheel.speed(0);
greiner218 3:ce743dbbd4a5 186 lwheel.speed(0);
greiner218 3:ce743dbbd4a5 187 }
greiner218 3:ce743dbbd4a5 188
greiner218 3:ce743dbbd4a5 189
greiner218 3:ce743dbbd4a5 190 void sendCmd(char cmd, float arg)
greiner218 3:ce743dbbd4a5 191 {
greiner218 3:ce743dbbd4a5 192 unsigned char c[sizeof arg];
greiner218 3:ce743dbbd4a5 193 memcpy(c, &arg, sizeof arg);
greiner218 3:ce743dbbd4a5 194 bt.putc('!');
greiner218 3:ce743dbbd4a5 195 bt.putc(cmd);
greiner218 3:ce743dbbd4a5 196 bt.putc(c[0]);
greiner218 3:ce743dbbd4a5 197 bt.putc(c[1]);
greiner218 3:ce743dbbd4a5 198 bt.putc(c[2]);
greiner218 3:ce743dbbd4a5 199 bt.putc(c[3]);
greiner218 3:ce743dbbd4a5 200
greiner218 3:ce743dbbd4a5 201 }
greiner218 3:ce743dbbd4a5 202
greiner218 1:8638bdaf172b 203 int ping(int i)
greiner218 1:8638bdaf172b 204 {
greiner218 3:ce743dbbd4a5 205 float distance = 0;
greiner218 1:8638bdaf172b 206 switch(i)
greiner218 1:8638bdaf172b 207 {
greiner218 1:8638bdaf172b 208
greiner218 1:8638bdaf172b 209 case(1): //Ping Left Sonar
greiner218 1:8638bdaf172b 210 // trigger sonar to send a ping
greiner218 3:ce743dbbd4a5 211 pc.printf("Pinging sonar 1...\n\r");
greiner218 1:8638bdaf172b 212 triggerL = 1;
greiner218 1:8638bdaf172b 213 sonar.reset();
greiner218 3:ce743dbbd4a5 214 //wait_us(10.0);
greiner218 1:8638bdaf172b 215 //wait for echo high
greiner218 1:8638bdaf172b 216 while (echoL==0) {};
greiner218 1:8638bdaf172b 217 //echo high, so start timer
greiner218 1:8638bdaf172b 218 sonar.start();
greiner218 3:ce743dbbd4a5 219 triggerL = 0;
greiner218 1:8638bdaf172b 220 //wait for echo low
greiner218 1:8638bdaf172b 221 while (echoL==1) {};
greiner218 1:8638bdaf172b 222 //stop timer and read value
greiner218 1:8638bdaf172b 223 sonar.stop();
greiner218 1:8638bdaf172b 224 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 225 distance = (sonar.read_us()-correction)/58.0;
greiner218 3:ce743dbbd4a5 226 pc.printf("Got distance %f cm back.\n\r", distance);
greiner218 1:8638bdaf172b 227 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 228 wait(0.015);
greiner218 1:8638bdaf172b 229 break;
greiner218 1:8638bdaf172b 230 case(2): //Ping Right Sonar
greiner218 1:8638bdaf172b 231 // trigger sonar to send a ping
greiner218 3:ce743dbbd4a5 232 pc.printf("Pinging sonar 2...\n\r");
greiner218 1:8638bdaf172b 233 triggerR = 1;
greiner218 1:8638bdaf172b 234 sonar.reset();
greiner218 3:ce743dbbd4a5 235 //wait_us(10.0);
greiner218 1:8638bdaf172b 236 //wait for echo high
greiner218 1:8638bdaf172b 237 while (echoR==0) {};
greiner218 1:8638bdaf172b 238 //echo high, so start timer
greiner218 1:8638bdaf172b 239 sonar.start();
greiner218 3:ce743dbbd4a5 240 triggerR = 0;
greiner218 1:8638bdaf172b 241 //wait for echo low
greiner218 1:8638bdaf172b 242 while (echoR==1) {};
greiner218 1:8638bdaf172b 243 //stop timer and read value
greiner218 1:8638bdaf172b 244 sonar.stop();
greiner218 1:8638bdaf172b 245 //subtract software overhead timer delay and scale to cm
greiner218 1:8638bdaf172b 246 distance = (sonar.read_us()-correction)/58.0;
greiner218 3:ce743dbbd4a5 247 pc.printf("Got distance %f cm back.\n\r", distance);
greiner218 1:8638bdaf172b 248 //wait so that any echo(s) return before sending another ping
greiner218 1:8638bdaf172b 249 wait(0.015);
greiner218 1:8638bdaf172b 250 break;
greiner218 1:8638bdaf172b 251 default:
greiner218 1:8638bdaf172b 252 break;
greiner218 3:ce743dbbd4a5 253 }
greiner218 3:ce743dbbd4a5 254 return distance;
greiner218 3:ce743dbbd4a5 255 }
greiner218 3:ce743dbbd4a5 256
greiner218 3:ce743dbbd4a5 257 void sweep()
greiner218 3:ce743dbbd4a5 258 {
greiner218 3:ce743dbbd4a5 259 int dL = 0;
greiner218 3:ce743dbbd4a5 260 int dR = 0;
greiner218 3:ce743dbbd4a5 261 if (angle <=0.5) //Set to min angle and execute forward sweep.
greiner218 3:ce743dbbd4a5 262 {
greiner218 3:ce743dbbd4a5 263 angle = 0;
greiner218 3:ce743dbbd4a5 264 wait(.5);
greiner218 3:ce743dbbd4a5 265 while( angle >= 0 && angle < 1)
greiner218 3:ce743dbbd4a5 266 {
greiner218 3:ce743dbbd4a5 267 dL = ping(1);
greiner218 3:ce743dbbd4a5 268 dR = ping(2);
greiner218 3:ce743dbbd4a5 269 sendCmd('D',dL);
greiner218 3:ce743dbbd4a5 270 sendCmd('D',dR);
greiner218 3:ce743dbbd4a5 271 angle = angle + 0.05;
greiner218 3:ce743dbbd4a5 272 }
greiner218 3:ce743dbbd4a5 273 angle = 1;
greiner218 1:8638bdaf172b 274 }
greiner218 3:ce743dbbd4a5 275 else if (angle > 0.5) //Set to max angle and execute backward sweep.
greiner218 3:ce743dbbd4a5 276 {
greiner218 3:ce743dbbd4a5 277 angle = 1;
greiner218 3:ce743dbbd4a5 278 wait(.5);
greiner218 3:ce743dbbd4a5 279 while( angle > 0 && angle <= 1)
greiner218 3:ce743dbbd4a5 280 {
greiner218 3:ce743dbbd4a5 281 dL = ping(1);
greiner218 3:ce743dbbd4a5 282 dR = ping(2);
greiner218 3:ce743dbbd4a5 283 sendCmd('D',dL);
greiner218 3:ce743dbbd4a5 284 sendCmd('D',dR);
greiner218 3:ce743dbbd4a5 285 angle = angle - 0.05;
greiner218 3:ce743dbbd4a5 286 }
greiner218 3:ce743dbbd4a5 287 angle = 0;
greiner218 3:ce743dbbd4a5 288 }
greiner218 3:ce743dbbd4a5 289 }
greiner218 3:ce743dbbd4a5 290
greiner218 3:ce743dbbd4a5 291 void updatePos()
greiner218 3:ce743dbbd4a5 292 {
greiner218 3:ce743dbbd4a5 293 float deltaX;
greiner218 3:ce743dbbd4a5 294 float deltaY;
greiner218 3:ce743dbbd4a5 295 float dist;
greiner218 3:ce743dbbd4a5 296 while(!imu.magAvailable(X_AXIS));
greiner218 3:ce743dbbd4a5 297 imu.readMag();
greiner218 3:ce743dbbd4a5 298 while(!imu.accelAvailable());
greiner218 3:ce743dbbd4a5 299 imu.readAccel();
greiner218 3:ce743dbbd4a5 300 while(!imu.gyroAvailable());
greiner218 3:ce743dbbd4a5 301 imu.readGyro();
greiner218 3:ce743dbbd4a5 302 getHeading(imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az), imu.calcMag(imu.mx),
greiner218 3:ce743dbbd4a5 303 imu.calcMag(imu.my), imu.calcMag(imu.mz));
greiner218 3:ce743dbbd4a5 304 dist = (tickDistR + tickDistL)/2.0f*DIST_FACTOR; //Average the ticks and convert to linear distance.
greiner218 3:ce743dbbd4a5 305 pc.printf("Magnetic Heading: %f degress ",heading);
greiner218 3:ce743dbbd4a5 306 pc.printf("L ticks: %d R ticks: %d ",tickDistL, tickDistR);
greiner218 3:ce743dbbd4a5 307 tickDistR = 0;//Reset the ticks.
greiner218 3:ce743dbbd4a5 308 tickDistL = 0;
greiner218 3:ce743dbbd4a5 309 deltaX = dist*(float)sin((double)heading*PI/180.0f); //Do trig.
greiner218 3:ce743dbbd4a5 310 deltaY = dist*(float)cos((double)heading*PI/180.0f);
greiner218 3:ce743dbbd4a5 311 posX = posX + deltaX;
greiner218 3:ce743dbbd4a5 312 posY = posY + deltaY;
greiner218 3:ce743dbbd4a5 313 pc.printf("deltaX: %f deltaY: %f ",deltaX,deltaY);
greiner218 3:ce743dbbd4a5 314 pc.printf("posX: %f posY:%f \n\r",posX,posY);
greiner218 1:8638bdaf172b 315 }
greiner218 1:8638bdaf172b 316
greiner218 2:64585b8d8404 317 void incTicksR()
greiner218 2:64585b8d8404 318 {
greiner218 2:64585b8d8404 319 ticksR++;
greiner218 3:ce743dbbd4a5 320 tickDistR++;
greiner218 2:64585b8d8404 321 }
greiner218 2:64585b8d8404 322
greiner218 2:64585b8d8404 323 void incTicksL()
greiner218 2:64585b8d8404 324 {
greiner218 2:64585b8d8404 325 ticksL++;
greiner218 3:ce743dbbd4a5 326 tickDistL++;
greiner218 2:64585b8d8404 327 }
greiner218 2:64585b8d8404 328
greiner218 0:616fbf21a20e 329 int main()
greiner218 0:616fbf21a20e 330 {
greiner218 2:64585b8d8404 331 char cmd;
greiner218 0:616fbf21a20e 332 timestamp.start();
greiner218 2:64585b8d8404 333 angle = 0.0f;
greiner218 3:ce743dbbd4a5 334 //Calibrate the IMU
greiner218 3:ce743dbbd4a5 335 if (!imu.begin()) {
greiner218 3:ce743dbbd4a5 336 pc.printf("Failed to communicate with LSM9DS1.\n");
greiner218 3:ce743dbbd4a5 337 }
greiner218 3:ce743dbbd4a5 338 imu.calibrate();
greiner218 3:ce743dbbd4a5 339 imu.calibrateMag(0);
greiner218 1:8638bdaf172b 340 // Initialize hall-effect control
greiner218 2:64585b8d8404 341 Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
greiner218 1:8638bdaf172b 342 EncL.mode(PullUp); // Use internal pullups
greiner218 1:8638bdaf172b 343 EncR.mode(PullUp);
greiner218 2:64585b8d8404 344 EncR.rise(&incTicksR);
greiner218 2:64585b8d8404 345 EncL.rise(&incTicksL);
greiner218 3:ce743dbbd4a5 346 nav.attach(&updatePos,1);//Update X,Y,H every second
greiner218 2:64585b8d8404 347 while(1)
greiner218 2:64585b8d8404 348 {
greiner218 2:64585b8d8404 349 //Check if char is ready to be read and put into command buffer;
greiner218 2:64585b8d8404 350 if(bt.getc() =='!')
greiner218 2:64585b8d8404 351 {
greiner218 2:64585b8d8404 352 cmd = bt.getc();
greiner218 2:64585b8d8404 353 switch (cmd)
greiner218 2:64585b8d8404 354 {
greiner218 2:64585b8d8404 355 case 'B':
greiner218 2:64585b8d8404 356 pc.printf("Got Command B!\n\r");
greiner218 2:64585b8d8404 357 break;
greiner218 2:64585b8d8404 358
greiner218 2:64585b8d8404 359 case 'S': //Stop moving
greiner218 2:64585b8d8404 360 pc.printf("Got Command STOP!\n\r");
greiner218 2:64585b8d8404 361 tracking = 0; //Turn off wheel feedback updates
greiner218 2:64585b8d8404 362 speedL = 0;
greiner218 2:64585b8d8404 363 speedR = 0;
greiner218 2:64585b8d8404 364 lwheel.speed(0);
greiner218 2:64585b8d8404 365 rwheel.speed(0);
greiner218 2:64585b8d8404 366 break;
greiner218 2:64585b8d8404 367
greiner218 2:64585b8d8404 368 case 'F': //Forward
greiner218 2:64585b8d8404 369 pc.printf("Got Command FORWARD!\n\r");
greiner218 2:64585b8d8404 370 dirL = 1;
greiner218 2:64585b8d8404 371 dirR = 1;
greiner218 2:64585b8d8404 372 speedL = 0.9f;
greiner218 2:64585b8d8404 373 speedR = 0.9f;
greiner218 2:64585b8d8404 374 rwheel.speed(speedR);
greiner218 2:64585b8d8404 375 lwheel.speed(speedL);
greiner218 2:64585b8d8404 376 tracking = 1; //Turn on wheel feedback updates
greiner218 3:ce743dbbd4a5 377 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 378 tickDistR = 0;
greiner218 2:64585b8d8404 379 break;
greiner218 2:64585b8d8404 380
greiner218 2:64585b8d8404 381 case 'R': //Reverse
greiner218 2:64585b8d8404 382 pc.printf("Got Command REVERSE!\n\r");
greiner218 2:64585b8d8404 383 dirL = -1;
greiner218 2:64585b8d8404 384 dirR = -1;
greiner218 2:64585b8d8404 385 speedL = -0.9f;
greiner218 2:64585b8d8404 386 speedR = -0.9f;
greiner218 2:64585b8d8404 387 rwheel.speed(speedR);
greiner218 2:64585b8d8404 388 lwheel.speed(speedL);
greiner218 2:64585b8d8404 389 tracking = 1;
greiner218 3:ce743dbbd4a5 390 tickDistL = 0; //Reset the distance ticks.
greiner218 3:ce743dbbd4a5 391 tickDistR = 0;
greiner218 3:ce743dbbd4a5 392 break;
greiner218 3:ce743dbbd4a5 393
greiner218 3:ce743dbbd4a5 394 case 'P': //Sweep
greiner218 3:ce743dbbd4a5 395 pc.printf("Got Command SWEEP!\n\r");
greiner218 3:ce743dbbd4a5 396 sweep();
greiner218 2:64585b8d8404 397 break;
greiner218 2:64585b8d8404 398
greiner218 2:64585b8d8404 399 default:
greiner218 2:64585b8d8404 400 pc.printf("Unknown Command!\n\r");
greiner218 2:64585b8d8404 401 break;
greiner218 2:64585b8d8404 402
greiner218 1:8638bdaf172b 403 }
greiner218 1:8638bdaf172b 404 }
greiner218 0:616fbf21a20e 405 }
greiner218 0:616fbf21a20e 406 }