Yee Aung / Mbed 2 deprecated 4180_FINAL_PROJECT

Route Fix

Dependencies:   mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library

main.cpp@0:9e014841f2b7, 2018-11-19 (annotated)

Committer:
cbrice6
Date:
Mon Nov 19 01:08:21 2018 +0000
Revision:
0:9e014841f2b7
Child:
1:92f6242c0196
First commit (template)

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 0:9e014841f2b7 5
cbrice6 0:9e014841f2b7 6
cbrice6 0:9e014841f2b7 7 //---------------------|
cbrice6 0:9e014841f2b7 8 // PIN INITIALIZATIONS |
cbrice6 0:9e014841f2b7 9 //---------------------|
cbrice6 0:9e014841f2b7 10 // Setup IMU pins:
cbrice6 0:9e014841f2b7 11 LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27..
cbrice6 0:9e014841f2b7 12
cbrice6 0:9e014841f2b7 13 // Setup Motor Driver pins:
cbrice6 0:9e014841f2b7 14 Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5...
cbrice6 0:9e014841f2b7 15 Motor Lback(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7...
cbrice6 0:9e014841f2b7 16 Motor Rfront(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9...
cbrice6 0:9e014841f2b7 17 Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11...
cbrice6 0:9e014841f2b7 18
cbrice6 0:9e014841f2b7 19 Timer t;
cbrice6 0:9e014841f2b7 20
cbrice6 0:9e014841f2b7 21 //-----------------------|
cbrice6 0:9e014841f2b7 22 // CLASS INITIALIZATIONS |
cbrice6 0:9e014841f2b7 23 //-----------------------|
cbrice6 0:9e014841f2b7 24 class mctrl {
cbrice6 0:9e014841f2b7 25 public:
cbrice6 0:9e014841f2b7 26 void stop(void);
cbrice6 0:9e014841f2b7 27 void fwd(float s);
cbrice6 0:9e014841f2b7 28 void rev(float s);
cbrice6 0:9e014841f2b7 29 void turnLeft(float s);
cbrice6 0:9e014841f2b7 30 void turnRight(float s);
cbrice6 0:9e014841f2b7 31 private:
cbrice6 0:9e014841f2b7 32 float tcurr;
cbrice6 0:9e014841f2b7 33 float turntime
cbrice6 0:9e014841f2b7 34 };
cbrice6 0:9e014841f2b7 35
cbrice6 0:9e014841f2b7 36 //------------------|
cbrice6 0:9e014841f2b7 37 // HELPER FUNCTIONS |
cbrice6 0:9e014841f2b7 38 //------------------|
cbrice6 0:9e014841f2b7 39 void mctrl::stop(void)
cbrice6 0:9e014841f2b7 40 {
cbrice6 0:9e014841f2b7 41 Lfront.speed(0);
cbrice6 0:9e014841f2b7 42 Lback.speed(0);
cbrice6 0:9e014841f2b7 43 Rfront.speed(0);
cbrice6 0:9e014841f2b7 44 Rback.speed(0);
cbrice6 0:9e014841f2b7 45 wait(0.02);
cbrice6 0:9e014841f2b7 46 }
cbrice6 0:9e014841f2b7 47
cbrice6 0:9e014841f2b7 48 void mctrl::fwd(float s)
cbrice6 0:9e014841f2b7 49 {
cbrice6 0:9e014841f2b7 50 mctrl.stop();
cbrice6 0:9e014841f2b7 51
cbrice6 0:9e014841f2b7 52 Lfront.speed(s);
cbrice6 0:9e014841f2b7 53 Lback.speed(s);
cbrice6 0:9e014841f2b7 54 Rfront.speed(s);
cbrice6 0:9e014841f2b7 55 Rback.speed(s);
cbrice6 0:9e014841f2b7 56 wait(0.02);
cbrice6 0:9e014841f2b7 57 }
cbrice6 0:9e014841f2b7 58
cbrice6 0:9e014841f2b7 59 void mctrl::rev(float s)
cbrice6 0:9e014841f2b7 60 {
cbrice6 0:9e014841f2b7 61 mctrl.stop();
cbrice6 0:9e014841f2b7 62
cbrice6 0:9e014841f2b7 63 Lfront.speed(-s);
cbrice6 0:9e014841f2b7 64 Lback.speed(-s);
cbrice6 0:9e014841f2b7 65 Rfront.speed(-s);
cbrice6 0:9e014841f2b7 66 Rback.speed(-s);
cbrice6 0:9e014841f2b7 67 wait(0.02);
cbrice6 0:9e014841f2b7 68 }
cbrice6 0:9e014841f2b7 69
cbrice6 0:9e014841f2b7 70 void mctrl::turnLeft(float s)
cbrice6 0:9e014841f2b7 71 {
cbrice6 0:9e014841f2b7 72 mctrl.stop();
cbrice6 0:9e014841f2b7 73
cbrice6 0:9e014841f2b7 74 tcurr = t.read();
cbrice6 0:9e014841f2b7 75 while ((t.read() - tcurr) < turntime) {
cbrice6 0:9e014841f2b7 76 Lfront.speed(-s);
cbrice6 0:9e014841f2b7 77 Lback.speed(-s);
cbrice6 0:9e014841f2b7 78 Rfront.speed(s);
cbrice6 0:9e014841f2b7 79 Rback.speed(s);
cbrice6 0:9e014841f2b7 80 }
cbrice6 0:9e014841f2b7 81
cbrice6 0:9e014841f2b7 82 mctrl.stop();
cbrice6 0:9e014841f2b7 83 wait(0.02);
cbrice6 0:9e014841f2b7 84 }
cbrice6 0:9e014841f2b7 85
cbrice6 0:9e014841f2b7 86 void mctrl::turnRight(float s)
cbrice6 0:9e014841f2b7 87 {
cbrice6 0:9e014841f2b7 88 mctrl.stop();
cbrice6 0:9e014841f2b7 89
cbrice6 0:9e014841f2b7 90 tcurr = t.read();
cbrice6 0:9e014841f2b7 91 while ((t.read() - tcurr) < turntime) {
cbrice6 0:9e014841f2b7 92 Lfront.speed(s);
cbrice6 0:9e014841f2b7 93 Lback.speed(s);
cbrice6 0:9e014841f2b7 94 Rfront.speed(-s);
cbrice6 0:9e014841f2b7 95 Rback.speed(-s);
cbrice6 0:9e014841f2b7 96 }
cbrice6 0:9e014841f2b7 97
cbrice6 0:9e014841f2b7 98 mctrl.stop();
cbrice6 0:9e014841f2b7 99 wait(0.02);
cbrice6 0:9e014841f2b7 100 }
cbrice6 0:9e014841f2b7 101
cbrice6 0:9e014841f2b7 102 void dist(int distance) // NOTE: by default "distance" is in mm...
cbrice6 0:9e014841f2b7 103 {
cbrice6 0:9e014841f2b7 104 if (distance < 150) {
cbrice6 0:9e014841f2b7 105 // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
cbrice6 0:9e014841f2b7 106 // 1) Turn right.
cbrice6 0:9e014841f2b7 107 // 2) Go forward __ seconds.
cbrice6 0:9e014841f2b7 108 // 3) Turn right.
cbrice6 0:9e014841f2b7 109 // 4) Continue forward until wall.
cbrice6 0:9e014841f2b7 110 }
cbrice6 0:9e014841f2b7 111 }
cbrice6 0:9e014841f2b7 112
cbrice6 0:9e014841f2b7 113 //------------------------------|
cbrice6 0:9e014841f2b7 114 // PIN INITIALIZATIONS (cont'd) |
cbrice6 0:9e014841f2b7 115 //------------------------------|
cbrice6 0:9e014841f2b7 116 // Setup Ultrasonic Sensor pins:
cbrice6 0:9e014841f2b7 117 ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14...
cbrice6 0:9e014841f2b7 118 // update every .07 secs w/ timeout after 1 sec...
cbrice6 0:9e014841f2b7 119 // call "dist" when the distance changes...
cbrice6 0:9e014841f2b7 120
cbrice6 0:9e014841f2b7 121 //---------------|
cbrice6 0:9e014841f2b7 122 // MAIN FUNCTION |
cbrice6 0:9e014841f2b7 123 //---------------|
cbrice6 0:9e014841f2b7 124 int main() {
cbrice6 0:9e014841f2b7 125 // Initialize and calibrate the IMU:
cbrice6 0:9e014841f2b7 126 IMU.begin();
cbrice6 0:9e014841f2b7 127 if (!IMU.begin()) {
cbrice6 0:9e014841f2b7 128 pc.printf("Failed to communicate with LSM9DS1.\n");
cbrice6 0:9e014841f2b7 129 }
cbrice6 0:9e014841f2b7 130 IMU.calibrate();
cbrice6 0:9e014841f2b7 131
cbrice6 0:9e014841f2b7 132 // Initialize the Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 133 usensor.startUpdates();
cbrice6 0:9e014841f2b7 134
cbrice6 0:9e014841f2b7 135 // Initialize the Timer:
cbrice6 0:9e014841f2b7 136 t.start();
cbrice6 0:9e014841f2b7 137
cbrice6 0:9e014841f2b7 138 while(1) {
cbrice6 0:9e014841f2b7 139 // Read IMU gyro:
cbrice6 0:9e014841f2b7 140 while(!IMU.gyroAvailable());
cbrice6 0:9e014841f2b7 141 IMU.readGyro();
cbrice6 0:9e014841f2b7 142 // X - IMU.calcGyro(IMU.gx)
cbrice6 0:9e014841f2b7 143 // Y - IMU.calcGyro(IMU.gx)
cbrice6 0:9e014841f2b7 144 // Z - IMU.calcGyro(IMU.gz)
cbrice6 0:9e014841f2b7 145
cbrice6 0:9e014841f2b7 146 // Read Ultrasonic Sensor:
cbrice6 0:9e014841f2b7 147 usensor.checkDistance();
cbrice6 0:9e014841f2b7 148
cbrice6 0:9e014841f2b7 149 // ...
cbrice6 0:9e014841f2b7 150
cbrice6 0:9e014841f2b7 151 wait(0.1);
cbrice6 0:9e014841f2b7 152 }
cbrice6 0:9e014841f2b7 153 }