Yee Aung / Mbed 2 deprecated 4180_FINAL_PROJECT

Route Fix

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@10:2a1c8ce9d76c, 2018-12-05 (annotated)

Committer:
Arkantos1695
Date:
Wed Dec 05 03:59:52 2018 +0000
Revision:
10:2a1c8ce9d76c
Parent:
9:ec0ceec8f5f5 
Most recent

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cbrice6 0:9e014841f2b7 1 #include "mbed.h"
cbrice6 0:9e014841f2b7 2 #include "LSM9DS1.h" // IMU
cbrice6 0:9e014841f2b7 3 #include "ultrasonic.h" // Ultrasonic Sensor
cbrice6 0:9e014841f2b7 4 #include "Motor.h" // Motor Drivers
cbrice6 3:c07ea8bf242e 5 #include "PinDetect.h" // Pin Detect (for switch)
cbrice6 0:9e014841f2b7 6
cbrice6 0:9e014841f2b7 7
cbrice6 1:92f6242c0196 8 // Global Variables:
cbrice6 3:c07ea8bf242e 9 bool tr = true;
cbrice6 1:92f6242c0196 10 bool turnflag = false;
Arkantos1695 9:ec0ceec8f5f5 11 int leftdistancestarting= 0;
Arkantos1695 9:ec0ceec8f5f5 12 int leftdistanceending= 0;
cbrice6 0:9e014841f2b7 13 //---------------------|
cbrice6 0:9e014841f2b7 14 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 15 //---------------------|
cbrice6 3:c07ea8bf242e 16 // Debug LED pin:
cbrice6 3:c07ea8bf242e 17 //DigitalOut led(p25);
cbrice6 6:d25157f50f14 18 DigitalIn sw(p20);
cbrice6 6:d25157f50f14 19 Serial pc(USBTX,USBRX);
cbrice6 0:9e014841f2b7 20 // Setup Motor Driver pins:
cbrice6 0:9e014841f2b7 21 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5...
cbrice6 1:92f6242c0196 22 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7...
cbrice6 1:92f6242c0196 23 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 24 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
Arkantos1695 10:2a1c8ce9d76c 25 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
cbrice6 0:9e014841f2b7 26
cbrice6 0:9e014841f2b7 27 Timer t;
cbrice6 0:9e014841f2b7 28
cbrice6 0:9e014841f2b7 29 //-----------------------|
cbrice6 0:9e014841f2b7 30 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 31 //-----------------------|
cbrice6 0:9e014841f2b7 32 class mctrl {
cbrice6 0:9e014841f2b7 33 public:
cbrice6 1:92f6242c0196 34 // Stop all motors:
cbrice6 1:92f6242c0196 35 void stop(void) {
cbrice6 1:92f6242c0196 36 Lfront.speed(0);
cbrice6 1:92f6242c0196 37 Lback.speed(0);
cbrice6 1:92f6242c0196 38 Rfront.speed(0);
cbrice6 1:92f6242c0196 39 Rback.speed(0);
Arkantos1695 7:5667ce16d526 40 wait(0.55);
cbrice6 1:92f6242c0196 41 };
cbrice6 3:c07ea8bf242e 42
cbrice6 1:92f6242c0196 43 // Go forward at constant speed:
cbrice6 1:92f6242c0196 44 void fwd(void){
Arkantos1695 7:5667ce16d526 45 //stop();
cbrice6 1:92f6242c0196 46
cbrice6 1:92f6242c0196 47 Lfront.speed(1);
cbrice6 1:92f6242c0196 48 Lback.speed(1);
cbrice6 1:92f6242c0196 49 Rfront.speed(1);
cbrice6 1:92f6242c0196 50 Rback.speed(1);
cbrice6 1:92f6242c0196 51 wait(0.02);
cbrice6 1:92f6242c0196 52 };
cbrice6 3:c07ea8bf242e 53
cbrice6 1:92f6242c0196 54 // Reverse at constant speed:
cbrice6 1:92f6242c0196 55 void rev(void){
cbrice6 1:92f6242c0196 56 stop();
cbrice6 1:92f6242c0196 57 Lfront.speed(-1);
cbrice6 1:92f6242c0196 58 Lback.speed(-1);
cbrice6 1:92f6242c0196 59 Rfront.speed(-1);
cbrice6 1:92f6242c0196 60 Rback.speed(-1);
cbrice6 1:92f6242c0196 61 wait(0.02);
cbrice6 1:92f6242c0196 62 };
cbrice6 3:c07ea8bf242e 63
cbrice6 1:92f6242c0196 64 // Turn left 90 degrees:
cbrice6 4:6546c17ac0a6 65 void turnLeft(float currt){
cbrice6 1:92f6242c0196 66 stop();
Arkantos1695 10:2a1c8ce9d76c 67 while ((t.read()-currt) < 1.53) {
cbrice6 1:92f6242c0196 68 Lfront.speed(-1);
cbrice6 1:92f6242c0196 69 Lback.speed(-1);
cbrice6 1:92f6242c0196 70 Rfront.speed(1);
cbrice6 1:92f6242c0196 71 Rback.speed(1);
cbrice6 1:92f6242c0196 72 }
cbrice6 1:92f6242c0196 73 stop();
Arkantos1695 7:5667ce16d526 74 //wait(0.02);
cbrice6 1:92f6242c0196 75 };
cbrice6 3:c07ea8bf242e 76
cbrice6 1:92f6242c0196 77 // Turn right 90 degrees:
Arkantos1695 10:2a1c8ce9d76c 78 void turnRight(){
cbrice6 1:92f6242c0196 79 stop();
Arkantos1695 10:2a1c8ce9d76c 80 float degree = 0.0, angularV = 0.0;
Arkantos1695 10:2a1c8ce9d76c 81 float currt = t.read();
Arkantos1695 10:2a1c8ce9d76c 82 /*while ((t.read()-currt) < 1.56) {
cbrice6 1:92f6242c0196 83 Lfront.speed(1);
cbrice6 1:92f6242c0196 84 Lback.speed(1);
cbrice6 1:92f6242c0196 85 Rfront.speed(-1);
cbrice6 1:92f6242c0196 86 Rback.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 87 }*/
Arkantos1695 10:2a1c8ce9d76c 88 while(degree < 90) {
Arkantos1695 10:2a1c8ce9d76c 89 while(imu.gyroAvailable());
Arkantos1695 10:2a1c8ce9d76c 90 imu.readGyro();
Arkantos1695 10:2a1c8ce9d76c 91 angularV = imu.gz;
Arkantos1695 10:2a1c8ce9d76c 92 Lfront.speed(1);
Arkantos1695 10:2a1c8ce9d76c 93 Lback.speed(1);
Arkantos1695 10:2a1c8ce9d76c 94 Rfront.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 95 Rback.speed(-1);
Arkantos1695 10:2a1c8ce9d76c 96 if(angularV > 50.0 || angularV <-50.0) {
Arkantos1695 10:2a1c8ce9d76c 97 degree += (abs(angularV))/100.00;
Arkantos1695 10:2a1c8ce9d76c 98 }
Arkantos1695 10:2a1c8ce9d76c 99 wait(.4);
cbrice6 1:92f6242c0196 100 }
cbrice6 1:92f6242c0196 101 stop();
Arkantos1695 10:2a1c8ce9d76c 102 wait(0.02);
cbrice6 1:92f6242c0196 103 };
Arkantos1695 9:ec0ceec8f5f5 104
Arkantos1695 9:ec0ceec8f5f5 105
Arkantos1695 9:ec0ceec8f5f5 106
Arkantos1695 9:ec0ceec8f5f5 107
cbrice6 1:92f6242c0196 108 } mctrl;
cbrice6 0:9e014841f2b7 109
cbrice6 0:9e014841f2b7 110 //------------------|
cbrice6 0:9e014841f2b7 111 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 112 //------------------|
cbrice6 0:9e014841f2b7 113 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 114 {
cbrice6 3:c07ea8bf242e 115 if (distance < 150) {
Arkantos1695 9:ec0ceec8f5f5 116
Arkantos1695 9:ec0ceec8f5f5 117 mctrl.stop();
cbrice6 3:c07ea8bf242e 118 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 5:85951362fd6d 119 // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 5:85951362fd6d 120 // 2) Go forward 1.5 seconds.
cbrice6 5:85951362fd6d 121 // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 3:c07ea8bf242e 122 // 4) Continue forward until wall.
Arkantos1695 9:ec0ceec8f5f5 123
Arkantos1695 9:ec0ceec8f5f5 124
cbrice6 3:c07ea8bf242e 125 // [Step 1]
Arkantos1695 10:2a1c8ce9d76c 126 float currt = t.read();
Arkantos1695 10:2a1c8ce9d76c 127 if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 128 // [Step 2]
cbrice6 3:c07ea8bf242e 129 currt = t.read();
Arkantos1695 9:ec0ceec8f5f5 130 while ((t.read()-currt) < 1) {mctrl.fwd();}
cbrice6 3:c07ea8bf242e 131 // [Step 3]
cbrice6 3:c07ea8bf242e 132 currt = t.read();
Arkantos1695 10:2a1c8ce9d76c 133 if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 134 // [End]
Arkantos1695 10:2a1c8ce9d76c 135 tr = !tr;
cbrice6 1:92f6242c0196 136 }
cbrice6 1:92f6242c0196 137 else {
cbrice6 1:92f6242c0196 138 mctrl.fwd();
Arkantos1695 9:ec0ceec8f5f5 139
cbrice6 0:9e014841f2b7 140 }
cbrice6 0:9e014841f2b7 141 }
cbrice6 0:9e014841f2b7 142
Arkantos1695 8:2d0fc244cc65 143 void dist2(int distance) // left sensor interrupt
Arkantos1695 8:2d0fc244cc65 144 {
Arkantos1695 9:ec0ceec8f5f5 145
Arkantos1695 9:ec0ceec8f5f5 146 leftdistanceending = distance;
Arkantos1695 9:ec0ceec8f5f5 147 //pc.printf(" Distance %d mm\r\n", distance);
Arkantos1695 9:ec0ceec8f5f5 148
Arkantos1695 9:ec0ceec8f5f5 149
Arkantos1695 8:2d0fc244cc65 150 }
Arkantos1695 8:2d0fc244cc65 151
Arkantos1695 8:2d0fc244cc65 152 void dist3(int distance) // right sensor interrupt
Arkantos1695 8:2d0fc244cc65 153 {
Arkantos1695 8:2d0fc244cc65 154 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 155 pc.printf("Right Sensor trigg");
Arkantos1695 8:2d0fc244cc65 156 }
Arkantos1695 8:2d0fc244cc65 157 else {
Arkantos1695 8:2d0fc244cc65 158 pc.printf("Right Sensor echo");
Arkantos1695 8:2d0fc244cc65 159 }*/
Arkantos1695 9:ec0ceec8f5f5 160 //rightdistance = distance;
Arkantos1695 8:2d0fc244cc65 161 }
Arkantos1695 8:2d0fc244cc65 162
Arkantos1695 8:2d0fc244cc65 163
cbrice6 0:9e014841f2b7 164 //------------------------------|
cbrice6 0:9e014841f2b7 165 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 166 //------------------------------|
cbrice6 0:9e014841f2b7 167 // Setup Ultrasonic Sensor pins:
Arkantos1695 10:2a1c8ce9d76c 168 ultrasonic usensor(p15, p16, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 169 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 170 // call "dist" when the distance changes...
Arkantos1695 10:2a1c8ce9d76c 171 ultrasonic usensor2(p15, p26, .07, 1, &dist2); // left sensor trigger
Arkantos1695 10:2a1c8ce9d76c 172 ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
Arkantos1695 10:2a1c8ce9d76c 173
cbrice6 0:9e014841f2b7 174
cbrice6 0:9e014841f2b7 175 //---------------|
cbrice6 0:9e014841f2b7 176 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 177 //---------------|
cbrice6 0:9e014841f2b7 178 int main() {
cbrice6 3:c07ea8bf242e 179 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 180 sw.mode(PullUp);
cbrice6 0:9e014841f2b7 181 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 182 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 183 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 184 usensor3.startUpdates();
Arkantos1695 9:ec0ceec8f5f5 185 wait(0.5);
Arkantos1695 9:ec0ceec8f5f5 186 int leftdistancestarting = usensor2.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 187 wait(0.5);
Arkantos1695 9:ec0ceec8f5f5 188 pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting);
Arkantos1695 9:ec0ceec8f5f5 189
cbrice6 0:9e014841f2b7 190 // Initialize the Timer:
cbrice6 0:9e014841f2b7 191 t.start();
Arkantos1695 9:ec0ceec8f5f5 192
Arkantos1695 9:ec0ceec8f5f5 193 while(1) {
Arkantos1695 9:ec0ceec8f5f5 194 while (!sw)
Arkantos1695 9:ec0ceec8f5f5 195 {
cbrice6 0:9e014841f2b7 196
cbrice6 0:9e014841f2b7 197 usensor.checkDistance();
Arkantos1695 8:2d0fc244cc65 198 usensor2.checkDistance();
Arkantos1695 8:2d0fc244cc65 199 usensor3.checkDistance();
Arkantos1695 9:ec0ceec8f5f5 200 pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
Arkantos1695 9:ec0ceec8f5f5 201
Arkantos1695 9:ec0ceec8f5f5 202 }
Arkantos1695 9:ec0ceec8f5f5 203 mctrl.stop();
Arkantos1695 9:ec0ceec8f5f5 204
cbrice6 0:9e014841f2b7 205 }
cbrice6 0:9e014841f2b7 206 }
cbrice6 3:c07ea8bf242e 207