Route Fix
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
main.cpp@4:6546c17ac0a6, 2018-11-21 (annotated)
- Committer:
- cbrice6
- Date:
- Wed Nov 21 19:33:33 2018 +0000
- Revision:
- 4:6546c17ac0a6
- Parent:
- 3:c07ea8bf242e
- Child:
- 5:85951362fd6d
jk now I fixed it
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:c07ea8bf242e | 17 | |
cbrice6 | 3:c07ea8bf242e | 18 | // Motor Power Control pins: |
cbrice6 | 3:c07ea8bf242e | 19 | /* |
cbrice6 | 3:c07ea8bf242e | 20 | DigitalOut Ctrl1(p29); |
cbrice6 | 3:c07ea8bf242e | 21 | DigitalOut Ctrl2(p30); |
cbrice6 | 3:c07ea8bf242e | 22 | PinDetect sw(p20); |
cbrice6 | 3:c07ea8bf242e | 23 | */ |
cbrice6 | 0:9e014841f2b7 | 24 | |
cbrice6 | 0:9e014841f2b7 | 25 | // Setup Motor Driver pins: |
cbrice6 | 0:9e014841f2b7 | 26 | Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... |
cbrice6 | 1:92f6242c0196 | 27 | Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... |
cbrice6 | 1:92f6242c0196 | 28 | Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... |
cbrice6 | 0:9e014841f2b7 | 29 | Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11... |
cbrice6 | 0:9e014841f2b7 | 30 | |
cbrice6 | 0:9e014841f2b7 | 31 | Timer t; |
cbrice6 | 0:9e014841f2b7 | 32 | |
cbrice6 | 0:9e014841f2b7 | 33 | //-----------------------| |
cbrice6 | 0:9e014841f2b7 | 34 | // CLASS INITIALIZATIONS | |
cbrice6 | 0:9e014841f2b7 | 35 | //-----------------------| |
cbrice6 | 0:9e014841f2b7 | 36 | class mctrl { |
cbrice6 | 0:9e014841f2b7 | 37 | public: |
cbrice6 | 1:92f6242c0196 | 38 | // Stop all motors: |
cbrice6 | 1:92f6242c0196 | 39 | void stop(void) { |
cbrice6 | 1:92f6242c0196 | 40 | Lfront.speed(0); |
cbrice6 | 1:92f6242c0196 | 41 | Lback.speed(0); |
cbrice6 | 1:92f6242c0196 | 42 | Rfront.speed(0); |
cbrice6 | 1:92f6242c0196 | 43 | Rback.speed(0); |
cbrice6 | 1:92f6242c0196 | 44 | wait(0.02); |
cbrice6 | 1:92f6242c0196 | 45 | }; |
cbrice6 | 3:c07ea8bf242e | 46 | |
cbrice6 | 1:92f6242c0196 | 47 | // Go forward at constant speed: |
cbrice6 | 1:92f6242c0196 | 48 | void fwd(void){ |
cbrice6 | 1:92f6242c0196 | 49 | stop(); |
cbrice6 | 1:92f6242c0196 | 50 | |
cbrice6 | 1:92f6242c0196 | 51 | Lfront.speed(1); |
cbrice6 | 1:92f6242c0196 | 52 | Lback.speed(1); |
cbrice6 | 1:92f6242c0196 | 53 | Rfront.speed(1); |
cbrice6 | 1:92f6242c0196 | 54 | Rback.speed(1); |
cbrice6 | 1:92f6242c0196 | 55 | wait(0.02); |
cbrice6 | 1:92f6242c0196 | 56 | }; |
cbrice6 | 3:c07ea8bf242e | 57 | |
cbrice6 | 1:92f6242c0196 | 58 | // Reverse at constant speed: |
cbrice6 | 1:92f6242c0196 | 59 | void rev(void){ |
cbrice6 | 1:92f6242c0196 | 60 | stop(); |
cbrice6 | 1:92f6242c0196 | 61 | Lfront.speed(-1); |
cbrice6 | 1:92f6242c0196 | 62 | Lback.speed(-1); |
cbrice6 | 1:92f6242c0196 | 63 | Rfront.speed(-1); |
cbrice6 | 1:92f6242c0196 | 64 | Rback.speed(-1); |
cbrice6 | 1:92f6242c0196 | 65 | wait(0.02); |
cbrice6 | 1:92f6242c0196 | 66 | }; |
cbrice6 | 3:c07ea8bf242e | 67 | |
cbrice6 | 1:92f6242c0196 | 68 | // Turn left 90 degrees: |
cbrice6 | 4:6546c17ac0a6 | 69 | void turnLeft(float currt){ |
cbrice6 | 1:92f6242c0196 | 70 | stop(); |
cbrice6 | 3:c07ea8bf242e | 71 | while ((t.read()-currt) < 2) { |
cbrice6 | 1:92f6242c0196 | 72 | Lfront.speed(-1); |
cbrice6 | 1:92f6242c0196 | 73 | Lback.speed(-1); |
cbrice6 | 1:92f6242c0196 | 74 | Rfront.speed(1); |
cbrice6 | 1:92f6242c0196 | 75 | Rback.speed(1); |
cbrice6 | 1:92f6242c0196 | 76 | } |
cbrice6 | 1:92f6242c0196 | 77 | stop(); |
cbrice6 | 1:92f6242c0196 | 78 | wait(0.02); |
cbrice6 | 1:92f6242c0196 | 79 | }; |
cbrice6 | 3:c07ea8bf242e | 80 | |
cbrice6 | 1:92f6242c0196 | 81 | // Turn right 90 degrees: |
cbrice6 | 4:6546c17ac0a6 | 82 | void turnRight(float currt){ |
cbrice6 | 1:92f6242c0196 | 83 | stop(); |
cbrice6 | 3:c07ea8bf242e | 84 | while ((t.read()-currt) < 2) { |
cbrice6 | 1:92f6242c0196 | 85 | Lfront.speed(1); |
cbrice6 | 1:92f6242c0196 | 86 | Lback.speed(1); |
cbrice6 | 1:92f6242c0196 | 87 | Rfront.speed(-1); |
cbrice6 | 1:92f6242c0196 | 88 | Rback.speed(-1); |
cbrice6 | 1:92f6242c0196 | 89 | } |
cbrice6 | 1:92f6242c0196 | 90 | stop(); |
cbrice6 | 1:92f6242c0196 | 91 | wait(0.02); |
cbrice6 | 1:92f6242c0196 | 92 | }; |
cbrice6 | 1:92f6242c0196 | 93 | } mctrl; |
cbrice6 | 0:9e014841f2b7 | 94 | |
cbrice6 | 0:9e014841f2b7 | 95 | //------------------| |
cbrice6 | 0:9e014841f2b7 | 96 | // HELPER FUNCTIONS | |
cbrice6 | 0:9e014841f2b7 | 97 | //------------------| |
cbrice6 | 0:9e014841f2b7 | 98 | void dist(int distance) // NOTE: by default "distance" is in mm... |
cbrice6 | 0:9e014841f2b7 | 99 | { |
cbrice6 | 3:c07ea8bf242e | 100 | if (distance < 150) { |
cbrice6 | 3:c07ea8bf242e | 101 | // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm |
cbrice6 | 3:c07ea8bf242e | 102 | // 1) Turn 90 degrees (hardcoded => 2 seconds). |
cbrice6 | 3:c07ea8bf242e | 103 | // 2) Go forward 2 seconds. |
cbrice6 | 3:c07ea8bf242e | 104 | // 3) Turn 90 degrees (hardcoded => 2 seconds). |
cbrice6 | 3:c07ea8bf242e | 105 | // 4) Continue forward until wall. |
cbrice6 | 3:c07ea8bf242e | 106 | // [Step 1] |
cbrice6 | 1:92f6242c0196 | 107 | float currt = t.read(); |
cbrice6 | 4:6546c17ac0a6 | 108 | if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} |
cbrice6 | 3:c07ea8bf242e | 109 | // [Step 2] |
cbrice6 | 3:c07ea8bf242e | 110 | currt = t.read(); |
cbrice6 | 4:6546c17ac0a6 | 111 | while ((t.read()-currt) < 2) {mctrl.fwd();} |
cbrice6 | 3:c07ea8bf242e | 112 | // [Step 3] |
cbrice6 | 3:c07ea8bf242e | 113 | currt = t.read(); |
cbrice6 | 4:6546c17ac0a6 | 114 | if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} |
cbrice6 | 3:c07ea8bf242e | 115 | // [End] |
cbrice6 | 3:c07ea8bf242e | 116 | tr = !tr; |
cbrice6 | 1:92f6242c0196 | 117 | } |
cbrice6 | 1:92f6242c0196 | 118 | else { |
cbrice6 | 1:92f6242c0196 | 119 | mctrl.fwd(); |
cbrice6 | 0:9e014841f2b7 | 120 | } |
cbrice6 | 0:9e014841f2b7 | 121 | } |
cbrice6 | 0:9e014841f2b7 | 122 | |
cbrice6 | 0:9e014841f2b7 | 123 | //------------------------------| |
cbrice6 | 0:9e014841f2b7 | 124 | // PIN INITIALIZATIONS (cont'd) | |
cbrice6 | 0:9e014841f2b7 | 125 | //------------------------------| |
cbrice6 | 0:9e014841f2b7 | 126 | // Setup Ultrasonic Sensor pins: |
cbrice6 | 0:9e014841f2b7 | 127 | ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14... |
cbrice6 | 0:9e014841f2b7 | 128 | // update every .07 secs w/ timeout after 1 sec... |
cbrice6 | 0:9e014841f2b7 | 129 | // call "dist" when the distance changes... |
cbrice6 | 0:9e014841f2b7 | 130 | |
cbrice6 | 0:9e014841f2b7 | 131 | //---------------| |
cbrice6 | 0:9e014841f2b7 | 132 | // MAIN FUNCTION | |
cbrice6 | 0:9e014841f2b7 | 133 | //---------------| |
cbrice6 | 0:9e014841f2b7 | 134 | int main() { |
cbrice6 | 3:c07ea8bf242e | 135 | // Use internal pullup for the switch: |
cbrice6 | 3:c07ea8bf242e | 136 | //sw.mode(PullUp); |
cbrice6 | 3:c07ea8bf242e | 137 | |
cbrice6 | 3:c07ea8bf242e | 138 | // Delay for initial pullups to take effect: |
cbrice6 | 3:c07ea8bf242e | 139 | //wait(0.01); |
cbrice6 | 3:c07ea8bf242e | 140 | |
cbrice6 | 0:9e014841f2b7 | 141 | // Initialize and calibrate the IMU: |
cbrice6 | 3:c07ea8bf242e | 142 | /* |
cbrice6 | 3:c07ea8bf242e | 143 | LSM9DS1 imu(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27... |
cbrice6 | 3:c07ea8bf242e | 144 | imu.begin(); |
cbrice6 | 3:c07ea8bf242e | 145 | imu.calibrate(1); |
cbrice6 | 3:c07ea8bf242e | 146 | imu.calibrateMag(0); |
cbrice6 | 1:92f6242c0196 | 147 | |
cbrice6 | 3:c07ea8bf242e | 148 | imu.readGyro(); |
cbrice6 | 3:c07ea8bf242e | 149 | start = imu.gz; |
cbrice6 | 3:c07ea8bf242e | 150 | */ |
cbrice6 | 0:9e014841f2b7 | 151 | |
cbrice6 | 0:9e014841f2b7 | 152 | // Initialize the Ultrasonic Sensor: |
cbrice6 | 0:9e014841f2b7 | 153 | usensor.startUpdates(); |
cbrice6 | 0:9e014841f2b7 | 154 | |
cbrice6 | 0:9e014841f2b7 | 155 | // Initialize the Timer: |
cbrice6 | 0:9e014841f2b7 | 156 | t.start(); |
cbrice6 | 0:9e014841f2b7 | 157 | |
cbrice6 | 0:9e014841f2b7 | 158 | while(1) { |
cbrice6 | 3:c07ea8bf242e | 159 | // Toggle motor battery banks: |
cbrice6 | 3:c07ea8bf242e | 160 | /* |
cbrice6 | 3:c07ea8bf242e | 161 | if (!sw == 1) { |
cbrice6 | 3:c07ea8bf242e | 162 | Ctrl1 = 1; |
cbrice6 | 3:c07ea8bf242e | 163 | Ctrl2 = 1; |
cbrice6 | 3:c07ea8bf242e | 164 | wait(0.2); |
cbrice6 | 3:c07ea8bf242e | 165 | } |
cbrice6 | 3:c07ea8bf242e | 166 | else { |
cbrice6 | 3:c07ea8bf242e | 167 | Ctrl1 = 0; |
cbrice6 | 3:c07ea8bf242e | 168 | Ctrl2 = 0; |
cbrice6 | 3:c07ea8bf242e | 169 | wait(0.2); |
cbrice6 | 3:c07ea8bf242e | 170 | } |
cbrice6 | 3:c07ea8bf242e | 171 | */ |
cbrice6 | 3:c07ea8bf242e | 172 | |
cbrice6 | 0:9e014841f2b7 | 173 | // Read IMU gyro: |
cbrice6 | 3:c07ea8bf242e | 174 | //while(!imu.gyroAvailable()); |
cbrice6 | 3:c07ea8bf242e | 175 | //imu.readGyro(); |
cbrice6 | 3:c07ea8bf242e | 176 | //heading = imu.calcGyro(imu.gz); |
cbrice6 | 3:c07ea8bf242e | 177 | // X - imu.calcGyro(imu.gx) |
cbrice6 | 3:c07ea8bf242e | 178 | // Y - imu.calcGyro(imu.gy) |
cbrice6 | 3:c07ea8bf242e | 179 | // Z - imu.calcGyro(imu.gz) |
cbrice6 | 3:c07ea8bf242e | 180 | |
cbrice6 | 3:c07ea8bf242e | 181 | //if (heading > 90) {led=1;} else {led=0;} |
cbrice6 | 0:9e014841f2b7 | 182 | |
cbrice6 | 0:9e014841f2b7 | 183 | // Read Ultrasonic Sensor: |
cbrice6 | 0:9e014841f2b7 | 184 | usensor.checkDistance(); |
cbrice6 | 0:9e014841f2b7 | 185 | |
cbrice6 | 0:9e014841f2b7 | 186 | wait(0.1); |
cbrice6 | 0:9e014841f2b7 | 187 | } |
cbrice6 | 0:9e014841f2b7 | 188 | } |
cbrice6 | 3:c07ea8bf242e | 189 |