Yee Aung / Mbed 2 deprecated 4180_FINAL_PROJECT

Route Fix

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@9:ec0ceec8f5f5, 2018-12-03 (annotated)

Committer:
Arkantos1695
Date:
Mon Dec 03 17:01:09 2018 +0000
Revision:
9:ec0ceec8f5f5
Parent:
8:2d0fc244cc65
Child:
10:2a1c8ce9d76c
updated one;

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...
cbrice6 0:9e014841f2b7 25
cbrice6 0:9e014841f2b7 26 Timer t;
cbrice6 0:9e014841f2b7 27
cbrice6 0:9e014841f2b7 28 //-----------------------|
cbrice6 0:9e014841f2b7 29 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 30 //-----------------------|
cbrice6 0:9e014841f2b7 31 class mctrl {
cbrice6 0:9e014841f2b7 32 public:
cbrice6 1:92f6242c0196 33 // Stop all motors:
cbrice6 1:92f6242c0196 34 void stop(void) {
cbrice6 1:92f6242c0196 35 Lfront.speed(0);
cbrice6 1:92f6242c0196 36 Lback.speed(0);
cbrice6 1:92f6242c0196 37 Rfront.speed(0);
cbrice6 1:92f6242c0196 38 Rback.speed(0);
Arkantos1695 7:5667ce16d526 39 wait(0.55);
cbrice6 1:92f6242c0196 40 };
cbrice6 3:c07ea8bf242e 41
cbrice6 1:92f6242c0196 42 // Go forward at constant speed:
cbrice6 1:92f6242c0196 43 void fwd(void){
Arkantos1695 7:5667ce16d526 44 //stop();
cbrice6 1:92f6242c0196 45
cbrice6 1:92f6242c0196 46 Lfront.speed(1);
cbrice6 1:92f6242c0196 47 Lback.speed(1);
cbrice6 1:92f6242c0196 48 Rfront.speed(1);
cbrice6 1:92f6242c0196 49 Rback.speed(1);
cbrice6 1:92f6242c0196 50 wait(0.02);
cbrice6 1:92f6242c0196 51 };
cbrice6 3:c07ea8bf242e 52
cbrice6 1:92f6242c0196 53 // Reverse at constant speed:
cbrice6 1:92f6242c0196 54 void rev(void){
cbrice6 1:92f6242c0196 55 stop();
cbrice6 1:92f6242c0196 56 Lfront.speed(-1);
cbrice6 1:92f6242c0196 57 Lback.speed(-1);
cbrice6 1:92f6242c0196 58 Rfront.speed(-1);
cbrice6 1:92f6242c0196 59 Rback.speed(-1);
cbrice6 1:92f6242c0196 60 wait(0.02);
cbrice6 1:92f6242c0196 61 };
cbrice6 3:c07ea8bf242e 62
cbrice6 1:92f6242c0196 63 // Turn left 90 degrees:
cbrice6 4:6546c17ac0a6 64 void turnLeft(float currt){
cbrice6 1:92f6242c0196 65 stop();
Arkantos1695 7:5667ce16d526 66 while ((t.read()-currt) < 1.30) {
cbrice6 1:92f6242c0196 67 Lfront.speed(-1);
cbrice6 1:92f6242c0196 68 Lback.speed(-1);
cbrice6 1:92f6242c0196 69 Rfront.speed(1);
cbrice6 1:92f6242c0196 70 Rback.speed(1);
cbrice6 1:92f6242c0196 71 }
cbrice6 1:92f6242c0196 72 stop();
Arkantos1695 7:5667ce16d526 73 //wait(0.02);
cbrice6 1:92f6242c0196 74 };
cbrice6 3:c07ea8bf242e 75
cbrice6 1:92f6242c0196 76 // Turn right 90 degrees:
cbrice6 4:6546c17ac0a6 77 void turnRight(float currt){
cbrice6 1:92f6242c0196 78 stop();
Arkantos1695 7:5667ce16d526 79 while ((t.read()-currt) < 1.30) {
cbrice6 1:92f6242c0196 80 Lfront.speed(1);
cbrice6 1:92f6242c0196 81 Lback.speed(1);
cbrice6 1:92f6242c0196 82 Rfront.speed(-1);
cbrice6 1:92f6242c0196 83 Rback.speed(-1);
cbrice6 1:92f6242c0196 84 }
cbrice6 1:92f6242c0196 85 stop();
Arkantos1695 7:5667ce16d526 86 //wait(0.02);
cbrice6 1:92f6242c0196 87 };
Arkantos1695 9:ec0ceec8f5f5 88
Arkantos1695 9:ec0ceec8f5f5 89
Arkantos1695 9:ec0ceec8f5f5 90
Arkantos1695 9:ec0ceec8f5f5 91
cbrice6 1:92f6242c0196 92 } mctrl;
cbrice6 0:9e014841f2b7 93
cbrice6 0:9e014841f2b7 94 //------------------|
cbrice6 0:9e014841f2b7 95 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 96 //------------------|
cbrice6 0:9e014841f2b7 97 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 98 {
cbrice6 3:c07ea8bf242e 99 if (distance < 150) {
Arkantos1695 9:ec0ceec8f5f5 100
Arkantos1695 9:ec0ceec8f5f5 101 mctrl.stop();
cbrice6 3:c07ea8bf242e 102 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 5:85951362fd6d 103 // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 5:85951362fd6d 104 // 2) Go forward 1.5 seconds.
cbrice6 5:85951362fd6d 105 // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 3:c07ea8bf242e 106 // 4) Continue forward until wall.
Arkantos1695 9:ec0ceec8f5f5 107
Arkantos1695 9:ec0ceec8f5f5 108
cbrice6 3:c07ea8bf242e 109 // [Step 1]
Arkantos1695 9:ec0ceec8f5f5 110 /*float currt = t.read();
cbrice6 4:6546c17ac0a6 111 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 112 // [Step 2]
cbrice6 3:c07ea8bf242e 113 currt = t.read();
Arkantos1695 9:ec0ceec8f5f5 114 while ((t.read()-currt) < 1) {mctrl.fwd();}
cbrice6 3:c07ea8bf242e 115 // [Step 3]
cbrice6 3:c07ea8bf242e 116 currt = t.read();
cbrice6 4:6546c17ac0a6 117 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 118 // [End]
Arkantos1695 9:ec0ceec8f5f5 119 tr = !tr;*/
cbrice6 1:92f6242c0196 120 }
cbrice6 1:92f6242c0196 121 else {
cbrice6 1:92f6242c0196 122 mctrl.fwd();
Arkantos1695 9:ec0ceec8f5f5 123
cbrice6 0:9e014841f2b7 124 }
cbrice6 0:9e014841f2b7 125 }
cbrice6 0:9e014841f2b7 126
Arkantos1695 8:2d0fc244cc65 127 void dist2(int distance) // left sensor interrupt
Arkantos1695 8:2d0fc244cc65 128 {
Arkantos1695 9:ec0ceec8f5f5 129
Arkantos1695 9:ec0ceec8f5f5 130 leftdistanceending = distance;
Arkantos1695 9:ec0ceec8f5f5 131 //pc.printf(" Distance %d mm\r\n", distance);
Arkantos1695 9:ec0ceec8f5f5 132
Arkantos1695 9:ec0ceec8f5f5 133
Arkantos1695 8:2d0fc244cc65 134 }
Arkantos1695 8:2d0fc244cc65 135
Arkantos1695 8:2d0fc244cc65 136 void dist3(int distance) // right sensor interrupt
Arkantos1695 8:2d0fc244cc65 137 {
Arkantos1695 8:2d0fc244cc65 138 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 139 pc.printf("Right Sensor trigg");
Arkantos1695 8:2d0fc244cc65 140 }
Arkantos1695 8:2d0fc244cc65 141 else {
Arkantos1695 8:2d0fc244cc65 142 pc.printf("Right Sensor echo");
Arkantos1695 8:2d0fc244cc65 143 }*/
Arkantos1695 9:ec0ceec8f5f5 144 //rightdistance = distance;
Arkantos1695 8:2d0fc244cc65 145 }
Arkantos1695 8:2d0fc244cc65 146
Arkantos1695 8:2d0fc244cc65 147
cbrice6 0:9e014841f2b7 148 //------------------------------|
cbrice6 0:9e014841f2b7 149 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 150 //------------------------------|
cbrice6 0:9e014841f2b7 151 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 152 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 153 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 154 // call "dist" when the distance changes...
Arkantos1695 9:ec0ceec8f5f5 155 ultrasonic usensor2(p13, p26, .07, 1, &dist2); // left sensor trigger
Arkantos1695 9:ec0ceec8f5f5 156 ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right sensor trigger
cbrice6 0:9e014841f2b7 157
cbrice6 0:9e014841f2b7 158 //---------------|
cbrice6 0:9e014841f2b7 159 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 160 //---------------|
cbrice6 0:9e014841f2b7 161 int main() {
cbrice6 3:c07ea8bf242e 162 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 163 sw.mode(PullUp);
cbrice6 0:9e014841f2b7 164 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 165 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 166 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 167 usensor3.startUpdates();
Arkantos1695 9:ec0ceec8f5f5 168 wait(0.5);
Arkantos1695 9:ec0ceec8f5f5 169 int leftdistancestarting = usensor2.getCurrentDistance();
Arkantos1695 9:ec0ceec8f5f5 170 wait(0.5);
Arkantos1695 9:ec0ceec8f5f5 171 pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting);
Arkantos1695 9:ec0ceec8f5f5 172
cbrice6 0:9e014841f2b7 173 // Initialize the Timer:
cbrice6 0:9e014841f2b7 174 t.start();
Arkantos1695 9:ec0ceec8f5f5 175
Arkantos1695 9:ec0ceec8f5f5 176 while(1) {
Arkantos1695 9:ec0ceec8f5f5 177 while (!sw)
Arkantos1695 9:ec0ceec8f5f5 178 {
cbrice6 0:9e014841f2b7 179
cbrice6 0:9e014841f2b7 180 usensor.checkDistance();
Arkantos1695 8:2d0fc244cc65 181 usensor2.checkDistance();
Arkantos1695 8:2d0fc244cc65 182 usensor3.checkDistance();
Arkantos1695 9:ec0ceec8f5f5 183 pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);
Arkantos1695 9:ec0ceec8f5f5 184
Arkantos1695 9:ec0ceec8f5f5 185 }
Arkantos1695 9:ec0ceec8f5f5 186 mctrl.stop();
Arkantos1695 9:ec0ceec8f5f5 187
cbrice6 0:9e014841f2b7 188 }
cbrice6 0:9e014841f2b7 189 }
cbrice6 3:c07ea8bf242e 190