Merge pull request #25 from ad101-lab/ad-tune-pid

ad tune pid

include/chassis/drivetrain.hpp

@@ -74,7 +74,7 @@ namespace Pronounce {
          * @return backLeft motor pointer
          */
         pros::Motor* getBackLeft() {
-            return this->frontLeft;
+            return this->backLeft;
         }
 
         /**
@@ -92,7 +92,7 @@ namespace Pronounce {
          * @return backRight motor pointer
          */
         pros::Motor* getBackRight() {
-            return this->frontLeft;
+            return this->backRight;
         }
 
         /**

include/chassis/tankDrive.hpp

@@ -4,6 +4,8 @@
 #include "drivetrain.hpp"
 #include "position/tankOdom.hpp"
 #include "pid/pid.hpp"
+#include "okapi/api.hpp"
+#include "utils/utils.hpp"
 
 namespace Pronounce {
     class TankDrivetrain : public Drivetrain {
@@ -25,10 +27,10 @@ namespace Pronounce {
         double maxVoltage = 127.0;
 
         double speedThreshhold = 2.0;
-        double errorThreshhold = 3.0;
+        double errorThreshhold = 0.5;
 
         double turnThreshhold = 2.0;
-        double turnErrorThreshhold = 3.0;
+        double turnErrorThreshhold = 5.0;
     public:
         TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu);
         void reset();
@@ -90,6 +92,8 @@ namespace Pronounce {
         void setStartingPosition(Position* position) {
             this->tankOdom->setPosition(position);
             this->startingPosition = startingPosition;
+            this->imu->set_rotation(position->getTheta());
+            angle = NAN;
         }
 
         void setPosition(Position* position) {
@@ -102,6 +106,7 @@ namespace Pronounce {
 
         void setTargetPosition(Position* targetPosition) {
             this->startingPosition = this->getPosition();
+            angle = NAN;
             this->targetPosition = targetPosition;
         }
 

src/autonSelector.cpp

@@ -30,21 +30,14 @@ int autonSelector::choose() {
     this->draw();
     this->addCallbacks();
 
-    // Wait until finnished.
-    while (!guiFinnished) {
-        pros::delay(20);
-    }
-
-    this->del();
-
     // Return the selection.
     return userSelection;
 }
 
 int autonSelector::runSelection() {
-    //this->preRun();
-    //this->functionMap[userSelection]();
-    //this->postAuton();
+    this->preRun();
+    this->functionMap[userSelection]();
+    this->postAuton();
 }
 
 void autonSelector::setFunction(int position, autonFunction function) {

src/chassis/tankDrive.cpp

@@ -1,5 +1,11 @@
 #include "tankDrive.hpp"
 
+std::shared_ptr<okapi::Logger> visionLogger = std::make_shared<okapi::Logger>(
+	okapi::TimeUtilFactory::createDefault().getTimer(),
+	"/ser/sout", // "/usd/visionLog.txt",
+	okapi::Logger::LogLevel::debug // Show everything
+	);
+
 namespace Pronounce {
 
 	TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) : Drivetrain(frontLeft, frontRight, backLeft, backRight, imu) {
@@ -21,8 +27,9 @@ namespace Pronounce {
 
 	void TankDrivetrain::update() {
 
-		if (!enabled)
+		if (!enabled) {
 			return;
+		}
 
 		tankOdom->update();
 
@@ -33,27 +40,41 @@ namespace Pronounce {
 		double xDiff = this->targetPosition->getX() - currentPosition->getX();
 		double yDiff = this->targetPosition->getY() - currentPosition->getY();
 
-		double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (reversed ? -1 : 1);
+		double angleToTarget = toDegrees(atan2(yDiff, xDiff));
+		double robotAngleToTarget = fmod(angleToTarget - imu->get_heading() + 360, 360.0);
+		bool flipDistance = -90 < fmod(robotAngleToTarget,  360.0) < 90;// Allow the robot to reverse when it passes the target
+
+		double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (flipDistance ? -1 : 1) * (reversed ? -1 : 1);
 		double linearPosition = sqrt(pow(this->targetPosition->getX() - this->startingPosition->getX(), 2) + pow(this->targetPosition->getY() - this->startingPosition->getY(), 2)) - distance;
-		double angle = nullRotationDistance < distance ? atan2(yDiff, xDiff) + (reversed ? 180 : 0) : prevAngle;
+		double angle = nullRotationDistance < distance ? angleToTarget + (reversed ? 180 : 0) : prevAngle;
 		this->prevAngle = angle;
 
 		this->movePid->setPosition(linearPosition);
-		this->movePid->setTarget(distance);
-		this->turnPid->setPosition(imu->get_heading());
-		this->turnPid->setTarget(this->getStopped() ? this->angle : toDegrees(angle));
+		this->movePid->setTarget(isnan(this->angle) ? distance : linearPosition);
+		this->turnPid->setPosition(imu->get_rotation());
+		this->turnPid->setTarget(isnan(this->angle) ? angle : this->angle);
 
-		double lateral = this->movePid->update();
+		double lateral = std::clamp(this->movePid->update(), -127.0, 127.0);
 		double turn = this->turnPid->update();
 
+		visionLogger.get()->debug<std::string>("imu angle: " + std::to_string(turnPid->getPosition())
+								 + " robotAngleToTarget:" + std::to_string(robotAngleToTarget) 
+								 + " X:" + std::to_string(currentPosition->getX())
+								 + " Y:" + std::to_string(currentPosition->getY())
+								 + " TargetX:" + std::to_string(targetPosition->getX())
+								 + " targetY:" + std::to_string(targetPosition->getY())
+								 + " heading:" + std::to_string(imu->get_heading()));// std::clamp(lateral, -maxVoltage, maxVoltage)));
+
 		this->getFrontLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage));
-		this->getFrontLeft()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage));
+		this->getBackLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage));
+		this->getFrontRight()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage));
+		this->getBackRight()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage));
 	}
 
 	bool TankDrivetrain::getStopped() {
 		// Derivitive ~= speed 
-		return this->movePid->getDerivitive() < speedThreshhold && this->movePid->getError() < errorThreshhold ||
-			this->turnPid->getDerivitive() < turnThreshhold && this->turnPid->getError() < turnErrorThreshhold;
+		return abs(this->movePid->getError()) < errorThreshhold &&
+			abs(this->turnPid->getError()) < turnErrorThreshhold;
 	}
 
 	void TankDrivetrain::waitForStop() {