Diff: main.cpp

Revision:

46:41c9367da539

Parent:

45:d9e6e89210f9

Child:

47:6693bffcdfd0

--- a/main.cpp	Sat May 14 17:32:16 2022 +0200
+++ b/main.cpp	Mon May 16 11:07:26 2022 +0200
@@ -83,13 +83,27 @@
     const float counts_per_turn = 64.0f * 19.0f;    // define counts per turn at gearbox end: counts/turn * gearratio
     const float kn = 530.0f / 12.0f;                // define motor constant in rpm per V
     
+    // create Motion objects (trajectory planner)
+    Motion* trajectoryPlaners[2];
+    trajectoryPlaners[0] = new Motion;
+    trajectoryPlaners[1] = new Motion;
+    trajectoryPlaners[0]->setProfileVelocity(max_voltage * kn / 60.0f);
+    trajectoryPlaners[1]->setProfileVelocity(max_voltage * kn / 60.0f);
+    trajectoryPlaners[0]->setProfileAcceleration(10.0f);
+    trajectoryPlaners[1]->setProfileAcceleration(10.0f);
+    trajectoryPlaners[0]->setProfileDeceleration(10.0f);
+    trajectoryPlaners[1]->setProfileDeceleration(10.0f);
+
+    // create SpeedController objects
     SpeedController* speedControllers[2];
     speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
     speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
     speedControllers[0]->setSpeedCntrlGain(0.04f);      // adjust speedcontroller gains
     speedControllers[1]->setSpeedCntrlGain(0.04f);
-    speedControllers[0]->setMaxAccelerationRPS(10.0f);  // adjust max. acceleration for smooth movement
-    speedControllers[1]->setMaxAccelerationRPS(10.0f);
+    //speedControllers[0]->setMaxAccelerationRPS(10.0f);  // use this if you're not using the trajectoryPlaners
+    //speedControllers[1]->setMaxAccelerationRPS(10.0f);
+    speedControllers[0]->setMaxAccelerationRPS(999.0f);  // adjust max. acceleration for smooth movement
+    speedControllers[1]->setMaxAccelerationRPS(999.0f);
 
     // robot kinematics
     const float r_wheel = 0.0766f / 2.0f; // wheel radius
@@ -100,14 +114,16 @@
                     -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);
-    Eigen::Vector2f robot_coord;  // contains v and w (robot translational and rotational velocities)
-    Eigen::Vector2f wheel_speed;  // w1 w2 (wheel speed)
+    Eigen::Vector2f robot_coord;         // contains v and w (robot translational and rotational velocities)
+    Eigen::Vector2f wheel_speed;         // w1 w2 (wheel speed)
+    Eigen::Vector2f wheel_speed_smooth;  // w1 w2 (wheel speed)
+    Eigen::Vector2f robot_coord_actual;
     Eigen::Vector2f wheel_speed_actual;
-    Eigen::Vector2f robot_coord_actual;
     robot_coord.setZero();
     wheel_speed.setZero();
+    wheel_speed_smooth.setZero();
+    robot_coord_actual.setZero();
     wheel_speed_actual.setZero();
-    robot_coord_actual.setZero();
 
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
@@ -208,9 +224,16 @@
         // transform robot coordinates to wheel speed
         wheel_speed = Cwheel2robot.inverse() * robot_coord;
 
+        // smooth desired wheel_speeds
+        trajectoryPlaners[0]->incrementToVelocity(wheel_speed(0) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
+        trajectoryPlaners[1]->incrementToVelocity(wheel_speed(1) / (2.0f * M_PI), static_cast<float>(main_task_period_ms) * 1.0e-3f);
+        wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
+
         // command speedController objects
-        speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
-        speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
+        //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
+        //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // use this if you're not using the trajectoryPlaners
+        speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0)); // set a desired speed for speed controlled dc motors M1
+        speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1)); // set a desired speed for speed controlled dc motors M2
         
         user_led = !user_led;