Saving current tuning progress, debugging and extraniouse changes are present

do not use at this commit

Author: jschlind411

src/abridge/src/abridge.cpp

@@ -163,7 +163,7 @@ void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {
 
   // Cap motor commands at 120. Experimentally determined that high values (tested 180 and 255) can cause 
   // the hardware to fail when the robot moves itself too violently.
-  int max_motor_cmd = 180;
+  int max_motor_cmd = 255;
 
   if (currentMode == 1)
   {

src/behaviours/src/DriveController.cpp

@@ -220,7 +220,7 @@ void DriveController::ProcessData()
 
 void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel, float setPointYaw) {
 
-  float velOut = fastVelPID.PIDOut(errorVel, setPointVel);
+  float velOut = 0;//fastVelPID.PIDOut(errorVel, setPointVel);
   float yawOut = fastYawPID.PIDOut(errorYaw, setPointYaw);
 
   int left = velOut - yawOut;
@@ -238,7 +238,7 @@ void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel
 
 void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw) {
 
-  float velOut = slowVelPID.PIDOut(errorVel, setPointVel);
+  float velOut = 0;//slowVelPID.PIDOut(errorVel, setPointVel);
   float yawOut = slowYawPID.PIDOut(errorYaw, setPointYaw);
 
   int left = velOut - yawOut;
@@ -306,9 +306,9 @@ PIDConfig DriveController::fastVelConfig() {
 PIDConfig DriveController::fastYawConfig() {
   PIDConfig config;
 
-  config.Kp = 240;
-  config.Ki = 25;
-  config.Kd = 0.5;
+  config.Kp = 100;
+  config.Ki = 10;
+  config.Kd = 14;
   config.satUpper = 255;
   config.satLower = -255;
   config.antiWindup = config.satUpper/2;
@@ -350,9 +350,9 @@ PIDConfig DriveController::slowVelConfig() {
 PIDConfig DriveController::slowYawConfig() {
   PIDConfig config;
 
-  config.Kp = 80;
-  config.Ki = 15;
-  config.Kd = 1.7;
+  config.Kp = 70;
+  config.Ki = 16;
+  config.Kd = 10;
   config.satUpper = 255;
   config.satLower = -255;
   config.antiWindup = config.satUpper/4;

src/behaviours/src/LogicController.cpp

@@ -154,10 +154,14 @@ Result LogicController::DoWork() {
     //used for precision driving pass through
   case LOGIC_STATE_PRECISION_COMMAND: {
 
+//  cout << "I MADE IT HERE!" << endl;
+
     //unlike waypoints precision commands change every update tick so we ask the
     //controller for new commands on every update tick.
     result = control_queue.top().controller->DoWork();
 
+ //  cout << result.pd.cmdAngularError;
+
     //pass the driving commands to the drive controller so it can interpret them
     driveController.SetResultData(result);
 
@@ -191,7 +195,7 @@ void LogicController::ProcessData() {
   if (processState == PROCCESS_STATE_SEARCHING) {
     prioritizedControllers = {
       PrioritizedController{0, (Controller*)(&searchController)},
-      PrioritizedController{10, (Controller*)(&obstacleController)},
+      PrioritizedController{-1, (Controller*)(&obstacleController)},
       PrioritizedController{15, (Controller*)(&pickUpController)},
       PrioritizedController{5, (Controller*)(&range_controller)},
       PrioritizedController{-1, (Controller*)(&dropOffController)}

src/behaviours/src/PID.cpp

@@ -105,20 +105,25 @@ float PID::PIDOut(float calculatedError, float setPoint) {
   if (fabs(P) < config.antiWindup)
   {
     float avgPrevError = 0;
-    for (int i = 1; i < Error.size(); i++) {
+    for (int i = 1; i < Error.size(); i++)
+    {
       avgPrevError += Error[i];
     }
-    if (Error.size() > 1) {
+    if (Error.size() > 1)
+    {
       avgPrevError /= Error.size()-1;
     }
-    else {
+    else
+    {
       avgPrevError = Error[0];
     }
 
-    D = config.Kd * (((1-config.derivativeAlpha)*Error[0]) - (config.derivativeAlpha * avgPrevError));
+    D = config.Kd * (Error[0] - Error[1]) * hz;
   }
 
   float PIDOut = P + I + D + FF;
+
+  cout << "PID: " << "P:" << P << "  I:" << I << "  D:" << D << endl;
   if (PIDOut > config.satUpper) //cap vel command
   {
     PIDOut = config.satUpper;

src/behaviours/src/PID.h

@@ -44,7 +44,7 @@ class PID
   float prevSetPoint = std::numeric_limits<float>::min();
   vector<float> integralErrorHistArray;
   int step = 0;
-  float hz = 10;
+  float hz = 10;	//Rate that PID is running at
 
 };
 

src/behaviours/src/SearchController.cpp

@@ -1,4 +1,5 @@
 #include "SearchController.h"
+#include <angles/angles.h>
 
 SearchController::SearchController() {
   rng = new random_numbers::RandomNumberGenerator();
@@ -24,7 +25,14 @@ void SearchController::Reset() {
  */
 Result SearchController::DoWork() {
 
-  if (!result.wpts.waypoints.empty()) {
+    result.type = precisionDriving;
+    result.pd.cmdAngularError = angles::shortest_angular_distance(currentLocation.theta, 0);
+    result.pd.cmdVel = 0;
+    result.PIDMode = SLOW_PID;
+
+    cout << "PID:  " << result.pd.cmdAngularError << endl;
+
+/*  if (!result.wpts.waypoints.empty()) {
     if (hypot(result.wpts.waypoints[0].x-currentLocation.x, result.wpts.waypoints[0].y-currentLocation.y) < 0.10) {
       attemptCount = 0;
     }
@@ -63,9 +71,9 @@ Result SearchController::DoWork() {
 
     result.wpts.waypoints.clear();
     result.wpts.waypoints.insert(result.wpts.waypoints.begin(), searchLocation);
-
+    */
     return result;
-  }
+  //}
 
 }