Yee Aung / Mbed 2 deprecated 4180_FINAL_PROJECT

Route Fix

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@7:5667ce16d526, 2018-11-25 (annotated)

Committer:
Arkantos1695
Date:
Sun Nov 25 20:09:50 2018 +0000
Revision:
7:5667ce16d526
Parent:
6:d25157f50f14
Child:
8:2d0fc244cc65
route progress

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;
cbrice6 1:92f6242c0196 11
cbrice6 0:9e014841f2b7 12 //---------------------|
cbrice6 0:9e014841f2b7 13 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 14 //---------------------|
cbrice6 3:c07ea8bf242e 15 // Debug LED pin:
cbrice6 3:c07ea8bf242e 16 //DigitalOut led(p25);
cbrice6 6:d25157f50f14 17 DigitalIn sw(p20);
cbrice6 6:d25157f50f14 18 Serial pc(USBTX,USBRX);
cbrice6 0:9e014841f2b7 19 // Setup Motor Driver pins:
cbrice6 0:9e014841f2b7 20 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5...
cbrice6 1:92f6242c0196 21 Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7...
cbrice6 1:92f6242c0196 22 Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 23 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
cbrice6 0:9e014841f2b7 24
cbrice6 0:9e014841f2b7 25 Timer t;
cbrice6 0:9e014841f2b7 26
cbrice6 0:9e014841f2b7 27 //-----------------------|
cbrice6 0:9e014841f2b7 28 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 29 //-----------------------|
cbrice6 0:9e014841f2b7 30 class mctrl {
cbrice6 0:9e014841f2b7 31 public:
cbrice6 1:92f6242c0196 32 // Stop all motors:
cbrice6 1:92f6242c0196 33 void stop(void) {
cbrice6 1:92f6242c0196 34 Lfront.speed(0);
cbrice6 1:92f6242c0196 35 Lback.speed(0);
cbrice6 1:92f6242c0196 36 Rfront.speed(0);
cbrice6 1:92f6242c0196 37 Rback.speed(0);
Arkantos1695 7:5667ce16d526 38 wait(0.55);
cbrice6 1:92f6242c0196 39 };
cbrice6 3:c07ea8bf242e 40
cbrice6 1:92f6242c0196 41 // Go forward at constant speed:
cbrice6 1:92f6242c0196 42 void fwd(void){
Arkantos1695 7:5667ce16d526 43 //stop();
cbrice6 1:92f6242c0196 44
cbrice6 1:92f6242c0196 45 Lfront.speed(1);
cbrice6 1:92f6242c0196 46 Lback.speed(1);
cbrice6 1:92f6242c0196 47 Rfront.speed(1);
cbrice6 1:92f6242c0196 48 Rback.speed(1);
cbrice6 1:92f6242c0196 49 wait(0.02);
cbrice6 1:92f6242c0196 50 };
cbrice6 3:c07ea8bf242e 51
cbrice6 1:92f6242c0196 52 // Reverse at constant speed:
cbrice6 1:92f6242c0196 53 void rev(void){
cbrice6 1:92f6242c0196 54 stop();
cbrice6 1:92f6242c0196 55 Lfront.speed(-1);
cbrice6 1:92f6242c0196 56 Lback.speed(-1);
cbrice6 1:92f6242c0196 57 Rfront.speed(-1);
cbrice6 1:92f6242c0196 58 Rback.speed(-1);
cbrice6 1:92f6242c0196 59 wait(0.02);
cbrice6 1:92f6242c0196 60 };
cbrice6 3:c07ea8bf242e 61
cbrice6 1:92f6242c0196 62 // Turn left 90 degrees:
cbrice6 4:6546c17ac0a6 63 void turnLeft(float currt){
cbrice6 1:92f6242c0196 64 stop();
Arkantos1695 7:5667ce16d526 65 while ((t.read()-currt) < 1.30) {
cbrice6 1:92f6242c0196 66 Lfront.speed(-1);
cbrice6 1:92f6242c0196 67 Lback.speed(-1);
cbrice6 1:92f6242c0196 68 Rfront.speed(1);
cbrice6 1:92f6242c0196 69 Rback.speed(1);
cbrice6 1:92f6242c0196 70 }
cbrice6 1:92f6242c0196 71 stop();
Arkantos1695 7:5667ce16d526 72 //wait(0.02);
cbrice6 1:92f6242c0196 73 };
cbrice6 3:c07ea8bf242e 74
cbrice6 1:92f6242c0196 75 // Turn right 90 degrees:
cbrice6 4:6546c17ac0a6 76 void turnRight(float currt){
cbrice6 1:92f6242c0196 77 stop();
Arkantos1695 7:5667ce16d526 78 while ((t.read()-currt) < 1.30) {
cbrice6 1:92f6242c0196 79 Lfront.speed(1);
cbrice6 1:92f6242c0196 80 Lback.speed(1);
cbrice6 1:92f6242c0196 81 Rfront.speed(-1);
cbrice6 1:92f6242c0196 82 Rback.speed(-1);
cbrice6 1:92f6242c0196 83 }
cbrice6 1:92f6242c0196 84 stop();
Arkantos1695 7:5667ce16d526 85 //wait(0.02);
cbrice6 1:92f6242c0196 86 };
cbrice6 1:92f6242c0196 87 } mctrl;
cbrice6 0:9e014841f2b7 88
cbrice6 0:9e014841f2b7 89 //------------------|
cbrice6 0:9e014841f2b7 90 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 91 //------------------|
cbrice6 0:9e014841f2b7 92 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 93 {
cbrice6 3:c07ea8bf242e 94 if (distance < 150) {
cbrice6 3:c07ea8bf242e 95 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 5:85951362fd6d 96 // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 5:85951362fd6d 97 // 2) Go forward 1.5 seconds.
cbrice6 5:85951362fd6d 98 // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 3:c07ea8bf242e 99 // 4) Continue forward until wall.
cbrice6 3:c07ea8bf242e 100 // [Step 1]
cbrice6 1:92f6242c0196 101 float currt = t.read();
cbrice6 4:6546c17ac0a6 102 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 103 // [Step 2]
cbrice6 3:c07ea8bf242e 104 currt = t.read();
Arkantos1695 7:5667ce16d526 105 while ((t.read()-currt) < .5) {mctrl.fwd();}
cbrice6 3:c07ea8bf242e 106 // [Step 3]
cbrice6 3:c07ea8bf242e 107 currt = t.read();
cbrice6 4:6546c17ac0a6 108 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 109 // [End]
cbrice6 3:c07ea8bf242e 110 tr = !tr;
cbrice6 1:92f6242c0196 111 }
cbrice6 1:92f6242c0196 112 else {
cbrice6 1:92f6242c0196 113 mctrl.fwd();
cbrice6 0:9e014841f2b7 114 }
cbrice6 0:9e014841f2b7 115 }
cbrice6 0:9e014841f2b7 116
cbrice6 0:9e014841f2b7 117 //------------------------------|
cbrice6 0:9e014841f2b7 118 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 119 //------------------------------|
cbrice6 0:9e014841f2b7 120 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 121 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
cbrice6 0:9e014841f2b7 122 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 123 // call "dist" when the distance changes...
cbrice6 0:9e014841f2b7 124
cbrice6 0:9e014841f2b7 125 //---------------|
cbrice6 0:9e014841f2b7 126 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 127 //---------------|
cbrice6 0:9e014841f2b7 128 int main() {
cbrice6 3:c07ea8bf242e 129 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 130 sw.mode(PullUp);
cbrice6 0:9e014841f2b7 131 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 132 usensor.startUpdates();
cbrice6 0:9e014841f2b7 133
cbrice6 0:9e014841f2b7 134 // Initialize the Timer:
cbrice6 0:9e014841f2b7 135 t.start();
cbrice6 6:d25157f50f14 136 while(!sw) {
cbrice6 0:9e014841f2b7 137
cbrice6 0:9e014841f2b7 138 usensor.checkDistance();
cbrice6 0:9e014841f2b7 139
cbrice6 0:9e014841f2b7 140 wait(0.1);
cbrice6 0:9e014841f2b7 141 }
cbrice6 0:9e014841f2b7 142 }
cbrice6 3:c07ea8bf242e 143