Diff: main.cpp

Revision:

17:c19b471f05cb

Parent:

16:1be4a1c2d08a

Child:

20:7e7325edcf5c

--- a/main.cpp	Sat Jan 22 13:25:25 2022 +0000
+++ b/main.cpp	Thu Feb 10 12:04:51 2022 +0000
@@ -1,157 +1,167 @@
 #include "mbed.h"
-#include "platform/mbed_thread.h"
-
-/* PM2_Libary */
-#include "EncoderCounter.h"
-#include "Servo.h"
-#include "SpeedController.h"
-#include "PositionController.h"
-#include "FastPWM.h"
-#include "RangeFinder.h"
-
-using namespace std::chrono;
+#include "PM2_Libary.h"
 
 InterruptIn user_button(PC_13);
 DigitalOut  led(LED1);
 
-bool  executeMainTask = false;
+
+bool  do_execute_main = false;
 Timer user_button_timer, loop_timer;
-int   Ts_ms = 50;
+int   loop_period_ms = 50;
+
 
 /* declaration of custom button functions */
 void button_fall();
 void button_rise();
 
+
 // SHARP GP2Y0A21Y IR Sensor
-// https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds
 /* create analog input object */
-AnalogIn analogIn(PC_2);
-float    dist_IRSensor = 0.0f;
+AnalogIn analog_in(PC_2);
+float    dist_ir_sensor = 0.0f;
+
 
 // 78:1 Metal Gearmotor 20Dx43L mm 12V CB
-// https://www.pololu.com/product/3477
 /* create enable dc motor digital out object */
 DigitalOut enable_motors(PB_15);
+
 /* create pwm objects */
-FastPWM pwmOut_M1(PB_13);
-FastPWM pwmOut_M2(PA_9);
-FastPWM pwmOut_M3(PA_10);
-double  Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
+FastPWM pwm_M1(PB_13);
+FastPWM pwm_M2(PA_9);
+FastPWM pwm_M3(PA_10);
+float   pwm_period_s = 0.00005f;
+
 /* create encoder read objects */
-EncoderCounter  encoderCounter_M1(PA_6, PC_7);
-EncoderCounter  encoderCounter_M2(PB_6, PB_7);
-EncoderCounter  encoderCounter_M3(PA_0, PA_1);
-/* create speed controller objects, only M1 and M2, M3 is used open-loop */
-float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
-float kn = 180.0f/12.0f;               // (RPM/V)
-float max_voltage = 12.0f;             // adjust this to 6.0f if only one batterypack is used
-SpeedController    speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
-PositionController positionController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
-float max_speed_rps = 1.0f;            // has to be smaller or equal to kn * max_voltage
+EncoderCounter  encoder_M1(PA_6, PC_7);
+EncoderCounter  encoder_M2(PB_6, PB_7);
+EncoderCounter  encoder_M3(PA_0, PA_1);
+
+/* create speed and position controller objects, default parametrization is for 78.125:1 gear */
+float max_voltage = 12.0f;               // adjust this to 6.0f if only one batterypack is used
+float counts_per_turn = 20.0f * 78.125f; // counts/turn * gearratio
+float kn = 180.0f / 12.0f;               // motor constant (RPM/V)
 
-// Futaba Servo S3001 20mm 3kg Analog 
-// https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE
+float k_gear = 25.0f / 78.125f; // in case you are using a dc motor with a different gear box
+float kp = 0.1f;                // this is the default speed controller gain for gear box 78.125:1
+
+SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear with default contoller parameters
+// SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 25:1 gear
+
+PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
+// PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 25:1 gear, we need a different speed controller gain here
+float max_speed_rps = 0.5f;              // has to be smaller or equal to kn * max_voltage
+
+
+// Futaba Servo S3001 20mm 3kg Analog
 /* create servo objects */
 Servo servo_S1(PB_2);
 Servo servo_S2(PC_8);
 // Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
-int Ts_pwm_mus = 20000;
-int servoOutput_mus_S1 = 0;
-int servoOutput_mus_S2 = 0;
+int servo_position_S1_mus = 0;
+int servo_position_S2_mus = 0;
+int servo_period_mus = 20000;
 int servo_counter = 0;
-int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
+int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)loop_period_ms)));
+
 
 // Groove Ultrasonic Ranger V2.0
-// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 
+// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds
 /* create range finder object (ultra sonic distance sensor) */
-RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
-// RangeFinder rangefinder(PB_6, 5782.0f, 0.02f,  7000); // 1/Ts_ms = 50 Hz parametrization
-float       dist_USSensor = 0.0f;
+// RangeFinder range_finder(PC_6, 5782.0f, 0.02f, 17500); // 1/loop_period_ms = 20 Hz parametrization
+RangeFinder range_finder(PB_6, 5782.0f, 0.02f,  7000); // 1/loop_period_ms = 50 Hz parametrization
+float       dist_us_sensor = 0.0f;
+
 
 int main()
 {
+    /* attach button fall and rise functions */
     user_button.fall(&button_fall);
     user_button.rise(&button_rise);
+
+    /* start loop_timer */
     loop_timer.start();
 
-    /* enable hardwaredriver dc motors */
+    /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */
     enable_motors = 1;
-    /* initialize pwm for motor M3*/
-    pwmOut_M3.period(Ts_pwm_s);
-    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
-    pwmOut_M3.write(0.5);
+
+    /* initialize pwm for motor M1*/
+    pwm_M1.period(pwm_period_s);
+    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */
+    pwm_M1.write(0.5);
 
     /* enable servos, you can also disable them */
-    servo_S1.Enable(servoOutput_mus_S1, Ts_pwm_mus);
-    servo_S2.Enable(servoOutput_mus_S2, Ts_pwm_mus);
+    servo_S1.Enable(servo_position_S1_mus, servo_period_mus);
+    servo_S2.Enable(servo_position_S2_mus, servo_period_mus);
 
     while (true) {
 
         loop_timer.reset();
 
-        /* ------------- start hacking ------------- -------------*/
-
-        if (executeMainTask) {
+        if (do_execute_main) {
 
             /* read analog input */
-            dist_IRSensor = analogIn.read() * 3.3f;
+            dist_ir_sensor = analog_in.read() * 3.3f;
+
+            /* write output voltage to motor M1 */
+            pwm_M1.write(0.75);
 
-            /* command a speed to dc motors M1 and M2*/
-            speedController_M1.setDesiredSpeedRPS( 1.0f);
-            positionController_M2.setDesiredRotation(0.5f, max_speed_rps);
-            /* write output voltage to motor M3 */
-            pwmOut_M3.write(0.75);
+            /* set a desired speed for speed controlled dc motors M2 */
+            speedController_M2.setDesiredSpeedRPS(0.5f);
+
+            /* set a desired rotation for position controlled dc motors M3 */
+            positionController_M3.setDesiredRotation(1.5f, max_speed_rps);
+
 
             /* command servo position via output time, this needs to be calibrated */
-            servo_S1.SetPosition(servoOutput_mus_S1);
-            servo_S2.SetPosition(servoOutput_mus_S2);
-            if (servoOutput_mus_S1 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
-                servoOutput_mus_S1 += 100;
+            servo_S1.SetPosition(servo_position_S1_mus);
+            servo_S2.SetPosition(servo_position_S2_mus);
+            if (servo_position_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
+                servo_position_S1_mus += 100;
             }
-            if (servoOutput_mus_S2 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
-                servoOutput_mus_S2 += 100;
+            if (servo_position_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
+                servo_position_S2_mus += 100;
             }
             servo_counter++;
 
             /* read ultra sonic distance sensor */
-            dist_USSensor = rangeFinder.read_cm();
+            dist_us_sensor = range_finder.read_cm();
 
             /* visual feedback that the main task is executed */
             led = !led;
 
         } else {
 
-            dist_IRSensor = 0.0f;
+            dist_ir_sensor = 0.0f;
 
-            speedController_M1.setDesiredSpeedRPS(0.0f);
-            positionController_M2.setDesiredRotation(0.0f, max_speed_rps);
-            pwmOut_M3.write(0.5);
+            pwm_M1.write(0.5);
+            speedController_M2.setDesiredSpeedRPS(0.0f);
+            positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
 
-            servoOutput_mus_S1 = 0;
-            servoOutput_mus_S2 = 0;
-            servo_S1.SetPosition(servoOutput_mus_S1);
-            servo_S2.SetPosition(servoOutput_mus_S2);
-            
-            dist_USSensor = 0.0f;
+            servo_position_S1_mus = 0;
+            servo_position_S2_mus = 0;
+            servo_S1.SetPosition(servo_position_S1_mus);
+            servo_S2.SetPosition(servo_position_S2_mus);
+
+            dist_us_sensor = 0.0f;
 
             led = 0;
         }
 
         /* do only output via serial what's really necessary (this makes your code slow)*/
-        printf("%3.3f, %3d, %3d, %3.3f, %3.3f, %3d, %3.3f;\r\n",
-               dist_IRSensor,
-               servoOutput_mus_S1,
-               servoOutput_mus_S2,
-               speedController_M1.getSpeedRPS(),
-               positionController_M2.getRotation(),
-               encoderCounter_M3.read(),
-               dist_USSensor);
-               
-        /* ------------- stop hacking ------------- -------------*/
+        printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n",
+               dist_ir_sensor * 1e3,
+               encoder_M1.read(),
+               speedController_M2.getSpeedRPS(),
+               positionController_M3.getRotation(),
+               servo_position_S1_mus,
+               servo_position_S2_mus,
+               dist_us_sensor);
 
-        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);
+
+        /* ------------- stop hacking ----------------------------------------*/
+
+        int loop_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(loop_timer.elapsed_time()).count();
+        thread_sleep_for(loop_period_ms - loop_time_ms);
     }
 }
 
@@ -163,9 +173,9 @@
 
 void button_rise()
 {
-    int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
+    int user_button_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
     user_button_timer.stop();
-    if (t_button_ms > 200) {
-        executeMainTask = !executeMainTask;
+    if (user_button_time_ms > 200) {
+        do_execute_main = !do_execute_main;
     }
 }
\ No newline at end of file