Route Fix
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- Arkantos1695
- Date:
- 2018-11-28
- Revision:
- 8:2d0fc244cc65
- Parent:
- 7:5667ce16d526
- Child:
- 9:ec0ceec8f5f5
File content as of revision 8:2d0fc244cc65:
#include "mbed.h" #include "LSM9DS1.h" // IMU #include "ultrasonic.h" // Ultrasonic Sensor #include "Motor.h" // Motor Drivers #include "PinDetect.h" // Pin Detect (for switch) // Global Variables: bool tr = true; bool turnflag = false; int leftdistance= 0; int rightdistance = 0; //---------------------| // PIN INITIALIZATIONS | //---------------------| // Debug LED pin: //DigitalOut led(p25); DigitalIn sw(p20); Serial pc(USBTX,USBRX); // Setup Motor Driver pins: Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... Motor Rfront(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... Motor Lback(p23, p10, p9); // PWM to p23, forward to p10, reverse to p9... Motor Rback(p24, p12, p11); // PWM to p24, forward to p12, reverse to p11... Timer t; //-----------------------| // CLASS INITIALIZATIONS | //-----------------------| class mctrl { public: // Stop all motors: void stop(void) { Lfront.speed(0); Lback.speed(0); Rfront.speed(0); Rback.speed(0); wait(0.55); }; // Go forward at constant speed: void fwd(void){ //stop(); Lfront.speed(1); Lback.speed(1); Rfront.speed(1); Rback.speed(1); wait(0.02); }; // Reverse at constant speed: void rev(void){ stop(); Lfront.speed(-1); Lback.speed(-1); Rfront.speed(-1); Rback.speed(-1); wait(0.02); }; // Turn left 90 degrees: void turnLeft(float currt){ stop(); while ((t.read()-currt) < 1.30) { Lfront.speed(-1); Lback.speed(-1); Rfront.speed(1); Rback.speed(1); } stop(); //wait(0.02); }; // Turn right 90 degrees: void turnRight(float currt){ stop(); while ((t.read()-currt) < 1.30) { Lfront.speed(1); Lback.speed(1); Rfront.speed(-1); Rback.speed(-1); } stop(); //wait(0.02); }; } mctrl; //------------------| // HELPER FUNCTIONS | //------------------| void dist(int distance) // NOTE: by default "distance" is in mm... { if (distance < 150) { // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm // 1) Turn 90 degrees (hardcoded => 1.4 seconds). // 2) Go forward 1.5 seconds. // 3) Turn 90 degrees (hardcoded => 1.4 seconds). // 4) Continue forward until wall. // [Step 1] float currt = t.read(); if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} // [Step 2] currt = t.read(); while ((t.read()-currt) < .5) {mctrl.fwd();} // [Step 3] currt = t.read(); if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);} // [End] tr = !tr; } else { mctrl.fwd(); } } void dist2(int distance) // left sensor interrupt { /*if (distance < 150) { pc.printf("Left Sensor trigg"); } else { pc.printf("Left Sensor echo"); }*/ leftdistance = distance; } void dist3(int distance) // right sensor interrupt { /*if (distance < 150) { pc.printf("Right Sensor trigg"); } else { pc.printf("Right Sensor echo"); }*/ rightdistance = distance; } //------------------------------| // PIN INITIALIZATIONS (cont'd) | //------------------------------| // Setup Ultrasonic Sensor pins: ultrasonic usensor(p13, p14, .07, 1, &dist); // trigger to p13, echo to p14... // update every .07 secs w/ timeout after 1 sec... // call "dist" when the distance changes... ultrasonic usensor2(p13, p26, .07, 1, &dist2); // left trigger ultrasonic usensor3(p13, p25, .07, 1, &dist3); //right trigger //---------------| // MAIN FUNCTION | //---------------| int main() { // Use internal pullup for the switch: sw.mode(PullUp); // Initialize the Ultrasonic Sensor: usensor.startUpdates(); usensor2.startUpdates(); usensor3.startUpdates(); // Initialize the Timer: t.start(); while(!sw) { usensor.checkDistance(); usensor2.checkDistance(); usensor3.checkDistance(); pc.printf("Left Distance %d mm\r\n", leftdistance); pc.printf("Right Distance %d mm\r\n", rightdistance); wait(1); } }