Diff: main.cpp

Revision:

43:5648b7083fe5

Parent:

42:b54a4f294aa9

Child:

44:340cdc4b6e47

--- a/main.cpp	Fri May 06 10:33:36 2022 +0200
+++ b/main.cpp	Sat May 07 12:50:49 2022 +0200
@@ -45,7 +45,7 @@
 
 SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1); // default 78.125:1 gear box  with default contoller parameters
 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
+//SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 100:1 gear
 
 // sparkfun line follower array
 I2C i2c(PB_9, PB_8); // I2C (PinName sda, PinName scl)
@@ -59,8 +59,9 @@
 Eigen::Matrix<float, 2, 1> robot_coord;  // contains v and w
 Eigen::Matrix<float, 2, 1> wheel_speed;  // w1 w2
 
-float fcn_vel_cntrl(const float& vel_max, const float& vel_min, const float& ang_max, const float& ang);
-float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& ang);
+float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& angle);
+float fcn_vel_cntrl_v1(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle);
+float fcn_vel_cntrl_v2(float wheel_speed_max, float b, float robot_omega);
 
 int main()
 {    
@@ -89,31 +90,36 @@
         if (do_execute_main_task) {
 
             // read SensorBar
-            float sensor_bar_avgAngleRad = 0.0f;
+            static float sensor_bar_avgAngleRad = 0.0f; // by making this static it will not be overwritten (only fist time set to zero)
             if (sensor_bar.isAnyLedActive()) {
                 sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad();
             }
 
-            // proportional controller for angle
-            //robot_coord(1) = 3.0f * sensor_bar_avgAngleRad;
-            // robot_coord(0) = fcn_vel_cntrl(0.10f, 0.02f, 27.0f * M_PI / 180.0f, sensor_bar_avgAngleRad);
-            // robot_coord(1) = fcn_ang_cntrl(2.0f, 5.0f, sensor_bar_avgAngleRad);
-            const static float vel_max = 0.30f; //0.10f;
-            const static float vel_min = 0.02f; //0.02f;
-            const static float ang_max = 27.0f * M_PI / 180.0f;
-            robot_coord(0) = fcn_vel_cntrl(vel_max, vel_min, ang_max, sensor_bar_avgAngleRad);
             const static float Kp = 2.0f; //2.0f;
             const static float Kp_nl = 17.0f; //10.0f; //5.0f;
             robot_coord(1) = fcn_ang_cntrl(Kp, Kp_nl, sensor_bar_avgAngleRad);
 
+            // nonlinear controllers version 1 (whatever came to my mind)
+            /*
+            const static float vel_max = 0.3374f; //0.10f;
+            const static float vel_min = 0.00f; //0.02f;
+            const static float ang_max = 27.0f * M_PI / 180.0f;
+            robot_coord(0) = fcn_vel_cntrl_v1(vel_max, vel_min, ang_max, sensor_bar_avgAngleRad);
+            */
+
+            // nonlinear controllers version 2 (one wheel always at full speed controller)
+            ///*
+            static float wheel_speed_max = max_voltage * kn / 60.0f * 2.0f * M_PI;
+            static float b = L_wheel / (2.0f * r_wheel);
+            robot_coord(0) = fcn_vel_cntrl_v2(wheel_speed_max, b, robot_coord(1));
+            //*/
+
             // transform to robot coordinates
             wheel_speed = Crobot2wheel * robot_coord;
 
             // read analog input
             ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
 
-            //speedController_M1.setDesiredSpeedRPS(1.0f); // set a desired speed for speed controlled dc motors M1
-            //speedController_M2.setDesiredSpeedRPS(1.0f); // set a desired speed for speed controlled dc motors M2
             speedController_M1.setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
             speedController_M2.setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
 
@@ -179,20 +185,35 @@
     }
 }
 
-float fcn_vel_cntrl(const float& vel_max, const float& vel_min, const float& ang_max, const float& ang)
+float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& angle)
+{
+    float retval = 0.0f;
+    if (angle > 0) {
+        retval = Kp * angle + Kp_nl * angle * angle;
+    } else if (angle < 0) {
+        retval = Kp * angle - Kp_nl * angle * angle;
+    }
+    return retval;
+}
+
+float fcn_vel_cntrl_v1(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle)
 {
     const static float gain = (vel_min - vel_max) / ang_max;
     const static float offset = vel_max;
-    return gain * fabs(ang) + offset;
+    return gain * fabs(angle) + offset;
 }
 
-float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& ang)
+float fcn_vel_cntrl_v2(float wheel_speed_max, float b, float robot_omega)
 {
-    float retval = 0.0f;
-    if (ang > 0) {
-        retval = Kp * ang + Kp_nl * ang * ang;
-    } else if (ang < 0) {
-        retval = Kp * ang - Kp_nl * ang * ang;
+    static Eigen::Matrix<float, 2, 2> _wheel_speed;
+    static Eigen::Matrix<float, 2, 2> _robot_coord;
+    if (robot_omega > 0) {
+        _wheel_speed(0) = wheel_speed_max;
+        _wheel_speed(1) = wheel_speed_max - 2*b*robot_omega;
+    } else {
+        _wheel_speed(0) = wheel_speed_max + 2*b*robot_omega;
+        _wheel_speed(1) = wheel_speed_max;
     }
-    return retval;
+    _robot_coord = Cwheel2robot * _wheel_speed;
+    return _robot_coord(0);
 }
\ No newline at end of file