Diff: main.cpp

Revision:

1:93d997d6b232

Parent:

0:f44522e28559

Child:

2:7c03fefb77ef

--- a/main.cpp	Tue Mar 30 12:50:43 2021 +0000
+++ b/main.cpp	Tue Mar 30 14:20:41 2021 +0000
@@ -1,65 +1,118 @@
-#include "mbed.h"
-#include "platform/mbed_thread.h"
- 
-using namespace std::chrono;
- 
-InterruptIn    user_button(USER_BUTTON);
-DigitalOut     led(LED1);
- 
-bool           executeMainTask = false;
-Timer          user_button_timer, loop_timer;
-int            Ts_ms = 50;
- 
-void           button_fall();
-void           button_rise();
- 
-/* input your stuff here */
-AnalogIn       analogIn(PA_0);
-float          dist = 0.0f;
- 
-int main()
-{
-    user_button.fall(&button_fall);
-    user_button.rise(&button_rise);
-    loop_timer.start();
- 
-    while (true) {
- 
-        loop_timer.reset();
- 
-        /* ------------- start hacking ------------- -------------*/
- 
-        if(executeMainTask) {
- 
-            dist = analogIn.read()*3.3f;
- 
-            /* do only output what's really necessary, outputting "Measured value in mV: "" within the loop is no good solution */           
-            printf("Measured value in mV: %d\r\n", (static_cast<int>(dist * 1e3)));
- 
-            /* visual feedback that the main task is executed */
-            led = !led;
- 
-        } else {
-            led = 0;
-        }
- 
-        /* ------------- stop hacking ------------- -------------*/
- 
-        int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
-        int dT_loop_ms = Ts_ms - T_loop_ms;
-        thread_sleep_for(dT_loop_ms);
-    }
-}
- 
-void button_fall()
-{
-    user_button_timer.reset();
-    user_button_timer.start();
-}
- 
-void button_rise()
-{
-    int t_button = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
-    user_button_timer.stop();
-    if(t_button > 200) executeMainTask = !executeMainTask;
-}
\ No newline at end of file
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "SDBlockDevice.h"
+#include "FATFileSystem.h"
+#include "EncoderCounter.h"
+#include "Servo.h"
+#include "Controller.h"
+
+
+int main()
+{
+    
+    DigitalIn user_button(USER_BUTTON);
+    
+    // initialise PWM
+    PwmOut pwm_motor1(PB_13);
+    PwmOut pwm_motor2(PA_9);
+    PwmOut pwm_motor3(PA_10);
+    
+    // crete Encoder read objects
+    EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
+    EncoderCounter counter2(PB_6, PB_7);
+    EncoderCounter counter3(PA_0, PA_1);
+    
+    // create controller
+    Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
+    
+    DigitalOut enable(PB_15);
+    
+    // create servo objects
+    Servo  S0(PB_2);
+    Servo  S1(PC_8);
+    Servo  S2(PC_6);
+    
+    SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
+    printf("BlockDevice created\r\n"); 
+    FATFileSystem fs("fs", &sd);
+    
+    // Initialise the digital pin LED1 as an output
+    DigitalOut myled(LED1);
+    
+    
+    /*
+    // initialise PWM
+    pwm_motor1.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor1.write(0.5f);
+    pwm_motor2.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor2.write(0.5f);*/
+    pwm_motor3.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor3.write(0.5f);
+    
+    // initialise and test Servo
+    S0.Enable(1000,20000);
+    S1.Enable(1000,20000);
+    S2.Enable(1000,20000);
+    
+    printf("Test writing... ");
+    FILE* fp = fopen("/fs/data.csv", "w");
+    fprintf(fp, "test %.5f\r\n",1.23);
+    fclose(fp);
+    printf("done\r\n");
+    
+    printf("Test reading... ");
+    // read from SD card
+    fp = fopen("/fs/data.csv", "r");
+    if (fp != NULL) {
+        char c = fgetc(fp);
+        if (c == 't')
+            printf("done\r\n");
+        else
+            printf("incorrect char (%c)!\n", c);
+        fclose(fp);
+    } else {
+        printf("Reading failed!\n");
+    }
+    
+    // enable driver DC motors
+    enable = 1;
+    
+    while (true) {
+        
+        if(!user_button) {
+            // LED off, set controller speed, pwm2, position servo
+            myled = 0;
+            controller.setDesiredSpeedLeft(50.0f);
+            controller.setDesiredSpeedRight(50.0f);
+            pwm_motor3.write(0.7f);
+            
+
+            S0.SetPosition(1000);
+            S1.SetPosition(1000);
+            S2.SetPosition(1000);
+
+        } else {
+            // LED on, reset controller speed, pwm2, position servo
+            myled = 1;
+            controller.setDesiredSpeedLeft(0.0f);
+            controller.setDesiredSpeedRight(0.0f);
+            pwm_motor3.write(0.5f);
+
+            S0.SetPosition(1500);
+            S1.SetPosition(1500);
+            S2.SetPosition(1500);
+
+        }
+
+        
+        printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
+        //printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+        
+        thread_sleep_for(200);
+    }
+}