Saul Crumpton / Mbed 2 deprecated 4012Code

forking so that I can submit a pull request to jmiller322

Dependencies:   LSM9DS1_Library_cal TCA9548A VL53L0X mbed

Fork of PWM by Jordan Miller

main.cpp@3:77b1f376acea, 2018-04-15 (annotated)

Committer:
Joe5181
Date:
Sun Apr 15 17:49:27 2018 +0000
Revision:
3:77b1f376acea
Parent:
2:2a4e77f78c8a 
Trying to push back to Jordan's branch

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmiller322 0:52b06cb96c86 1 #include "mbed.h"
jmiller322 0:52b06cb96c86 2 #include "tca9548a.h"
jmiller322 0:52b06cb96c86 3 #include "VL53L0X.h"
jmiller322 0:52b06cb96c86 4 #include "LSM9DS1.h"
jmiller322 0:52b06cb96c86 5 #define PI 3.14159
jmiller322 0:52b06cb96c86 6 #define DECLINATION -4.94
jmiller322 0:52b06cb96c86 7
jmiller322 0:52b06cb96c86 8 PwmOut PWM(p21); //Right Side Motor
jmiller322 0:52b06cb96c86 9 PwmOut PWM1(p22); //Left Side Motor
jmiller322 0:52b06cb96c86 10 Serial pc(USBTX, USBRX);
Joe5181 2:2a4e77f78c8a 11 TCA9548A i2c_sw(I2C_SDA, I2C_SCL); //default address 0x70 applied
Joe5181 2:2a4e77f78c8a 12 VL53L0X ToF(I2C_SDA, I2C_SCL);
jmiller322 0:52b06cb96c86 13 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //imu
Joe5181 2:2a4e77f78c8a 14
jmiller322 0:52b06cb96c86 15
jmiller322 0:52b06cb96c86 16 float dutyLeft=0.130;
jmiller322 0:52b06cb96c86 17 float dutyRight=0.130;
jmiller322 1:bae4f01ae25c 18 float desHeading = 180.0;
jmiller322 0:52b06cb96c86 19 float currentDegreeHeading;
jmiller322 0:52b06cb96c86 20 float distanceAway;
jmiller322 0:52b06cb96c86 21
jmiller322 0:52b06cb96c86 22 float calculateHeading(float ax, float ay, float az, float mx, float my, float mz){
jmiller322 0:52b06cb96c86 23 float roll = atan2(ay, az);
jmiller322 0:52b06cb96c86 24 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jmiller322 0:52b06cb96c86 25 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jmiller322 0:52b06cb96c86 26 mx = -mx;
jmiller322 0:52b06cb96c86 27 float heading;
jmiller322 0:52b06cb96c86 28 if (my == 0.0)
jmiller322 0:52b06cb96c86 29 heading = (mx < 0.0) ? 180.0 : 0.0;
jmiller322 0:52b06cb96c86 30 else
jmiller322 0:52b06cb96c86 31 heading = atan2(mx, my)*360.0/(2.0*PI);
jmiller322 0:52b06cb96c86 32 //pc.printf("heading atan=%f \n\r",heading);
jmiller322 0:52b06cb96c86 33 heading -= DECLINATION; //correct for geo location
jmiller322 0:52b06cb96c86 34 if(heading>180.0) heading = heading - 360.0;
jmiller322 0:52b06cb96c86 35 else if(heading<-180.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 36 else if(heading<0.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 37
jmiller322 0:52b06cb96c86 38 pitch *= 180.0 / PI;
jmiller322 0:52b06cb96c86 39 roll *= 180.0 / PI;
jmiller322 0:52b06cb96c86 40
jmiller322 0:52b06cb96c86 41 return heading;
jmiller322 0:52b06cb96c86 42 }
jmiller322 0:52b06cb96c86 43
jmiller322 0:52b06cb96c86 44
jmiller322 0:52b06cb96c86 45 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
jmiller322 0:52b06cb96c86 46 {
jmiller322 0:52b06cb96c86 47 float roll = atan2(ay, az);
jmiller322 0:52b06cb96c86 48 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jmiller322 0:52b06cb96c86 49 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jmiller322 0:52b06cb96c86 50 mx = -mx;
jmiller322 0:52b06cb96c86 51 float heading;
jmiller322 0:52b06cb96c86 52 if (my == 0.0)
jmiller322 0:52b06cb96c86 53 heading = (mx < 0.0) ? 180.0 : 0.0;
jmiller322 0:52b06cb96c86 54 else
jmiller322 0:52b06cb96c86 55 heading = atan2(mx, my)*360.0/(2.0*PI);
jmiller322 0:52b06cb96c86 56 heading -= DECLINATION; //correct for geo location
jmiller322 0:52b06cb96c86 57 if(heading>180.0) heading = heading - 360.0;
jmiller322 0:52b06cb96c86 58 else if(heading<-180.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 59 else if(heading<0.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 60
jmiller322 0:52b06cb96c86 61 pitch *= 180.0 / PI;
jmiller322 0:52b06cb96c86 62 roll *= 180.0 / PI;
jmiller322 0:52b06cb96c86 63
jmiller322 0:52b06cb96c86 64 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
jmiller322 0:52b06cb96c86 65 pc.printf("Magnetic Heading: %f degress\n\r",heading);
jmiller322 0:52b06cb96c86 66 }
jmiller322 0:52b06cb96c86 67
jmiller322 0:52b06cb96c86 68 void calibrateIMU()
jmiller322 0:52b06cb96c86 69 {
jmiller322 0:52b06cb96c86 70 IMU.begin();
jmiller322 0:52b06cb96c86 71 if (!IMU.begin()) {
jmiller322 0:52b06cb96c86 72 pc.printf("Failed to communicate with LSM9DS1.\n");
jmiller322 0:52b06cb96c86 73 }
jmiller322 0:52b06cb96c86 74 IMU.calibrate(1);
jmiller322 0:52b06cb96c86 75 IMU.calibrateMag(0);
jmiller322 0:52b06cb96c86 76 return;
jmiller322 0:52b06cb96c86 77 }
jmiller322 0:52b06cb96c86 78
jmiller322 0:52b06cb96c86 79 void initToF ()
jmiller322 0:52b06cb96c86 80 {
jmiller322 0:52b06cb96c86 81 // By default TCA9548A performs a power on reset and all downstream ports are deselected
Joe5181 2:2a4e77f78c8a 82 for(int i=0;i<4;++i){
Joe5181 2:2a4e77f78c8a 83 printf(" initializing Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 84 i2c_sw.select(i); // select the channel 0
Joe5181 2:2a4e77f78c8a 85 ToF.init(false);
Joe5181 2:2a4e77f78c8a 86 printf("finished initializing Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 87 ToF.startContinuous();
Joe5181 2:2a4e77f78c8a 88 printf("finished starting continous readings for Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 89 }
Joe5181 2:2a4e77f78c8a 90 i2c_sw.select(0);
jmiller322 0:52b06cb96c86 91 }
jmiller322 0:52b06cb96c86 92
jmiller322 0:52b06cb96c86 93 void readToF()
jmiller322 0:52b06cb96c86 94 {
Joe5181 2:2a4e77f78c8a 95 printf("Tof 1:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 96 i2c_sw.select(1);
Joe5181 2:2a4e77f78c8a 97 printf("Tof 2:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 98 i2c_sw.select(2);
Joe5181 2:2a4e77f78c8a 99 printf("Tof 3:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 100 i2c_sw.select(3);
Joe5181 2:2a4e77f78c8a 101 printf("Tof 4:%d mm \n",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 102 i2c_sw.select(0);
Joe5181 2:2a4e77f78c8a 103 wait( 0.1 );
jmiller322 0:52b06cb96c86 104 }
jmiller322 0:52b06cb96c86 105
jmiller322 0:52b06cb96c86 106 void readIMU()
jmiller322 0:52b06cb96c86 107 {
jmiller322 0:52b06cb96c86 108 while(1){
jmiller322 0:52b06cb96c86 109 while(!IMU.tempAvailable());
jmiller322 0:52b06cb96c86 110 IMU.readTemp();
jmiller322 0:52b06cb96c86 111 while(!IMU.magAvailable(X_AXIS));
jmiller322 0:52b06cb96c86 112 IMU.readMag();
jmiller322 0:52b06cb96c86 113 while(!IMU.accelAvailable());
jmiller322 0:52b06cb96c86 114 IMU.readAccel();
jmiller322 0:52b06cb96c86 115 while(!IMU.gyroAvailable());
jmiller322 0:52b06cb96c86 116 IMU.readGyro();
jmiller322 0:52b06cb96c86 117 //pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
jmiller322 0:52b06cb96c86 118 //pc.printf(" X axis Y axis Z axis\n\r");
jmiller322 0:52b06cb96c86 119 //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jmiller322 0:52b06cb96c86 120 //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
jmiller322 0:52b06cb96c86 121 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jmiller322 0:52b06cb96c86 122 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jmiller322 0:52b06cb96c86 123 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jmiller322 0:52b06cb96c86 124 //myled = 1;
jmiller322 0:52b06cb96c86 125 wait(0.25);
jmiller322 0:52b06cb96c86 126 //myled = 0;
jmiller322 0:52b06cb96c86 127 wait(0.25);
jmiller322 0:52b06cb96c86 128 }
jmiller322 0:52b06cb96c86 129 }
jmiller322 0:52b06cb96c86 130
jmiller322 0:52b06cb96c86 131 void turnRight()
jmiller322 0:52b06cb96c86 132 {
jmiller322 0:52b06cb96c86 133 dutyLeft = 0.18;
jmiller322 0:52b06cb96c86 134 dutyRight = 0.05; //turn right
jmiller322 0:52b06cb96c86 135 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 136 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 137 pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 138 }
jmiller322 0:52b06cb96c86 139
jmiller322 0:52b06cb96c86 140 void turnLeft()
jmiller322 0:52b06cb96c86 141 {
jmiller322 0:52b06cb96c86 142 dutyRight = 0.18; //increase right motor speed
jmiller322 0:52b06cb96c86 143 dutyLeft = 0.05; //Stop left motor
jmiller322 0:52b06cb96c86 144 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 145 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 146 pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 147 }
jmiller322 0:52b06cb96c86 148
jmiller322 0:52b06cb96c86 149 void goForward()
jmiller322 0:52b06cb96c86 150 {
jmiller322 0:52b06cb96c86 151 if(dutyRight < dutyLeft) {
jmiller322 0:52b06cb96c86 152 dutyLeft = dutyRight;
jmiller322 0:52b06cb96c86 153 }
jmiller322 0:52b06cb96c86 154 if(dutyLeft < dutyRight) {
jmiller322 0:52b06cb96c86 155 dutyRight = dutyLeft;
jmiller322 0:52b06cb96c86 156 }
jmiller322 0:52b06cb96c86 157 dutyRight = 0.18;
jmiller322 0:52b06cb96c86 158 dutyLeft = 0.18;
jmiller322 0:52b06cb96c86 159 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 160 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 161 pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 162 }
jmiller322 0:52b06cb96c86 163
jmiller322 0:52b06cb96c86 164 void slowDown()
jmiller322 0:52b06cb96c86 165 {
jmiller322 0:52b06cb96c86 166 if(dutyRight >=0.130) {
jmiller322 0:52b06cb96c86 167 dutyRight -= 0.001;
jmiller322 0:52b06cb96c86 168 }
jmiller322 0:52b06cb96c86 169 if(dutyLeft >=0.130) {
jmiller322 0:52b06cb96c86 170 dutyLeft -= 0.001;
jmiller322 0:52b06cb96c86 171 }
jmiller322 0:52b06cb96c86 172 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 173 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 174 pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 175 }
jmiller322 0:52b06cb96c86 176
jmiller322 0:52b06cb96c86 177 void stop()
jmiller322 0:52b06cb96c86 178 {
jmiller322 0:52b06cb96c86 179 dutyLeft = dutyRight = 0.13;
jmiller322 0:52b06cb96c86 180 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 181 PWM1.write(dutyLeft); //stop motor
jmiller322 0:52b06cb96c86 182 pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 183 }
jmiller322 0:52b06cb96c86 184
jmiller322 0:52b06cb96c86 185 void steady()
jmiller322 0:52b06cb96c86 186 {
jmiller322 0:52b06cb96c86 187 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 188 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 189 }
jmiller322 0:52b06cb96c86 190
jmiller322 0:52b06cb96c86 191 void startupESC()
jmiller322 0:52b06cb96c86 192 {
jmiller322 0:52b06cb96c86 193 PWM.write(0.01);
jmiller322 0:52b06cb96c86 194 PWM1.write(0.01);
jmiller322 0:52b06cb96c86 195 wait(0.5);
jmiller322 0:52b06cb96c86 196 PWM.write(1.0);
jmiller322 0:52b06cb96c86 197 PWM1.write(1.0);
jmiller322 0:52b06cb96c86 198 wait(8);
jmiller322 0:52b06cb96c86 199 PWM.write(0.01);
jmiller322 0:52b06cb96c86 200 PWM1.write(0.01);
jmiller322 0:52b06cb96c86 201 wait(8);
jmiller322 0:52b06cb96c86 202 }
jmiller322 0:52b06cb96c86 203
jmiller322 0:52b06cb96c86 204 void maintainHeading(float desiredHeading){
jmiller322 0:52b06cb96c86 205 while(!IMU.tempAvailable());
jmiller322 0:52b06cb96c86 206 IMU.readTemp();
jmiller322 0:52b06cb96c86 207 while(!IMU.magAvailable(X_AXIS));
jmiller322 0:52b06cb96c86 208 IMU.readMag();
jmiller322 0:52b06cb96c86 209 while(!IMU.accelAvailable());
jmiller322 0:52b06cb96c86 210 IMU.readAccel();
jmiller322 0:52b06cb96c86 211 while(!IMU.gyroAvailable());
jmiller322 0:52b06cb96c86 212 IMU.readGyro();
jmiller322 0:52b06cb96c86 213 currentDegreeHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jmiller322 0:52b06cb96c86 214 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jmiller322 0:52b06cb96c86 215 pc.printf("heading: %f", currentDegreeHeading);
jmiller322 0:52b06cb96c86 216 currentDegreeHeading = fmod(((double)currentDegreeHeading-desiredHeading),360.0);
jmiller322 0:52b06cb96c86 217 if(currentDegreeHeading<0.0){currentDegreeHeading=360.0-fabs(currentDegreeHeading);}
jmiller322 0:52b06cb96c86 218 pc.printf(" heading: %f\n", currentDegreeHeading);
jmiller322 0:52b06cb96c86 219
jmiller322 0:52b06cb96c86 220 if(currentDegreeHeading < 180.0 && currentDegreeHeading > 10.0) {
jmiller322 0:52b06cb96c86 221 turnRight();
jmiller322 0:52b06cb96c86 222 } else if(currentDegreeHeading > 180.0 && currentDegreeHeading < 350.0) {
jmiller322 0:52b06cb96c86 223 turnLeft();
jmiller322 0:52b06cb96c86 224 } else {
jmiller322 0:52b06cb96c86 225 goForward();
jmiller322 0:52b06cb96c86 226 }
jmiller322 0:52b06cb96c86 227 }
jmiller322 0:52b06cb96c86 228
jmiller322 1:bae4f01ae25c 229 void serialRx(){
jmiller322 1:bae4f01ae25c 230 char inp = pc.getc();
jmiller322 1:bae4f01ae25c 231 ///// GO FORWARD /////
jmiller322 1:bae4f01ae25c 232 if(inp == 'w') {
jmiller322 1:bae4f01ae25c 233 //goForward();
jmiller322 1:bae4f01ae25c 234 desHeading += 15.0;
jmiller322 1:bae4f01ae25c 235 }
jmiller322 1:bae4f01ae25c 236 ///// SLOW DOWN /////
jmiller322 1:bae4f01ae25c 237 else if(inp == 's') {
jmiller322 1:bae4f01ae25c 238 //slowDown();
jmiller322 1:bae4f01ae25c 239 desHeading -= 15.0;
jmiller322 1:bae4f01ae25c 240 }
jmiller322 1:bae4f01ae25c 241 desHeading = fmod(desHeading, 360);
jmiller322 1:bae4f01ae25c 242 }
jmiller322 1:bae4f01ae25c 243
Joe5181 2:2a4e77f78c8a 244 void init_motors(){
jmiller322 0:52b06cb96c86 245 PWM.period(0.001);
jmiller322 0:52b06cb96c86 246 PWM1.period(0.001);
jmiller322 0:52b06cb96c86 247 startupESC();
jmiller322 0:52b06cb96c86 248 pc.printf("finished ecs startup");
Joe5181 2:2a4e77f78c8a 249 }
Joe5181 2:2a4e77f78c8a 250 void overall_init(){
Joe5181 2:2a4e77f78c8a 251 init_motors();
Joe5181 2:2a4e77f78c8a 252 calibrateIMU();
Joe5181 2:2a4e77f78c8a 253 printf("finished imu cal\n");
Joe5181 2:2a4e77f78c8a 254 desHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
Joe5181 2:2a4e77f78c8a 255 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
Joe5181 2:2a4e77f78c8a 256 }
Joe5181 2:2a4e77f78c8a 257
Joe5181 2:2a4e77f78c8a 258 void take_input(){
Joe5181 2:2a4e77f78c8a 259 char inp = pc.getc();
jmiller322 0:52b06cb96c86 260 ///// GO FORWARD /////
jmiller322 0:52b06cb96c86 261 if(inp == 'w' && dutyRight <= 0.230 && dutyLeft <= 0.230) {
jmiller322 0:52b06cb96c86 262 goForward();
jmiller322 0:52b06cb96c86 263 }
jmiller322 0:52b06cb96c86 264 ///// SLOW DOWN /////
jmiller322 0:52b06cb96c86 265 else if(inp == 's') {
jmiller322 0:52b06cb96c86 266 slowDown();
jmiller322 0:52b06cb96c86 267 }
jmiller322 0:52b06cb96c86 268 else if(inp == 't') {
jmiller322 0:52b06cb96c86 269 readToF();
jmiller322 0:52b06cb96c86 270 }
jmiller322 0:52b06cb96c86 271 else if(inp == 'i') {
jmiller322 0:52b06cb96c86 272 //readIMU();
jmiller322 0:52b06cb96c86 273 }
jmiller322 0:52b06cb96c86 274 ///// TURN LEFT /////
jmiller322 0:52b06cb96c86 275 else if(inp == 'a' && dutyRight <= 0.230) {
jmiller322 0:52b06cb96c86 276 turnLeft();
jmiller322 0:52b06cb96c86 277 }
jmiller322 0:52b06cb96c86 278 ///// TURN RIGHT /////
jmiller322 0:52b06cb96c86 279 else if(inp == 'd' && dutyLeft <= 0.230) {
jmiller322 0:52b06cb96c86 280 turnRight();
jmiller322 0:52b06cb96c86 281 }
jmiller322 0:52b06cb96c86 282 ///// STOP /////
jmiller322 0:52b06cb96c86 283 else if(inp == 'x') {
jmiller322 0:52b06cb96c86 284 stop();
jmiller322 0:52b06cb96c86 285 }
jmiller322 0:52b06cb96c86 286
jmiller322 0:52b06cb96c86 287 ///// STEADY PACE /////
Joe5181 2:2a4e77f78c8a 288 else {
jmiller322 0:52b06cb96c86 289
Joe5181 2:2a4e77f78c8a 290 steady();
Joe5181 2:2a4e77f78c8a 291 }
jmiller322 0:52b06cb96c86 292 }
Joe5181 2:2a4e77f78c8a 293
Joe5181 2:2a4e77f78c8a 294 int main(){
Joe5181 2:2a4e77f78c8a 295 //overall_init();
Joe5181 2:2a4e77f78c8a 296 initToF();
Joe5181 2:2a4e77f78c8a 297 calibrateIMU();
Joe5181 2:2a4e77f78c8a 298 while(1) {
Joe5181 2:2a4e77f78c8a 299 readToF();
Joe5181 2:2a4e77f78c8a 300 maintainHeading(desHeading);
Joe5181 2:2a4e77f78c8a 301 //take_input();
Joe5181 2:2a4e77f78c8a 302 }
Joe5181 2:2a4e77f78c8a 303 }
Joe5181 2:2a4e77f78c8a 304
Joe5181 2:2a4e77f78c8a 305 int backup_main(){
Joe5181 2:2a4e77f78c8a 306
Joe5181 2:2a4e77f78c8a 307 }