Diff: main.cpp

Revision:

6:e1fa1a2d7483

Parent:

4:67506e285ad0

Child:

7:c0f5bb355f41

--- a/main.cpp	Fri Apr 02 09:33:18 2021 +0200
+++ b/main.cpp	Tue Apr 06 08:00:43 2021 +0200
@@ -1,69 +1,91 @@
 #include "mbed.h"
 #include "platform/mbed_thread.h"
-#include "SDBlockDevice.h"
-#include "FATFileSystem.h"
+#include "string"
+
+/* PM2_Libary */
 #include "EncoderCounter.h"
 #include "Servo.h"
 #include "SpeedController.h"
+// #include "FATFileSystem.h"
+// #include "SDBlockDevice.h"
+
+using namespace std::chrono;
+
+InterruptIn    user_button(USER_BUTTON);
+DigitalOut     led(LED1);
+BufferedSerial bufferedSerial(USBTX, USBRX);
+char buffer[100];
+
+bool  executeMainTask = false;
+Timer user_button_timer, loop_timer;
+int   Ts_ms = 2;
+
+/* declaration of custom button functions */
+void button_fall();
+void button_rise();
+
+/* create analog input object */
+AnalogIn analogIn(PC_2);
+float    dist = 0.0f;
+
+/* create pwm objects */
+PwmOut     pwmOut_m0(PB_13);
+PwmOut     pwmOut_m1(PA_9);
+PwmOut     pwmOut_m2(PA_10);
+float  Ts_pwm = 0.00005f;
+/* create enable dc motor digital out object */
+DigitalOut enable_motors(PB_15);
+/* create encoder read objects */
+EncoderCounter  encoderCounter_m0(PA_6, PC_7);
+EncoderCounter  encoderCounter_m1(PB_6, PB_7);
+EncoderCounter  encoderCounter_m2(PA_0, PA_1);
+/* create speed controller objects, only m0 and m1, m2 is used open-loop */
+SpeedController speedController_m0(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m0, encoderCounter_m0);
+SpeedController speedController_m1(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m1, encoderCounter_m1);
+
+/* create servo objects */
+Servo servo_0(PB_2);
+Servo servo_1(PC_8);
+Servo servo_2(PC_6); // not used in this example
+int Ts_servo_mus = 20000;
+int servo_desval_0 = 0;
+int servo_desval_1 = 0;
+
+/* create sd object */
+// SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
+// FATFileSystem fs("fs", &sd);
 
 int main()
 {
-    
-    DigitalIn user_button(USER_BUTTON);
-    
-    // initialise PWM
-    PwmOut pwm_motor_0(PB_13);
-    PwmOut pwm_motor_1(PA_9);
-    PwmOut pwm_motor_2(PA_10);
-    
-    // crete Encoder read objects
-    EncoderCounter encoderCounter_0(PA_6, PC_7);
-    EncoderCounter encoderCounter_1(PB_6, PB_7);
-    EncoderCounter encoderCounter_2(PA_0, PA_1);
-    
-    // create controller
-    SpeedController speedController_0(pwm_motor_0, encoderCounter_0);
-    SpeedController speedController_1(pwm_motor_1, encoderCounter_1);
-        
-    DigitalOut enable_dc_motors(PB_15);
-    
-    // create servo objects
-    Servo  servo_0(PB_2);
-    Servo  servo_1(PC_8);
-    Servo  servo_2(PC_6);
+    bufferedSerial.set_baud(115200);
     
-    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);
+    user_button.fall(&button_fall);
+    user_button.rise(&button_rise);
+    loop_timer.start();
+
+    /* initialize pwm */
+    pwmOut_m0.period(Ts_pwm);
+    pwmOut_m1.period(Ts_pwm);
+    pwmOut_m2.period(Ts_pwm);
+    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
+    pwmOut_m1.write(0.5f);
+    pwmOut_m0.write(0.5f);
+    pwmOut_m2.write(0.5f);
+    /* enable driver DC motors */
+    enable_motors = 1;
+
+    /* initialize servo */
+    servo_0.Enable(servo_desval_0, Ts_servo_mus); // 1 ms / 20 ms
+    servo_1.Enable(servo_desval_0, Ts_servo_mus);
     
