Revision 48:72bba06c3680, committed 2022-05-19

Comitter:

huels035

Date:

Thu May 19 04:51:50 2022 -0500

Parent:

47:31726ce58a6d

Commit message:

speed control basic

Changed in this revision

--- a/main.cpp	Thu May 19 02:23:27 2022 -0500
+++ b/main.cpp	Thu May 19 04:51:50 2022 -0500
@@ -50,8 +50,8 @@
     speedControllerM1.setSpeedCntrlGain(0.1f/3);
     speedControllerM2.setSpeedCntrlGain(0.1f/3);
 
-    speedControllerM1.setMaxAccelerationRPM(999999.0f);
-    speedControllerM2.setMaxAccelerationRPM(999999.0f);
+    speedControllerM1.setMaxAccelerationRPS(10.0f);
+    speedControllerM2.setMaxAccelerationRPS(10.0f);
 
 
     enable_motors = 1;
@@ -61,6 +61,24 @@
     float ir_distance_cm = 0.0f;
     float distAxisToSensor = 0.12f;
 
+    const float r_wheel = 0.0358f/ 2.0f;
+    const float L_wheel = 0.143f;
+    Eigen::Matrix2f Cwheel2robot;
+
+    Cwheel2robot << r_wheel/2.0f , r_wheel/2.0f ,
+                    r_wheel/L_wheel, -r_wheel/L_wheel;
+
+    Eigen::Vector2f robot_coord;
+    Eigen::Vector2f wheel_speed;
+    Eigen::Vector2f actual_wheel_speed;
+    Eigen::Vector2f actual_robot_coord;
+    Eigen::Vector2f wheel_speed_error;
+
+    robot_coord.setZero();
+    wheel_speed.setZero();
+    wheel_speed_error.setZero();
+
+    
     //I2C i2c(PB_9, PB_8);
 
     //SensorBar light_sensor(i2c, distAxisToSensor);
@@ -84,15 +102,33 @@
         if (do_execute_main_task) {
             //pwm_M1.write(.75f);
             //pwm_M2.write(.75f);
-            speedControllerM1.setDesiredSpeedRPS(1.5f); // max 3
-            speedControllerM2.setDesiredSpeedRPS(1.5f);
+            //speedControllerM1.setDesiredSpeedRPS(3.0f*0.75f); // max 3
+           // speedControllerM2.setDesiredSpeedRPS(3.0f*0.75f);
+           robot_coord << 0.2f, 0.0f;
+
         } else {
+            robot_coord << 0.0f, 0.0f;
             //pwm_M1.write(.5f);
             //pwm_M2.write(.5f);
-            speedControllerM1.setDesiredSpeedRPS(0.0f);
-            speedControllerM2.setDesiredSpeedRPS(0.0f);
+            //speedControllerM1.setDesiredSpeedRPS(0.0f);
+            //speedControllerM2.setDesiredSpeedRPS(0.0f);
         } 
+
+        //robot_coord(0) = 0.2f;
         
+        wheel_speed = Cwheel2robot.inverse()*robot_coord;
+        
+        speedControllerM1.setDesiredSpeedRPS((wheel_speed(0) + wheel_speed_error(0))/(2*M_PI)); // max 3
+        speedControllerM2.setDesiredSpeedRPS((wheel_speed(1) + wheel_speed_error(1))/(2*M_PI));
+
+        actual_wheel_speed[0] = 2*M_PI*speedControllerM1.getSpeedRPS();
+        actual_wheel_speed[1] = 2*M_PI*speedControllerM2.getSpeedRPS();
+ 
+        wheel_speed_error = wheel_speed - actual_wheel_speed;
+        //printf("Desired: %f, %f \r\n", wheel_speed(0)/(2*M_PI), wheel_speed(1)/(2*M_PI));
+        //printf("Actual: %f, %f \r\n", speedControllerM1.getSpeedRPS(), speedControllerM2.getSpeedRPS());    
+        printf("%f %f \r\n", wheel_speed_error[0], wheel_speed_error[1]);
+        actual_robot_coord = Cwheel2robot*actual_wheel_speed;
 
         // user_led is switching its state every second
         if ( (main_task_cntr%(1000 / main_task_period_ms) == 0) && (main_task_cntr!=0) ) {
@@ -104,7 +140,7 @@
         //printf("IR sensor (cm): %f \r\n", ir_distance_cm);
 
 
-        printf("%f \r\n", speedControllerM2.getSpeedRPS());   
+        //printf("%f \r\n", speedControllerM2.getSpeedRPS());   
         
         
         // do only output via serial what's really necessary (this makes your code slow)