Merge pull request #150 from BCLab-UNM/Drive_commenting

Drive commenting

Author: gmfricke

src/behaviours/src/DriveController.cpp

@@ -28,12 +28,14 @@ void DriveController::Reset()
 
 Result DriveController::DoWork()
 {
+  
+  ///WARNING waypoint input must use FAST_PID at this point in time failure to set fast pid will result in no movment
 
   if(result.type == behavior)
   {
     if(result.b == noChange)
     {
-      //if drive controller gets a no change command it is allowed to continue its previouse action
+      //if drive controller gets a no change command it is allowed to continue its previous action
       //normally this will be to follow waypoints but it is not specified as such.
     }
 
@@ -66,7 +68,7 @@ Result DriveController::DoWork()
   {
 
   //Handlers and the final state of STATE_MACHINE are the only parts allowed to call INTERUPT
-  //This should be d one as little as possible. I Suggest to Use timeouts to set control bools false.
+  //This should be d one as little as possible. I suggest using timeouts to set control bools to false.
   //Then only call INTERUPT if bool switches to true.
   case STATE_MACHINE_PRECISION_DRIVING:
   {
@@ -79,20 +81,26 @@ Result DriveController::DoWork()
   case STATE_MACHINE_WAYPOINTS:
   {
 
-    //Handles route planning and navigation as well as makeing sure all waypoints are valid.
+    //Handles route planning and navigation as well as making sure all waypoints are valid.
 
     bool tooClose = true;
+    //while we have waypoints and they are tooClose to drive to
     while (!waypoints.empty() && tooClose)
     {
+      //check next waypoint for distance
       if (hypot(waypoints.back().x-currentLocation.x, waypoints.back().y-currentLocation.y) < waypointTolerance)
       {
+        //if too close remove it
         waypoints.pop_back();
       }
       else
       {
+        //this waypoint is far enough to be worth driving to
         tooClose = false;
       }
     }
+    
+    //if we are out of waypoints then interupt and return to logic controller
     if (waypoints.empty())
     {
       stateMachineState = STATE_MACHINE_WAITING;
@@ -102,12 +110,13 @@ Result DriveController::DoWork()
     }
     else
     {
+      //select setpoint for heading and begin driving to the next waypoint
       stateMachineState = STATE_MACHINE_ROTATE;
       waypoints.back().theta = atan2(waypoints.back().y - currentLocation.y, waypoints.back().x - currentLocation.x);
       result.pd.setPointYaw = waypoints.back().theta;
 
-      //cout << "**************************************************************************" << endl;
-      //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << endl;
+      //cout << "**************************************************************************" << endl; //DEBUGGING CODE
+      //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << endl; //DEBUGGING CODE
       //fall through on purpose
     }
   }
@@ -124,8 +133,8 @@ Result DriveController::DoWork()
     // Calculate the diffrence between current and desired heading in radians.
     float errorYaw = angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta);
 
-    //cout << "ROTATE Error yaw:  " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << endl;
-    //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl;
+    //cout << "ROTATE Error yaw:  " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << endl; //DEBUGGING CODE
+    //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl; //DEBUGGING CODE
 
     result.pd.setPointVel = 0.0;
     //Calculate absolute value of angle
@@ -163,8 +172,8 @@ Result DriveController::DoWork()
     float errorYaw = angles::shortest_angular_distance(currentLocation.theta, waypoints.back().theta);
     float distance = hypot(waypoints.back().x - currentLocation.x, waypoints.back().y - currentLocation.y);
 
-    //cout << "Skid steer, Error yaw:  " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << " error distance : " << distance << endl;
-    //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl;
+    //cout << "Skid steer, Error yaw:  " << errorYaw << " target heading : " << waypoints.back().theta << " current heading : " << currentLocation.theta << " error distance : " << distance << endl; //DEBUGGING CODE
+    //cout << "Waypoint x : " << waypoints.back().x << " y : " << waypoints.back().y << " currentLoc x : " << currentLocation.x << " y : " << currentLocation.y << endl; //DEBUGGING CODE
 
 
 
@@ -175,7 +184,7 @@ Result DriveController::DoWork()
       result.pd.setPointVel = searchVelocity;
       if (result.PIDMode == FAST_PID)
       {
-        //cout << "linear velocity:  " << linearVelocity << endl;
+        //cout << "linear velocity:  " << linearVelocity << endl; //DEBUGGING CODE
         fastPID((searchVelocity-linearVelocity) ,errorYaw, result.pd.setPointVel, result.pd.setPointYaw);
       }
     }
@@ -227,14 +236,18 @@ bool DriveController::HasWork() {   }
 
 void DriveController::ProcessData()
 {
+  //determine if the drive commands are waypoint or precision driving
   if (result.type == waypoint) {
+    
+    //sets logic controller into stand by mode while drive controller works
     result.type = behavior;
     result.b = noChange;
 
     if(result.reset) {
       waypoints.clear();
     }
 
+    //add waypoints onto stack and change state to start following them
     if (!result.wpts.waypoints.empty()) {
       waypoints.insert(waypoints.end(),result.wpts.waypoints.begin(), result.wpts.waypoints.end());
       stateMachineState = STATE_MACHINE_WAYPOINTS;
@@ -243,6 +256,7 @@ void DriveController::ProcessData()
   else if (result.type == precisionDriving)
   {
 
+    //calculate inputs into the PIDS for precision driving
     if (result.PIDMode == FAST_PID)
     {
       float vel = result.pd.cmdVel -linearVelocity;
@@ -251,17 +265,19 @@ void DriveController::ProcessData()
     }
     else if (result.PIDMode == SLOW_PID)
     {
-      //will take longer to reach the setPoint but has less chanse of an overshoot
+      //will take longer to reach the setPoint but has less chanse of an overshoot especially with slow feedback
       float vel = result.pd.cmdVel -linearVelocity;
       float setVel = result.pd.cmdVel;
       slowPID(vel,result.pd.cmdAngularError, setVel, result.pd.setPointYaw);
     }
     else if (result.PIDMode == CONST_PID)
     {
+      //vel is the same as fast PID however
+      //this setup takes a target angular velocity to constantly turn at instead of a target heading
       float vel = result.pd.cmdVel - linearVelocity;
       float angular = result.pd.cmdAngular - angularVelocity;
 
-      //cout << "Ang. Vel.  " << angularVelocity << "  Ang. Error" << angular << endl;
+      //cout << "Ang. Vel.  " << angularVelocity << "  Ang. Error" << angular << endl; //DEBUGGING CODE
 
       constPID(vel, angular ,result.pd.setPointVel, result.pd.setPointYaw);
     }
@@ -272,27 +288,27 @@ void DriveController::ProcessData()
 void DriveController::fastPID(float errorVel, float errorYaw , float setPointVel, float setPointYaw)
 {
 
-  // cout << "PID FAST" << endl;
+  // cout << "PID FAST" << endl; //DEBUGGING CODE
 
-  float velOut = fastVelPID.PIDOut(errorVel, setPointVel);
-  float yawOut = fastYawPID.PIDOut(errorYaw, setPointYaw);
+  float velOut = fastVelPID.PIDOut(errorVel, setPointVel); //returns PWM target to try and get error vel to 0
+  float yawOut = fastYawPID.PIDOut(errorYaw, setPointYaw); //returns PWM target to try and get yaw error to 0
 
-  int left = velOut - yawOut;
-  int right = velOut + yawOut;
+  int left = velOut - yawOut; //combine yaw and vel PWM values
+  int right = velOut + yawOut; //left and right are the same for vel output but opposite for yaw output
 
-  int sat = 180;
+  //prevent combine output from going over tihs value
+  int sat = 180; 
   if (left  >  sat) {left  =  sat;}
   if (left  < -sat) {left  = -sat;}
   if (right >  sat) {right =  sat;}
   if (right < -sat) {right = -sat;}
-
   this->left = left;
   this->right = right;
 }
 
 void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw)
 {
-  //cout << "PID SLOW" << endl;
+  //cout << "PID SLOW" << endl; //DEBUGGING CODE
 
   float velOut = slowVelPID.PIDOut(errorVel, setPointVel);
   float yawOut = slowYawPID.PIDOut(errorYaw, setPointYaw);
@@ -313,7 +329,7 @@ void DriveController::slowPID(float errorVel,float errorYaw, float setPointVel,
 void DriveController::constPID(float erroVel,float constAngularError, float setPointVel, float setPointYaw)
 {
 
-  //cout << "PID CONST" << endl;
+  //cout << "PID CONST" << endl; //DEBUGGING CODE
 
   float velOut = constVelPID.PIDOut(erroVel, setPointVel);
   float yawOut = constYawPID.PIDOut(constAngularError, setPointYaw);
@@ -345,20 +361,20 @@ PIDConfig DriveController::fastVelConfig()
 {
   PIDConfig config;
 
-  config.Kp = 60;
-  config.Ki = 10;
-  config.Kd = 2;
-  config.satUpper = 255;
-  config.satLower = -255;
-  config.antiWindup = config.satUpper;
-  config.errorHistLength = 4;
-  config.alwaysIntegral = true;
-  config.resetOnSetpoint = true;
+  config.Kp = 60; //proportional constant
+  config.Ki = 10; //integral constant
+  config.Kd = 2; //derivative constant
+  config.satUpper = 255; //upper limit for PID output
+  config.satLower = -255; //lower limit for PID output
+  config.antiWindup = config.satUpper; //prevent integral from acruing error untill proportional output drops below a certain limit
+  config.errorHistLength = 4; //how many time steps to average error over
+  config.alwaysIntegral = true; //should the integral alway be on or only when there is error
+  config.resetOnSetpoint = true; //reset the integral and error history whent he setpoint changes
   config.feedForwardMultiplier = 610; //gives 127 pwm at 0.4 commandedspeed  ORIG:320
-  config.integralDeadZone = 0.01;
-  config.integralErrorHistoryLength = 10000;
-  config.integralMax = config.satUpper/2;
-  config.derivativeAlpha = 0.7;
+  config.integralDeadZone = 0.01; //set the integral dead zone, prevented integral from growing or shrinking do to noise
+  config.integralErrorHistoryLength = 10000; //how many time ticks should error history should be stored for integration
+  config.integralMax = config.satUpper/2; //what is the limit of the integral output for the PID
+  config.derivativeAlpha = 0.7; //dead code not used
 
   return config;
 

src/behaviours/src/DriveController.h

@@ -24,13 +24,15 @@ class DriveController : virtual Controller
 
   Result result;
 
-  float left;
-  float right;
+  //MAX PWM is 255
+  //abridge currently limits MAX to 120 to prevent overcurrent draw
+  float left; //left wheels PWM value
+  float right; //right wheels PWM value
 
-  bool interupt = false;
+  bool interupt = false; //hold if interupt has occured yet
 
   float rotateOnlyAngleTolerance = 0.05;  //May be too low?
-  float finalRotationTolerance = 0.1;
+  float finalRotationTolerance = 0.1; //dead code not used
   const float waypointTolerance = 0.15; //15 cm tolerance.
 
   //0.65 MAX value
@@ -62,6 +64,8 @@ class DriveController : virtual Controller
   void slowPID(float errorVel,float errorYaw, float setPointVel, float setPointYaw);
   void constPID(float erroVel,float constAngularError, float setPointVel, float setPointYaw);
 
+  //each PID movement paradigm needs at minimum two PIDs to acheive good robot motion.
+  //one PID is for linear movement and the second for rotational movements
   PID fastVelPID;
   PID fastYawPID;
 

src/behaviours/src/ObstacleController.cpp

@@ -172,7 +172,7 @@ void ObstacleController::setTagData(vector<Tag> tags){
 
   // this loop is to get the number of center tags
   if (!targetHeld) {
-    for (int i = 0; i < tags.size(); i++) { //redudant for loop
+    for (int i = 0; i < tags.size(); i++) { //redundant for loop
       if (tags[i].getID() == 256) {
 
 	collection_zone_seen = checkForCollectionZoneTags( tags );
@@ -264,7 +264,7 @@ void ObstacleController::setTargetHeld() {
 
 void ObstacleController::setTargetHeldClear()
 {
-  //adjust current state on transition from cube held to not cube helf
+  //adjust current state on transition from cube held to cube not held
   if (targetHeld)
   {
     Reset();