done

zphelps · Feb 6, 2021 · 7d8ed0d · 7d8ed0d
1 parent 09dd5ab
commit 7d8ed0d
Show file tree

Hide file tree

Showing 4 changed files with 351 additions and 423 deletions.
diff --git a/include/Subsystems/intake.hpp b/include/Subsystems/intake.hpp
@@ -26,7 +26,7 @@ extern pros::Vision vision;
 //Visions Functions
 void initVision();
 
-double driverBaseAngle(int inColor);
+double driverBaseAngle(int inColor, int PIDSpeed);
 void monitorVisionTask(void *);
 pros::vision_object_s_t calculateVision(int inColor);
 

diff --git a/src/Subsystems/drive.cpp b/src/Subsystems/drive.cpp
@@ -255,7 +255,7 @@ void Drive::driveOP(int driveMode)
     if (master.get_digital(DIGITAL_UP))
     {
 
-        double baseTurnBias = driverBaseAngle(RED_ID);
+        double baseTurnBias = driverBaseAngle(RED_ID, 80);
 
         int n = vision3.get_object_count();
         // pros::lcd::print(0, "%d ", n);
@@ -1184,7 +1184,7 @@ void Drive::moveTask(void *parameter)
             {
                 double PIDSpeed = movePID.getOutput(target, L.get_value());
                 driveError = moveTargets.targetDistance - ((L.get_value() / TICS_PER_REVOLUTION) * (WHEEL_DIAMETER * PI));
-                double baseTurnBias = driverBaseAngle(moveTargets.color);
+                double baseTurnBias = driverBaseAngle(moveTargets.color, PIDSpeed);
 
                 right(PIDSpeed - baseTurnBias);
                 left(PIDSpeed + baseTurnBias);
@@ -1219,7 +1219,7 @@ void Drive::moveTask(void *parameter)
             while (getX() < target)
             {
                 double PIDSpeed = movePID.getOutput(target, getX());
-                double baseTurnBias = driverBaseAngle(moveTargets.color);
+                double baseTurnBias = driverBaseAngle(moveTargets.color, PIDSpeed);
 
                 right(PIDSpeed - baseTurnBias);
                 left(PIDSpeed + baseTurnBias);
@@ -1230,7 +1230,7 @@ void Drive::moveTask(void *parameter)
             while (getX() > target)
             {
                 double PIDSpeed = -movePID.getOutput(target, getX());
-                double baseTurnBias = driverBaseAngle(moveTargets.color);
+                double baseTurnBias = driverBaseAngle(moveTargets.color, PIDSpeed);
 
                 right(PIDSpeed - baseTurnBias);
                 left(PIDSpeed + baseTurnBias);
@@ -1265,7 +1265,7 @@ void Drive::moveTask(void *parameter)
             while (getY() < target)
             {
                 double PIDSpeed = movePID.getOutput(target, getY());
-                double baseTurnBias = driverBaseAngle(moveTargets.color);
+                double baseTurnBias = driverBaseAngle(moveTargets.color, PIDSpeed);
 
                 right(PIDSpeed - baseTurnBias);
                 left(PIDSpeed + baseTurnBias);
@@ -1276,7 +1276,7 @@ void Drive::moveTask(void *parameter)
             while (getY() > target)
             {
                 double PIDSpeed = -movePID.getOutput(target, getY());
-                double baseTurnBias = driverBaseAngle(moveTargets.color);
+                double baseTurnBias = driverBaseAngle(moveTargets.color, PIDSpeed);
 
                 right(PIDSpeed - baseTurnBias);
                 left(PIDSpeed + baseTurnBias);

diff --git a/src/Subsystems/intake.cpp b/src/Subsystems/intake.cpp
@@ -69,10 +69,10 @@ void monitorVisionTask(void *)
     }
 }
 
-#define BASE_P 0.9 // The Kp for X error / base power 0.85
+// #define BASE_P 1.25 // The Kp for X error / base power 0.9
 
 //Function that outputs the power to be sent to the base for turning
-double driverBaseAngle(int inColor)
+double driverBaseAngle(int inColor, int PIDSpeed)
 {
     int x_error = calculateVision(inColor).x_middle_coord - VISION_FOV_WIDTH / 2;
     // Centers the vision, and any x deriviation is our error
@@ -88,6 +88,11 @@ double driverBaseAngle(int inColor)
     }
     else
     {
+        double BASE_P = 0.75;
+        if (PIDSpeed > 65)
+        {
+            double BASE_P = 1.25;
+        }
         finalBasePower = x_error * BASE_P; // For now a simple P based on X
         // deriviation from the center of the vision
     }