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);
+    }
+}