Diff: main.cpp

Revision:

42:b54a4f294aa9

Parent:

41:d8067ab9def5

Child:

43:5648b7083fe5

--- a/main.cpp	Thu May 05 18:32:17 2022 +0200
+++ b/main.cpp	Fri May 06 10:33:36 2022 +0200
@@ -51,23 +51,19 @@
 I2C i2c(PB_9, PB_8); // I2C (PinName sda, PinName scl)
 SensorBar sensor_bar(i2c, 0.1175f);
 
+// transformations and stuff
 float r_wheel = 0.0358f / 2.0f;
 float L_wheel = 0.143f;
-// transform wheel to robot
-Eigen::Matrix<float, 2, 2> Crw;
-Eigen::Matrix<float, 2, 2> Cwr;
-Eigen::Matrix<float, 2, 2> I;
+Eigen::Matrix<float, 2, 2> Cwheel2robot; // transform wheel to robot
+Eigen::Matrix<float, 2, 2> Crobot2wheel; // transform robot to wheel
+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);
 
 int main()
 {    
-    Crw << r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
-           r_wheel / L_wheel, -r_wheel / L_wheel;
-    Cwr << 1.0f / r_wheel,  L_wheel / (2.0f * r_wheel),
-           1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
-    I = Crw * Cwr;
-    printf("%f, %f, %f, %f\r\n", I(0,0), I(0,1), I(1,0), I(1,1));
-
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -78,17 +74,48 @@
     // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
     enable_motors = 1;
 
+    // initialise matrizes and vectros
+    Cwheel2robot << r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
+                    r_wheel / L_wheel, -r_wheel / L_wheel;
+    Crobot2wheel << 1.0f / r_wheel,  L_wheel / (2.0f * r_wheel),
+                    1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
+    robot_coord << 0.06f, 0.0f;
+    wheel_speed << 0.0f, 0.0f;
+
     while (true) { // this loop will run forever
 
         main_task_timer.reset();
 
         if (do_execute_main_task) {
 
+            // read SensorBar
+            float sensor_bar_avgAngleRad = 0.0f;
+            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);
+
+            // 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(0.5f); // set a desired speed for speed controlled dc motors M2
-            speedController_M2.setDesiredSpeedRPS(0.5f); // set a desired speed for speed controlled dc motors M2
+            //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
 
             /*
             uint8_t sensor_bar_raw_value = sensor_bar.getRaw();
@@ -97,7 +124,8 @@
             }
             printf(", ");
             */
-
+            
+            /*
             int8_t sensor_bar_binaryPosition = sensor_bar.getBinaryPosition();       
             printf("%d, ", sensor_bar_binaryPosition);
 
@@ -111,7 +139,10 @@
                 sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad();
             }
             printf("%f, ", sensor_bar_angleRad * 180.0f / M_PI);
-            printf("%f\r\n", sensor_bar_avgAngleRad * 180.0f / M_PI);
+            printf("%f, ", sensor_bar_avgAngleRad * 180.0f / M_PI);
+            */
+
+            printf("%f, %f\r\n", wheel_speed(0) / (2.0f * M_PI), wheel_speed(1) / (2.0f * M_PI));
 
         } else {
 
@@ -146,4 +177,22 @@
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
     }
+}
+
+float fcn_vel_cntrl(const float& vel_max, const float& vel_min, const float& ang_max, const float& ang)
+{
+    const static float gain = (vel_min - vel_max) / ang_max;
+    const static float offset = vel_max;
+    return gain * fabs(ang) + offset;
+}
+
+float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& ang)
+{
+    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;
+    }
+    return retval;
 }
\ No newline at end of file