Diff: main.cpp

Revision:

95:a82ac1cd5f6a

Parent:

88:bea4f2daa48c

--- a/main.cpp	Thu Apr 11 15:00:04 2019 +0100
+++ b/main.cpp	Mon Nov 18 17:24:39 2019 +0000
@@ -1,32 +1,82 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2018 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
+#include "mbed.h"
+#include "hcsr04.h"
+
+Serial pc(USBTX,USBRX);
+
+HCSR04  csensor(A0, A1);    //Trigger, Echo
+HCSR04  lsensor(A2, A3);    //Trigger, Echo
+HCSR04  rsensor(A4, A5);    //Trigger, Echo
+
+PwmOut motor_l1(D13);
+PwmOut motor_l2(D12);
+PwmOut motor_r1(D11);
+PwmOut motor_r2(D10);
+
+unsigned int dist;
+
+void initialise()
+{
+    motor_l1.write(0);
+    motor_r1.write(0);
+}
 
-#include "mbed.h"
-#include "stats_report.h"
+void forward()
+{
+    motor_l1.period_ms(50);
+    motor_l1.write(0.75f);
+    motor_l2.write(0);
+    motor_l2.period_ms(50);
+    motor_r1.write(0.75f);
+    motor_r2.write(0);
+}
+void stop()
+{
+    motor_l1.write(0);
+    motor_l2.write(0);
+    motor_r1.write(0);
+    motor_r2.write(0);
+}
 
-DigitalOut led1(LED1);
-
-#define SLEEP_TIME                  500 // (msec)
-#define PRINT_AFTER_N_LOOPS         20
+int cping()
+{
+    csensor.start();
+    wait_ms(200);
+    return(csensor.get_dist_cm());
+}
+int lping()
+{
+    lsensor.start();
+    wait_ms(200);
+    return(lsensor.get_dist_cm());
+}
+int rping()
+{
+    rsensor.start();
+    wait_ms(200);
+    return(rsensor.get_dist_cm());
+}
 
 // main() runs in its own thread in the OS
 int main()
 {
-    SystemReport sys_state( SLEEP_TIME * PRINT_AFTER_N_LOOPS /* Loop delay time in ms */);
-
-    int count = 0;
-    while (true) {
-        // Blink LED and wait 0.5 seconds
-        led1 = !led1;
-        wait_ms(SLEEP_TIME);
-
-        if ((0 == count) || (PRINT_AFTER_N_LOOPS == count)) {
-            // Following the main thread wait, report on the current system status
-            sys_state.report_state();
-            count = 0;
+    int cdistance, ldistance, rdistance;                 //Variable to store distance from an object
+    initialise();
+    while (true)
+    {
+        cdistance = cping();   // ping function
+        ldistance = lping();   // ping function
+        rdistance = rping();   // ping function
+        if (cdistance <20 || ldistance <20 || rdistance <20)
+        {
+            pc.printf("\n c-cm:%ld, l-cm:%ld, r-cm:%ld\n halting",cdistance, ldistance, rdistance);
+            wait_ms(100);
+            stop();
         }
-        ++count;
+        else
+        {
+            pc.printf("\n no object");
+            forward();
+            wait_ms(100);
+        }
     }
-}
+}
\ No newline at end of file