Route Fix
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- cbrice6
- Date:
- 2018-11-19
- Revision:
- 0:9e014841f2b7
- Child:
- 1:92f6242c0196
File content as of revision 0:9e014841f2b7:
#include "mbed.h" #include "LSM9DS1.h" // IMU #include "ultrasonic.h" // Ultrasonic Sensor #include "Motor.h" // Motor Drivers //---------------------| // PIN INITIALIZATIONS | //---------------------| // Setup IMU pins: LSM9DS1 IMU(p28, p27, 0xD6, 0x3C); // SDA to p28, SCL to p27.. // Setup Motor Driver pins: Motor Lfront(p21, p6, p5); // PWM to p21, forward to p6, reverse to p5... Motor Lback(p22, p8, p7); // PWM to p22, forward to p8, reverse to p7... Motor Rfront(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: void stop(void); void fwd(float s); void rev(float s); void turnLeft(float s); void turnRight(float s); private: float tcurr; float turntime }; //------------------| // HELPER FUNCTIONS | //------------------| void mctrl::stop(void) { Lfront.speed(0); Lback.speed(0); Rfront.speed(0); Rback.speed(0); wait(0.02); } void mctrl::fwd(float s) { mctrl.stop(); Lfront.speed(s); Lback.speed(s); Rfront.speed(s); Rback.speed(s); wait(0.02); } void mctrl::rev(float s) { mctrl.stop(); Lfront.speed(-s); Lback.speed(-s); Rfront.speed(-s); Rback.speed(-s); wait(0.02); } void mctrl::turnLeft(float s) { mctrl.stop(); tcurr = t.read(); while ((t.read() - tcurr) < turntime) { Lfront.speed(-s); Lback.speed(-s); Rfront.speed(s); Rback.speed(s); } mctrl.stop(); wait(0.02); } void mctrl::turnRight(float s) { mctrl.stop(); tcurr = t.read(); while ((t.read() - tcurr) < turntime) { Lfront.speed(s); Lback.speed(s); Rfront.speed(-s); Rback.speed(-s); } mctrl.stop(); wait(0.02); } 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 right. // 2) Go forward __ seconds. // 3) Turn right. // 4) Continue forward until wall. } } //------------------------------| // 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... //---------------| // MAIN FUNCTION | //---------------| int main() { // Initialize and calibrate the IMU: IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } IMU.calibrate(); // Initialize the Ultrasonic Sensor: usensor.startUpdates(); // Initialize the Timer: t.start(); while(1) { // Read IMU gyro: while(!IMU.gyroAvailable()); IMU.readGyro(); // X - IMU.calcGyro(IMU.gx) // Y - IMU.calcGyro(IMU.gx) // Z - IMU.calcGyro(IMU.gz) // Read Ultrasonic Sensor: usensor.checkDistance(); // ... wait(0.1); } }