Whose Line is it Anyways / Mbed OS PM2_WLiiA_robot_code

robot code for summer school

Dependencies:   PM2_Libary Eigen

Fork of PM2_Example_Summer_School by Alex Hawkins

Diff: Robot_Library/robot.cpp

Branch:
a-star
Revision:
71:350ae163ffa5
Parent:
66:d9f2bcf77e52
--- a/Robot_Library/robot.cpp	Mon May 30 10:27:39 2022 +0200
+++ b/Robot_Library/robot.cpp	Mon May 30 11:20:08 2022 +0200
@@ -85,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() {