-    // initialise PWM
-    pwm_motor_0.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor_0.write(0.5f);
-    pwm_motor_1.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor_1.write(0.5f);
-    pwm_motor_2.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor_2.write(0.5f);
-    
-    // initialise and test Servo
-    servo_0.Enable(1000, 20000); // 1 ms / 20 ms
-    servo_1.Enable(1000, 20000);
-    int servo_desval_0 = 0;
-    int servo_desval_1 = 0;
-    
-    /* input your stuff here */
-    AnalogIn       analogIn(PC_2);
-    float          dist = 0.0f;
-        
     /*
+    // example code for sd card, not tested from pmic, 02.04.2021
     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");
@@ -78,51 +100,75 @@
         printf("Reading failed!\n");
     }
     */
-    
-    // enable driver DC motors
-    enable_dc_motors = 1;
-    
+
     while (true) {
-        
-        if(!user_button) {
-            // LED off, set speedController speed, pwm2, position servo
-            myled = 0;
-            
-            speedController_0.setDesiredSpeed(  60.0f);
-            speedController_1.setDesiredSpeed(-120.0f);
-            pwm_motor_2.write(0.7f);
-            
+
+        loop_timer.reset();
+
+        /* ------------- start hacking ------------- -------------*/
+
+        if (executeMainTask) {
+
+            /* read analog input */
+            dist = analogIn.read() * 3.3f;
+
+            speedController_m0.setDesiredSpeedRPS( 1.0f);
+            speedController_m1.setDesiredSpeedRPS(-2.0f);
+            pwmOut_m2.write(0.75f);
+
             servo_0.SetPosition(servo_desval_0);
             servo_1.SetPosition(servo_desval_1);
-            if(servo_desval_0 < 10000) servo_desval_0 += 100;
-            if(servo_desval_1 < 10000) servo_desval_1 += 100;
+            if (servo_desval_0 < 10000)
+                servo_desval_0 += 100;
+            if (servo_desval_1 < 10000)
+                servo_desval_1 += 100;
+
+            /* visual feedback that the main task is executed */
+            led = !led;
+
+            /* write output via serial buffer */
+            int act_buffer_length = snprintf (buffer, 100,
+                                              "%3.6e, %3.6e; \r\n",
+                                              speedController_m0.getSpeedRPM(),
+                                              speedController_m1.getSpeedRPM());
+            bufferedSerial.write(buffer, act_buffer_length);
             
-            dist = analogIn.read()*3.3f;
-
         } else {
-            // LED on, reset speedController speed, pwm2, position servo
-            myled = 1;
-            
-            speedController_0.setDesiredSpeed(0.0f);
-            speedController_1.setDesiredSpeed(0.0f);
-            pwm_motor_2.write(0.5f);
+
+            dist = 0.0f;
 
-            servo_desval_0 = 0.0f;
-            servo_desval_1 = 0.0f;
+            speedController_m0.setDesiredSpeedRPS(0.0f);
+            speedController_m1.setDesiredSpeedRPS(0.0f);
+            pwmOut_m2.write(0.5f);
+
+            servo_desval_0 = 0;
+            servo_desval_1 = 0;
             servo_0.SetPosition(servo_desval_0);
             servo_1.SetPosition(servo_desval_1);
-            
-            dist = analogIn.read()*3.3f;
 
+            dist = analogIn.read() * 3.3f;
+
+            led = 0;
         }
-        
-        // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
-        // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
-        printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d measured value in mV: %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), 
-                                                                                                                   static_cast<int>(speedController_1.getSpeed()*1000.0f), 
-                                                                                                                   encoderCounter_2.read(), 
-                                                                                                                   (static_cast<int>(dist * 1e3)));
-        
-        thread_sleep_for(500);
+
+        /* ------------- 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_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
+    user_button_timer.stop();
+    if (t_button_ms > 1)
+        executeMainTask = !executeMainTask;
+}
\ No newline at end of file