Diff: main.cpp

Revision:

8:9bb806a7f585

Parent:

7:c0f5bb355f41

Child:

9:f10b974d01e0

--- a/main.cpp	Tue Apr 06 09:13:25 2021 +0200
+++ b/main.cpp	Tue Apr 06 12:48:09 2021 +0200
@@ -10,10 +10,8 @@
 
 using namespace std::chrono;
 
-InterruptIn    user_button(USER_BUTTON);
-DigitalOut     led(LED1);
-BufferedSerial bufferedSerial(USBTX, USBRX);
-char buffer[100];
+InterruptIn user_button(USER_BUTTON);
+DigitalOut  led(LED1);
 
 bool  executeMainTask = false;
 Timer user_button_timer, loop_timer;
@@ -28,9 +26,9 @@
 float    dist = 0.0f;
 
 /* create pwm objects */
-PwmOut     pwmOut_m0(PB_13);
-PwmOut     pwmOut_m1(PA_9);
-PwmOut     pwmOut_m2(PA_10);
+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);
@@ -46,19 +44,18 @@
 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;
+int servoHolePeriod_mus = 20000;
+int servoPeriod_mus_0 = 0;
+int servoPeriod_mus_1 = 0;
+int counter = 0;
+int loops_per_second = (int)ceilf(1.0f/(0.001f*(float)Ts_ms));
 
 /* create sd object */
 // SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
 // FATFileSystem fs("fs", &sd);
 
 int main()
-{
-    bufferedSerial.set_baud(115200);
-    // bufferedSerial.set_blocking(false);
-    
+{   
     user_button.fall(&button_fall);
     user_button.rise(&button_rise);
     loop_timer.start();
@@ -75,8 +72,8 @@
     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);
+    servo_0.Enable(servoPeriod_mus_0, servoHolePeriod_mus); // 1 ms / 20 ms
+    servo_1.Enable(servoPeriod_mus_0, servoHolePeriod_mus);
     
     /*
     // example code for sd card, not tested from pmic, 02.04.2021
@@ -116,22 +113,18 @@
             speedController_m1.setDesiredSpeedRPS( 0.5f);
             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;
+            servo_0.SetPosition(servoPeriod_mus_0);
+            servo_1.SetPosition(servoPeriod_mus_1);
+            if (servoPeriod_mus_0 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
+                servoPeriod_mus_0 += 100;
+            }
+            if (servoPeriod_mus_1 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
+                servoPeriod_mus_1 += 100;
+            }
+            counter++;
 
             /* 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.getSpeedRPS(),
-                                              speedController_m1.getSpeedRPS());
-            // bufferedSerial.write(buffer, act_buffer_length);
             
         } else {
 
@@ -141,16 +134,22 @@
             speedController_m1.setDesiredSpeedRPS(0.2f);
             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);
+            servoPeriod_mus_0 = 0;
+            servoPeriod_mus_1 = 0;
+            servo_0.SetPosition(servoPeriod_mus_0);
+            servo_1.SetPosition(servoPeriod_mus_1);
 
             dist = analogIn.read() * 3.3f;
 
             led = 0;
         }
 
+        /* do only output what's really necessary*/
+        printf("%3.3e, %3.3e, %3d, %3d; \r\n", speedController_m0.getSpeedRPS(),
+                                               speedController_m1.getSpeedRPS(),
+                                               servoPeriod_mus_0,
+                                               servoPeriod_mus_1);
+
         /* ------------- stop hacking ------------- -------------*/
 
         int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
@@ -169,6 +168,7 @@
 {
     int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
     user_button_timer.stop();
-    if (t_button_ms > 1)
+    if (t_button_ms > 200) {
         executeMainTask = !executeMainTask;
+    }
 }
\ No newline at end of file