Tournament Champtions

include/feedbackControllers/pid.hpp

@@ -27,8 +27,8 @@ namespace Pronounce {
 		double prevError = 0.0;
 		double derivitive;
 
-		double integralBound = 30.0;
-		double maxIntegral = 0.3;
+		double integralBound = 3000.0;
+		double maxIntegral = 30;
 
 		double power;
 

include/stateMachine/behaviors/drivetrain/initDrivetrain.hpp

@@ -78,30 +78,38 @@ namespace Pronounce {
 	ProfileConstraints rowIntakeProfileConstraints;
 
 	SinusoidalVelocityProfile testVelocityProfile(24_in, defaultProfileConstraints);
-	SinusoidalVelocityProfile frontRollerToAllianceStackVelocityProfile(16_in, defaultProfileConstraints);
+	SinusoidalVelocityProfile frontRollerToAllianceStackVelocityProfile(38_in, defaultProfileConstraints);
 	SinusoidalVelocityProfile intakeAllianceStackVelocityProfile(24_in, stackIntakeProfileConstraints);
-	SinusoidalVelocityProfile allianceStackToAllianceDiscsVelocityProfile(24_in, rowIntakeProfileConstraints);
+	SinusoidalVelocityProfile allianceStackToAllianceDiscsVelocityProfile(112.5_in, rowIntakeProfileConstraints);
 	SinusoidalVelocityProfile allianceDiscsToRollerVelocityProfile(24_in, defaultProfileConstraints);
+	SinusoidalVelocityProfile moveForward8inVelocityProfile(12_in, defaultProfileConstraints);
+	SinusoidalVelocityProfile moveForward24inVelocityProfile(24_in, defaultProfileConstraints);
 
-	PID turningPid(380, 0, 2000);
+	PID turningPid(330, 0, 2000);
 
 	OmniMotionProfiling testProfile("MotionProfilingTest", testVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, 0_deg);
-	OmniMotionProfiling frontRollerToAllianceStack("FrontRollerToAllianceStack", frontRollerToAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 45.0_deg, 0_deg);
+	OmniMotionProfiling frontRollerToAllianceStack("FrontRollerToAllianceStack", frontRollerToAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 30.0_deg, 0_deg);
 	OmniMotionProfiling intakeAllianceStack("IntakeAllianceStack", intakeAllianceStackVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, -45_deg);
-	OmniMotionProfiling allianceStackToAllianceDiscs("AllianceStackToAllianceDiscs", allianceStackToAllianceDiscsVelocityProfile, drivetrain, &turningPid, &odometry, 0.0_deg, -45_deg);
-	OmniMotionProfiling allianceDiscsToRoller("AllianceDiscsToRoller", allianceDiscsToRollerVelocityProfile, drivetrain, &turningPid, &odometry, -45.0_deg, 180_deg);
+	OmniMotionProfiling allianceStackToAllianceDiscs("AllianceStackToAllianceDiscs", allianceStackToAllianceDiscsVelocityProfile, drivetrain, &turningPid, &odometry, 6.0_deg, -45_deg);
+	OmniMotionProfiling allianceDiscsToRoller("AllianceDiscsToRoller", allianceDiscsToRollerVelocityProfile, drivetrain, &turningPid, &odometry, -45.0_deg, -90_deg);
+	OmniMotionProfiling moveForward8in("MoveForward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, 90.0_deg, -45_deg);
+	OmniMotionProfiling moveForward8in0deg("MoveForward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, 90.0_deg, 0_deg);
+	OmniMotionProfiling moveBackward8in0deg("MoveBackward8In", moveForward8inVelocityProfile, drivetrain, &turningPid, &odometry, -90.0_deg, 0_deg);
+	OmniMotionProfiling moveForward24in180("MoveForward24In180", moveForward24inVelocityProfile, drivetrain, &turningPid, &odometry, 180.0_deg, -90_deg);
 
 	// BezierPath testPath;
 	// PurePursuitProfile defaultPurePursuitProfile = {10.0_in, testVelocityProfile};
 	// OmniPurePursuit testPurePursuit(&drivetrain, &odometry, defaultPurePursuitProfile, testPath);
-	RotationController turnTo90("TurnTo90", &drivetrain, &odometry, &turningPid, 90_deg);
-	Wait turnTo905s(&turnTo90, 3000);
+	RotationController turnTo90("TurnTo90", &drivetrain, &odometry, &turningPid, -90_deg);
+	Wait turnTo905s(&turnTo90, 1.5_s);
 	RotationController turnTo315("TurnTo315", &drivetrain, &odometry, &turningPid, 315_deg);
-	Wait turnTo3155s(&turnTo315, 1500);
-	RotationController turnTo180("TurnTo180", &drivetrain, &odometry, &turningPid, 180_deg);
-	Wait turnTo1805s(&turnTo180, 1500);
+	Wait turnTo3155s(&turnTo315, 1.5_s);
+	RotationController turnTo180("TurnTo180", &drivetrain, &odometry, &turningPid, 270_deg);
+	Wait turnTo1805s(&turnTo180, 1.5_s);
+	RotationController turnTo_135("turnTo_135", &drivetrain, &odometry, &turningPid, -135_deg);
+	Wait turnTo_1355s(&turnTo_135, 1.5_s);
 	OmniMovement drivetrainRoller("DrivetrainRoller", &drivetrain, 50, -90_deg);
-	Wait drivetrainRollerWait(&drivetrainRoller, 600);
+	Wait drivetrainRollerWait(&drivetrainRoller, 600_ms);
 
 	StateController drivetrainStateController("DrivetrainStateController", &drivetrainStopped);
 
@@ -115,7 +123,7 @@ namespace Pronounce {
 		stackIntakeProfileConstraints.maxAcceleration = 60 * inchs2;
 		stackIntakeProfileConstraints.maxJerk = 3.0;
 
-		rowIntakeProfileConstraints.maxVelocity = 20_in/1_s;
+		rowIntakeProfileConstraints.maxVelocity = 30_in/1_s;
 		rowIntakeProfileConstraints.maxAcceleration = 60 * inchs2;
 		rowIntakeProfileConstraints.maxJerk = 3.0;
 
@@ -124,18 +132,25 @@ namespace Pronounce {
 		intakeAllianceStackVelocityProfile.setProfileConstraints(stackIntakeProfileConstraints);
 		allianceStackToAllianceDiscsVelocityProfile.setProfileConstraints(rowIntakeProfileConstraints);
 		allianceDiscsToRollerVelocityProfile.setProfileConstraints(defaultProfileConstraints);
+		moveForward8inVelocityProfile.setProfileConstraints(defaultProfileConstraints);
+		moveForward24inVelocityProfile.setProfileConstraints(defaultProfileConstraints);
 
 		testVelocityProfile.calculate(100);
 		frontRollerToAllianceStackVelocityProfile.calculate(100);
 		intakeAllianceStackVelocityProfile.calculate(100);
 		allianceStackToAllianceDiscsVelocityProfile.calculate(100);
 		allianceDiscsToRollerVelocityProfile.calculate(100);
+		moveForward8inVelocityProfile.calculate(100);
 
 		testProfile.setVelocityProfile(testVelocityProfile);
 		frontRollerToAllianceStack.setVelocityProfile(frontRollerToAllianceStackVelocityProfile);
 		intakeAllianceStack.setVelocityProfile(intakeAllianceStackVelocityProfile);
 		allianceStackToAllianceDiscs.setVelocityProfile(allianceStackToAllianceDiscsVelocityProfile);
 		allianceDiscsToRoller.setVelocityProfile(allianceDiscsToRollerVelocityProfile);
+		moveForward8in.setVelocityProfile(moveForward8inVelocityProfile);
+		moveForward8in0deg.setVelocityProfile(moveForward8inVelocityProfile);
+		moveForward24in180.setVelocityProfile(moveForward24inVelocityProfile);
+
 
 		// Motor brake modes
 		frontLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST);

include/stateMachine/behaviors/endgame/endgame.hpp

@@ -7,10 +7,10 @@
 namespace Pronounce {
 	class Endgame : public Behavior {
 	private:
-		ADIDigitalOutGroup& digitalOutputGroup;
+		pros::ADIDigitalOut& digitalOutputGroup;
 		bool enabled;
 	public:
-		Endgame(std::string name, ADIDigitalOutGroup& digitalOutputGroup, bool enabled);
+		Endgame(std::string name, pros::ADIDigitalOut& digitalOutputGroup, bool enabled);
 
 		void initialize() {
 			digitalOutputGroup.set_value(enabled);
@@ -31,7 +31,7 @@ namespace Pronounce {
 		~Endgame();
 	};
 
-	Endgame::Endgame(std::string name, ADIDigitalOutGroup& digitalOutputGroup, bool enabled) : digitalOutputGroup(digitalOutputGroup), enabled(enabled), Behavior(name) {
+	Endgame::Endgame(std::string name, pros::ADIDigitalOut& digitalOutputGroup, bool enabled) : digitalOutputGroup(digitalOutputGroup), enabled(enabled), Behavior(name) {
 	}
 
 	Endgame::~Endgame() {

include/stateMachine/behaviors/endgame/initEndgame.hpp

@@ -7,15 +7,13 @@
 #include "stateMachine/wait.hpp"
 
 namespace Pronounce {
-	ADIDigitalOutGroup endgameDigitalOutputs;
+	pros::ADIDigitalOut endgameDigitalOutputs(7, false);
 
 	Endgame endgameDisabled("EndgameDisabled", endgameDigitalOutputs, false);
 	Endgame endgameEnabled("EndgameEnabled", endgameDigitalOutputs, true);
 
-	StateController endgameStateController("EndgameStateController", &endgameEnabled);
+	StateController endgameStateController("EndgameStateController", &endgameDisabled);
 
 	void initEndgame() {
-		endgameDigitalOutputs.addDigitalOutput(new pros::ADIDigitalOut(8, false));
-		endgameDigitalOutputs.addDigitalOutput(new pros::ADIDigitalOut(7, false));
 	}
 }

include/stateMachine/behaviors/intake/initIntake.hpp

@@ -28,10 +28,10 @@ namespace Pronounce {
 	StateController intakeStateController("IntakeStateController", &intakeIntaking);
 	StateController intakeStateExtensionController("IntakeStateExtensionController", new Behavior());
 
-	Wait intakeDejam1(&intakeDejam, 500);
-	Wait intakeDejam2(&intakeIntaking, 500);
+	Wait intakeDejam1(&intakeDejam, 500_ms);
+	Wait intakeDejam2(&intakeIntaking, 500_ms);
 
-	Wait intakeRoller1(&intakeEjecting, 1000);
+	Wait intakeRoller1(&intakeEjecting, 1_s);
 
 	Sequence intakeRoller("IntakeRollerSequence");
 

include/stateMachine/behaviors/launcher/initLauncher.hpp

@@ -15,7 +15,7 @@
 
 namespace Pronounce {
 
-	pros::Motor flywheel1(11, pros::E_MOTOR_GEARSET_36, true);
+	pros::Motor flywheel1(16, pros::E_MOTOR_GEARSET_36, true);
 
 	pros::Motor turretMotor(3, pros::E_MOTOR_GEARSET_06, false);
 
@@ -24,7 +24,7 @@ namespace Pronounce {
 	pros::ADIDigitalOut indexer(1, false);
 
 	FlywheelPID flywheelPID(4.0, 0.1, 0.0, 3.55);
-	PID turretPID(30000.0, 0.0, 35000.0);
+	PID turretPID(30000.0, 0.0, 120000.0);
 
 	pros::Rotation turretRotation(4);
 
@@ -36,8 +36,8 @@ namespace Pronounce {
 	StateController launcherStateController("LauncherStateController", &launcherIdle);
 	StateController launcherStateExtensionController("LauncherStateExtensionController", new Behavior());
 
-	Wait launchDisc1(&launcherLaunching, 300);
-	Wait launchDisc2(&launcherFullSpeed, 700);
+	Wait launchDisc1(&launcherLaunching, 300_ms);
+	Wait launchDisc2(&launcherFullSpeed, 1500_ms);
 
 	Sequence launchDisc("LaunchDisc");
 
@@ -58,23 +58,19 @@ namespace Pronounce {
 		launchDisc.addState(&launcherStateController, &launcherFullSpeed);
 		launchDisc.addState(&launcherStateController, &launchDisc1);
 
-		launch2Disc.addState(&launcherStateController, &launcherFullSpeed);
+		launch2Disc.addState(&launcherStateController, &launchDisc2);
 		launch2Disc.addState(&launcherStateController, &launchDisc1);
-		launch2Disc.addState(&launcherStateController, &launcherFullSpeed);
+		launch2Disc.addState(&launcherStateController, &launchDisc2);
 		launch2Disc.addState(&launcherStateController, &launchDisc1);
 
-		launch3Disc.addState(&launcherStateController, &launcherFullSpeed);
-		launch3Disc.addState(&launcherStateController, &launchDisc1);
-		launch3Disc.addState(&launcherStateController, &launcherFullSpeed);
-		launch3Disc.addState(&launcherStateController, &launchDisc1);
-		launch3Disc.addState(&launcherStateController, &launcherFullSpeed);
 		launch3Disc.addState(&launcherStateController, &launchDisc1);
+		launch3Disc.addState(&launcherStateController, &launchDisc2);
 
 		turretMotor.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD);
 
-		pros::vision_signature_s_t redGoal = pros::Vision::signature_from_utility(RED_GOAL, 4979, 7501, 6240, -601, 307, -147, 3.000, 0);
-		turretVision.set_signature(RED_GOAL, &redGoal);	
-		turretVision.set_exposure(95);
+		// pros::vision_signature_s_t redGoal = pros::Vision::signature_from_utility(RED_GOAL, 4979, 7501, 6240, -601, 307, -147, 3.000, 0);
+		// turretVision.set_signature(RED_GOAL, &redGoal);	
+		// turretVision.set_exposure(95);
 
 		flywheelRPM.add(38.0, 2335.0);
 		flywheelRPM.add(62.0, 2400.0);

include/stateMachine/behaviors/launcher/launcher.hpp

@@ -86,7 +86,7 @@ namespace Pronounce {
 
 		bool isDone() {
 			if (useIsDone) {
-				return abs(this->flywheelSpeed - this->getFlywheelSpeed()) < 20;
+				return fabs(this->flywheelSpeed - this->getFlywheelSpeed()) < 20.0;
 			} else {
 				return false;
 			}

include/stateMachine/sequence.hpp

@@ -35,7 +35,7 @@ namespace Pronounce {
 					stateControllers.at(currentIndex)->setCurrentBehavior(behaviors.at(currentIndex));
 				}
 				else {
-					std::cout << this->getname() << "Ending" << std::endl;
+					std::cout << this->getName() << "Ending" << std::endl;
 					stateControllers.at(currentIndex)->useDefaultBehavior();
 					currentIndex++;
 					// Done

include/stateMachine/state/modeLogic.hpp

@@ -24,6 +24,7 @@ namespace Pronounce {
 		stateControllers.addBehavior(&launcherStateExtensionController);
 		stateControllers.addBehavior(&launcherStateController);
 		stateControllers.addBehavior(&drivetrainStateController);
+		stateControllers.addBehavior(&endgameStateController);
 		stateControllers.addBehavior(&teleopController);
 	}
 
@@ -50,7 +51,13 @@ namespace Pronounce {
 			launcherLaunching.setFlywheelSpeed(robotStatus->getFlywheelTarget());
 			launcherFullSpeed.setFlywheelSpeed(robotStatus->getFlywheelTarget());
 
-			double turretAngle = clamp(robotStatus->getTurretAngle().getValue(), -M_PI_2, M_PI_2);
+			double turretAngle = 0;
+
+			if (false) {
+				turretAngle = clamp(angleDifference(robotStatus->getTurretAngle().Convert(radian), 0), -M_PI_2, M_PI_2);
+			} else {
+				turretAngle = clamp(angleDifference((robotStatus->getTurretAngle() - odometry.getAngle()).getValue(), 0), -M_PI_2, M_PI_2);
+			}
 
 			launcherStopped.setTurretAngle(turretAngle);
 			launcherIdle.setTurretAngle(turretAngle);

include/stateMachine/state/robotStatus.hpp

@@ -18,11 +18,11 @@ namespace Pronounce {
 	// const GameMode gameMode = GameMode::Red;
 
 	double flywheelAdjustment = 0;
-	Angle turretAdjustment = 0.0;
+	Angle turretAdjustment = 90_deg;
+	double currentFlywheelRPM = 2300;
 
 	class RobotStatus : public Behavior {
 	private:
-		double flywheelRPM = 2200;
 		Angle turretAngle = 0.0;
 
 		FlywheelController allianceGoal;
@@ -44,24 +44,26 @@ namespace Pronounce {
 
 			FlywheelValue flywheelValues = this->allianceGoal.getFlywheelValue(*odometry.getPosition(), odometry.getCurrentVelocity());
 
-			// this->turretAngle = flywheelValues.turretAngle - odometry.getAngle();
-			// this->flywheelRPM = flywheelValues.flywheelSpeed;
+			if (pros::competition::is_autonomous()) {
+				// this->turretAngle = flywheelValues.turretAngle - odometry.getAngle();
+				// currentFlywheelRPM = flywheelValues.flywheelSpeed;
+			}
 		}
 
 		void exit() {
 			return;
 		}
 
 		double getFlywheelTarget() {
-			return flywheelRPM + flywheelAdjustment;
+			return currentFlywheelRPM + flywheelAdjustment;
 		}
 
 		double getActualFlywheelRpm() {
 			return launcherIdle.getFlywheelSpeed();
 		}
 
-		void setFlywheelRPM(double flywheelRPM) {
-			this->flywheelRPM = flywheelRPM;
+		void setFlywheelRPM(double flywheelRPM1) {
+			currentFlywheelRPM = flywheelRPM1;
 		}
 
 		Angle getTurretAngle() {

include/stateMachine/state/teleopModeLogic.hpp

@@ -65,19 +65,25 @@ namespace Pronounce {
 			}
 
 			if (controller2->is_connected()) {
-				turretAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 500.0 : 0;
-				flywheelAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 10.0 : 0;
+				turretAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X) / 3000.0 : 0;
+				flywheelAdjustment += abs(controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y)) > 15 ? (double) controller2->get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y) / 20.0 : 0;
 
-				if (controller2->get_digital_new_press(DIGITAL_L2))
-					robotStatus.setFlywheelRPM(2200);
+				if (controller2->get_digital_new_press(DIGITAL_L2)) {
+					robotStatus->setFlywheelRPM(2300);
+					flywheelAdjustment = 0.0;
+				}
 				if (controller2->get_digital_new_press(DIGITAL_L1))
-					robotStatus.setFlywheelRPM(3000);
+					robotStatus->setFlywheelRPM(2500);
 
-				if (controller2->get_digital_new_press(DIGITAL_R1) && controller2.get_digital_new_press(R2))
-					endgameStateController.setCurrentBehavior(engameEnabled);
+				if (controller2->get_digital(DIGITAL_X) && controller1->get_digital(DIGITAL_X))
+					endgameStateController.setCurrentBehavior(&endgameEnabled);
+
+				if (pros::millis() % 200 < 15) {
+					controller2->clear();
+				} else {
+					controller2->set_text(0, 0, std::to_string(this->robotStatus->getFlywheelTarget()));
+				}
 			} else {
-				if (controller1->get_digital_new_press(DIGITAL_R1) && controller1.get_digital_new_press(R2))
-					endgameStateController.setCurrentBehavior(engameEnabled);
 			}
 		}