mohammad karim / Mbed 2 deprecated 4180_FINAL_PROJECTUricascode

something

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@8:2d0fc244cc65, 2018-11-28 (annotated)

Committer:
Arkantos1695
Date:
Wed Nov 28 18:28:04 2018 +0000
Revision:
8:2d0fc244cc65
Parent:
7:5667ce16d526
Child:
9:ec0ceec8f5f5
installed the side sensors

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 8:2d0fc244cc65 11 int leftdistance= 0;
Arkantos1695 8:2d0fc244cc65 12 int rightdistance = 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 };
cbrice6 1:92f6242c0196 88 } mctrl;
cbrice6 0:9e014841f2b7 89
cbrice6 0:9e014841f2b7 90 //------------------|
cbrice6 0:9e014841f2b7 91 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 92 //------------------|
cbrice6 0:9e014841f2b7 93 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 94 {
cbrice6 3:c07ea8bf242e 95 if (distance < 150) {
cbrice6 3:c07ea8bf242e 96 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 5:85951362fd6d 97 // 1) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 5:85951362fd6d 98 // 2) Go forward 1.5 seconds.
cbrice6 5:85951362fd6d 99 // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
cbrice6 3:c07ea8bf242e 100 // 4) Continue forward until wall.
cbrice6 3:c07ea8bf242e 101 // [Step 1]
cbrice6 1:92f6242c0196 102 float currt = t.read();
cbrice6 4:6546c17ac0a6 103 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 104 // [Step 2]
cbrice6 3:c07ea8bf242e 105 currt = t.read();
Arkantos1695 7:5667ce16d526 106 while ((t.read()-currt) < .5) {mctrl.fwd();}
cbrice6 3:c07ea8bf242e 107 // [Step 3]
cbrice6 3:c07ea8bf242e 108 currt = t.read();
cbrice6 4:6546c17ac0a6 109 if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
cbrice6 3:c07ea8bf242e 110 // [End]
cbrice6 3:c07ea8bf242e 111 tr = !tr;
cbrice6 1:92f6242c0196 112 }
cbrice6 1:92f6242c0196 113 else {
cbrice6 1:92f6242c0196 114 mctrl.fwd();
cbrice6 0:9e014841f2b7 115 }
cbrice6 0:9e014841f2b7 116 }
cbrice6 0:9e014841f2b7 117
Arkantos1695 8:2d0fc244cc65 118 void dist2(int distance) // left sensor interrupt
Arkantos1695 8:2d0fc244cc65 119 {
Arkantos1695 8:2d0fc244cc65 120 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 121 pc.printf("Left Sensor trigg");
Arkantos1695 8:2d0fc244cc65 122 }
Arkantos1695 8:2d0fc244cc65 123 else {
Arkantos1695 8:2d0fc244cc65 124 pc.printf("Left Sensor echo");
Arkantos1695 8:2d0fc244cc65 125 }*/
Arkantos1695 8:2d0fc244cc65 126 leftdistance = distance;
Arkantos1695 8:2d0fc244cc65 127 }
Arkantos1695 8:2d0fc244cc65 128
Arkantos1695 8:2d0fc244cc65 129 void dist3(int distance) // right sensor interrupt
Arkantos1695 8:2d0fc244cc65 130 {
Arkantos1695 8:2d0fc244cc65 131 /*if (distance < 150) {
Arkantos1695 8:2d0fc244cc65 132 pc.printf("Right Sensor trigg");
Arkantos1695 8:2d0fc244cc65 133 }
Arkantos1695 8:2d0fc244cc65 134 else {
Arkantos1695 8:2d0fc244cc65 135 pc.printf("Right Sensor echo");
Arkantos1695 8:2d0fc244cc65 136 }*/
Arkantos1695 8:2d0fc244cc65 137 rightdistance = distance;
Arkantos1695 8:2d0fc244cc65 138 }
Arkantos1695 8:2d0fc244cc65 139
Arkantos1695 8:2d0fc244cc65 140
cbrice6 0:9e014841f2b7 141 //------------------------------|
cbrice6 0:9e014841f2b7 142 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 143 //------------------------------|
cbrice6 0:9e014841f2b7 144 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 145 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
Arkantos1695 8:2d0fc244cc65 146 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 147 // call "dist" when the distance changes...
Arkantos1695 8:2d0fc244cc65 148 ultrasonic usensor2(p13, p26, .07, 1, &dist2); // left trigger
Arkantos1695 8:2d0fc244cc65 149 ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right trigger
cbrice6 0:9e014841f2b7 150
cbrice6 0:9e014841f2b7 151 //---------------|
cbrice6 0:9e014841f2b7 152 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 153 //---------------|
cbrice6 0:9e014841f2b7 154 int main() {
cbrice6 3:c07ea8bf242e 155 // Use internal pullup for the switch:
cbrice6 6:d25157f50f14 156 sw.mode(PullUp);
cbrice6 0:9e014841f2b7 157 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 158 usensor.startUpdates();
Arkantos1695 8:2d0fc244cc65 159 usensor2.startUpdates();
Arkantos1695 8:2d0fc244cc65 160 usensor3.startUpdates();
cbrice6 0:9e014841f2b7 161 // Initialize the Timer:
cbrice6 0:9e014841f2b7 162 t.start();
cbrice6 6:d25157f50f14 163 while(!sw) {
cbrice6 0:9e014841f2b7 164
cbrice6 0:9e014841f2b7 165 usensor.checkDistance();
Arkantos1695 8:2d0fc244cc65 166 usensor2.checkDistance();
Arkantos1695 8:2d0fc244cc65 167 usensor3.checkDistance();
Arkantos1695 8:2d0fc244cc65 168 pc.printf("Left Distance %d mm\r\n", leftdistance);
Arkantos1695 8:2d0fc244cc65 169 pc.printf("Right Distance %d mm\r\n", rightdistance);
Arkantos1695 8:2d0fc244cc65 170 wait(1);
cbrice6 0:9e014841f2b7 171 }
cbrice6 0:9e014841f2b7 172 }
cbrice6 3:c07ea8bf242e 173