Diff: Robot_Library/robot.cpp

Revision:

67:90f378806cbe

Parent:

60:85a40e69ced6

Child:

68:61aad0993a29

--- a/Robot_Library/robot.cpp	Mon May 30 09:34:12 2022 +0200
+++ b/Robot_Library/robot.cpp	Mon May 30 10:29:41 2022 +0200
@@ -61,6 +61,9 @@
 void Robot::Update() {
 
   controller.Update();
+  
+  wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
+  robot_speed_actual = wheel_to_robot * wheel_speed_actual;
 
   printf("STATE: %d \r\n", state);
   switch (state) {
@@ -82,6 +85,17 @@
   default:
     state = IDLE; // on default, stop the car
   }
+  
+  wheel_speed_desired = robot_to_wheel * robot_speed_desired;
+
+  // smooth desired wheel_speeds
+  trajectoryPlanners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), controller.Period());
+  trajectoryPlanners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), controller.Period());
+  wheel_speed_smooth << trajectoryPlanners[0]->getVelocity(), trajectoryPlanners[1]->getVelocity();
+
+  // command speedController objects
+  speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0));
+  speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1));
 }
 
 void Robot::Initial() {