Diff: main.cpp

Revision:

30:1e8295770bc1

Parent:

29:d6f1ccf42a31

Child:

31:12faf1ba9bc7

--- a/main.cpp	Fri Mar 18 09:44:52 2022 +0100
+++ b/main.cpp	Tue Mar 22 10:45:49 2022 +0100
@@ -32,41 +32,41 @@
 
 float   pwm_period_s = 0.00005f;    // define pwm period time in seconds and create FastPWM objects to command dc motors
 FastPWM pwm_M1(PB_13);              // motor M1 is used open loop
-FastPWM pwm_M2(PA_9);               // motor M2 is speed controlled
-FastPWM pwm_M3(PA_10);              // motor M3 is position controlled (angle controlled)
+FastPWM pwm_M2(PA_9);               // motor M2 is closed-loop speed controlled (angle velocity)
+FastPWM pwm_M3(PA_10);              // motor M3 is closed-loop position controlled (angle controlled)
 
-EncoderCounter  encoder_M1(PA_6, PC_7); // create encoder read objects
+EncoderCounter  encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
 EncoderCounter  encoder_M2(PB_6, PB_7);
 EncoderCounter  encoder_M3(PA_0, PA_1);
 
-// create SpeedController and PositionController controller objects, default parametrization is for 78.125:1 gear box
+// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box
 float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
 float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
 float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
-// float k_gear = 25.0f / 78.125f;             // define additional ratio in case you are using a dc motor with a different gear box, e.g. 25:1
-// float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1
+float k_gear = 100.0f / 78.125f;            // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1
+float kp = 0.1f;                            // define custom kp, 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 box  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
+// SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box  with default contoller parameters
+SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 100:1 gear
 
 float max_speed_rps = 0.5f;                 // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage
-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
+// 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 100:1 gear, we need a different speed controller gain here
 
 // Futaba Servo S3001 20mm 3kg Analog
 Servo servo_S1(PB_2);           // create servo objects
 Servo servo_S2(PC_8);
-int servo_pos_S1_mus = 0;       // servo S1 position, the desired position gets commanded as a time
-int servo_pos_S2_mus = 0;       // servo S2 position
+float servo_S1_angle = 0;       // servo S1 normalized angle
+float servo_S2_angle = 0;       // servo S2 normalized angle
 int servo_period_mus = 20000;   // define servo period time in mus
 
 int servo_counter = 0;          // define servo counter, this is an additional variable to make the servos move
-int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms))); // define loops per second
+int loops_per_seconds = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms))); // define loops per second
 
 // Groove Ultrasonic Ranger V2.0
 float us_distance_cm = 0.0f;    // define variable to store measurement
-RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 17500);
-// RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f,  7000); // create range finder object (ultra sonic distance sensor), 1/main_task_period_ms = 20 Hz parametrization
+RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 17500); // create range finder object (ultra sonic distance sensor), 20 Hz parametrization
+// RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f,  7000); // create range finder object (ultra sonic distance sensor), 50 Hz parametrization
 
 // LSM9DS1 IMU, carefull: not all PES boards have an imu (chip shortage)
 // LSM9DS1 imu(PC_9, PA_8); // create LSM9DS1 comunication object, if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h"
@@ -88,11 +88,11 @@
 
     // motor M1 is used open-loop, we need to initialize the pwm and set pwm output to zero at the beginning, range: 0...1 -> u_min...u_max: 0.5 -> 0 V
     pwm_M1.period(pwm_period_s);
-    pwm_M1.write(0.5);
+    pwm_M1.write(0.5f);
 
     // enable servos, you can also disable them at any point in your program if you don't want your servos to become warm
-    servo_S1.Enable(servo_pos_S1_mus, servo_period_mus);
-    servo_S2.Enable(servo_pos_S2_mus, servo_period_mus);
+    servo_S1.Enable(servo_S1_angle, servo_period_mus);
+    servo_S2.Enable(servo_S2_angle, servo_period_mus);
 
     while (true) { // this loop will run forever
 
@@ -105,23 +105,26 @@
 
             // command dc motors if mechanical button is pressed
             if (mechanical_button.read()) {
-                pwm_M1.write(0.75); // write output voltage to motor M1
+                pwm_M1.write(0.75f); // write output voltage to motor M1
                 speedController_M2.setDesiredSpeedRPS(0.5f); // set a desired speed for speed controlled dc motors M2
                 positionController_M3.setDesiredRotation(1.5f, max_speed_rps); // set a desired rotation for position controlled dc motors M3
             } else {
-                pwm_M1.write(0.5);
+                pwm_M1.write(0.5f);
                 speedController_M2.setDesiredSpeedRPS(0.0f);
                 positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
             }
 
+            // check if servos are enabled
+            if (!servo_S1.isEnabled()) servo_S1.Enable(servo_S1_angle, servo_period_mus);
+            if (!servo_S2.isEnabled()) servo_S2.Enable(servo_S2_angle, servo_period_mus);
             // command servo position, this needs to be calibrated
-            servo_S1.SetPosition(servo_pos_S1_mus);
-            servo_S2.SetPosition(servo_pos_S2_mus);
-            if (servo_pos_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
-                servo_pos_S1_mus += 100;
+            servo_S1.SetPosition(servo_S1_angle);
+            if (servo_S1_angle <= 1.0f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) {
+                servo_S1_angle += 0.01f;
             }
-            if (servo_pos_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
-                servo_pos_S2_mus += 100;
+            servo_S2.SetPosition(servo_S2_angle);
+            if (servo_S2_angle <= 1.0f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) {
+                servo_S2_angle += 0.01f;
             }
             servo_counter++;
 
@@ -135,14 +138,16 @@
 
             ir_distance_mV = 0.0f;
 
-            pwm_M1.write(0.5);
+            pwm_M1.write(0.5f);
             speedController_M2.setDesiredSpeedRPS(0.0f);
             positionController_M3.setDesiredRotation(0.0f, max_speed_rps);
 
-            servo_pos_S1_mus = 0;
-            servo_pos_S2_mus = 0;
-            servo_S1.SetPosition(servo_pos_S1_mus);
-            servo_S2.SetPosition(servo_pos_S2_mus);
+            servo_S1_angle = 0;
+            servo_S2_angle = 0;
+            // servo_S1.SetPosition(servo_S1_angle);
+            // servo_S2.SetPosition(servo_S2_angle);
+            if (servo_S1.isEnabled()) servo_S1.Disable();
+            if (servo_S2.isEnabled()) servo_S2.Disable();
 
             us_distance_cm = 0.0f;
 
@@ -152,13 +157,13 @@
         user_led = !user_led;
 
         // do only output via serial what's really necessary (this makes your code slow)
-        printf("IR sensor (mV): %3.3f, Encoder M1: %3d, Speed M2 (rps) %3.3f, Position M3 (rot): %3.3f, Servo S1 position (ms): %3d, Servo S2 position (ms): %3d, US sensor (cm): %3.3f\r\n",
+        printf("IR sensor (mV): %3.3f, Encoder M1: %3d, Speed M2 (rps) %3.3f, Position M3 (rot): %3.3f, Servo S1 angle (normalized): %3.3f, Servo S2 angle (normalized): %3.3f, US sensor (cm): %3.3f\r\n",
                ir_distance_mV,
                encoder_M1.read(),
                speedController_M2.getSpeedRPS(),
                positionController_M3.getRotation(),
-               servo_pos_S1_mus,
-               servo_pos_S2_mus,
+               servo_S1_angle,
+               servo_S2_angle,
                us_distance_cm);
 
         // read out the imu, the actual frames of the sensor reading needs to be figured out