Test

Diff: main.cpp

Revision:

12:4f635df5b368

Parent:

11:5186cc367be0

Child:

13:1b31916de79e

diff -r 5186cc367be0 -r 4f635df5b368 main.cpp
--- a/main.cpp	Thu Sep 07 00:01:45 2017 +0000
+++ b/main.cpp	Fri Nov 22 00:08:24 2019 +0000
@@ -1,39 +1,121 @@
+// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)
+
 #include "mbed.h"
+#include "Motor.h"
 #include "XNucleo53L0A1.h"
-#include <stdio.h>
-Serial pc(USBTX,USBRX);
+
+#define VL53L0_I2C_SDA   p28 // To be changed based on Pins
+#define VL53L0_I2C_SCL   p27 // To be changed based on Pins
 DigitalOut shdn(p26);
-// This VL53L0X board test application performs a range measurement in polling mode
-// Use 3.3(Vout) for Vin, p28 for SDA, p27 for SCL, P26 for shdn on mbed LPC1768
 
-//I2C sensor pins
-#define VL53L0_I2C_SDA   p28
-#define VL53L0_I2C_SCL   p27
+Motor lm(p21, p7, p8); // pwm, fwd, rev
+Motor rm(p22, p6, p10); // pwm, fwd, rev
+//BusOut myled(LED1,LED2,LED3,LED4);
+Serial BT(p13,p14);
+Serial pc(USBTX,USBRX);
+DigitalOut led(p15);
 
 static XNucleo53L0A1 *board=NULL;
+bool stopMotor = false;
 
-int main()
-{
-    int status;
-    uint32_t distance;
-    DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
-    /* creates the 53L0A1 expansion board singleton obj */
-    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
-    shdn = 0; //must reset sensor for an mbed reset to work
-    wait(0.1);
-    shdn = 1;
-    wait(0.1);
+float getSpeed(char& value) {
+
+    float ones, tens = 0, hundreds, n2, n3, dot;
+
+    dot = BT.getc();
+    n2 = BT.getc();
+    n3 = BT.getc();
+
+    ones = (float)(value -'0');
+
+    tens = (float)(n2 -'0');
+    tens = tens / 10.00;
+
+    hundreds = (float)(n3 - '0');
+    hundreds = hundreds/100.00;
+
+    return (ones + tens + hundreds);
+}
+
+void setMotorSpeed(Motor* motor, float mspeed) {
+if(mspeed <= 1.0){
+    if(abs(mspeed) > 0.2 && !stopMotor || mspeed > .2){
+        pc.printf("%0.2f is the motor speed\n\r", mspeed);
+        motor->speed(mspeed);
+        }
+    else
+        motor->speed(0);
+}
+}
+void btMotorUpdate(Motor* motor) {
+
+    char PlusMinus = BT.getc();
+
+    if(PlusMinus == '-'){
+        char n1 = BT.getc();
+        setMotorSpeed(motor, -(getSpeed(n1)));
+
+    } else {
+        setMotorSpeed(motor, getSpeed(PlusMinus));
+
+    }
+}
+
+void btSerialInterrupt() {
+
+    char LRABYX = BT.getc();
+
+    if(LRABYX == 'L'){
+        btMotorUpdate(&lm);
+    }
+    if(LRABYX == 'R') {
+        btMotorUpdate(&rm);
+    }
+    
+
+}
+
+
+int main() {
+
+     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
+     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
+     uint32_t distance;
+     shdn = 0; //must reset sensor for an mbed reset to work
+     wait(0.1);
+     shdn = 1;
+     wait(0.1);
+
     /* init the 53L0A1 board with default values */
-    status = board->init_board();
+    int status = board->init_board();
     while (status) {
-        pc.printf("Failed to init board! \r\n");
         status = board->init_board();
     }
-    //loop taking and printing distance
+
+    BT.attach(&btSerialInterrupt);
+    lm.speed(0);
+    rm.speed(0);
+    led = 0;
+
     while (1) {
         status = board->sensor_centre->get_distance(&distance);
         if (status == VL53L0X_ERROR_NONE) {
-            pc.printf("D=%ld mm\r\n", distance);
+            pc.printf("%d\n\r",distance);
+            
+            if (distance < 250) { // Distance might need to be adjusted
+                stopMotor = true;
+                led = !led;
+                wait(.1);
+                }
+            else{
+                stopMotor = false;
+                led = 0;
+                }
+                
+            
+                // Any Lighting or Sound Effects for Crash Detection
+            
         }
     }
-}
+
+}
\ No newline at end of file