something
Dependencies: mbed Motor LSM9DS1_Library_cal PinDetect HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 0:9e014841f2b7
- Child:
- 1:92f6242c0196
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Nov 19 01:08:21 2018 +0000 @@ -0,0 +1,153 @@ +#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); + } +}