Finish PID Tuning

Area-53-Robotics · Feb 3, 2024 · dcc3500 · dcc3500
1 parent ae451b9
commit dcc3500
Show file tree

Hide file tree

Showing 4 changed files with 19 additions and 17 deletions.
diff --git a/src/autonomous.cpp b/src/autonomous.cpp
@@ -70,14 +70,15 @@ void autonomous() {
 
 	switch (autonSelect) {
 		case 0:
-			chassis.moveToPoint(0, 15, 5000, false);	
+			chassis.moveToPoint(0, 30, 50000);
+			// chassis.turnTo(45, 45, 5000);
 			break;
 		// LemLib left quals auton in progress
 		case 1:
 			controller.print(0, 0, "Left Quals Auton");
 			/*chassis.moveToPoint(0,8,1500);
 			AutonIntake(1000,false);
-			chassis.moveToPoint(0, -15, 5000, false);	
+			chassis.moveToPoint(0, -15, 5000, false);
 			chassis.turnTo(10,0,100);
 			chassis.moveToPoint(0,-35,5000,false);
 			chassis.turnTo(90,0,100,false);

diff --git a/src/global.cpp b/src/global.cpp
@@ -21,7 +21,7 @@ pros::Motor FRMotor(20, pros::E_MOTOR_GEAR_BLUE);
 pros::Motor_Group LMotors({BLMotor, MLMotor, FLMotor});
 pros::Motor_Group RMotors({BRMotor, MRMotor, FRMotor});
 
-pros::Motor KickerMotor(12, pros::E_MOTOR_GEAR_RED);
+pros::Motor KickerMotor(12, pros::E_MOTOR_GEAR_RED, pros::E_MOTOR_ENCODER_DEGREES);
 pros::Motor IntakeMotor(7, pros::E_MOTOR_GEAR_BLUE);
 
 // Initializes DigitalOut objects to control the pneumatic wings.
@@ -53,6 +53,7 @@ void ControllerDisplay() {
 /*
     Integer variable representing which auton is set to run during the autonomous phase
 
+    0: Odom Tests
     1: Left Quals
     2: Right Quals
     3: Left Elims
@@ -61,7 +62,7 @@ void ControllerDisplay() {
     6: No Auton
     7: Programming Skills
 */
-unsigned short int autonSelect = 2;
+unsigned short int autonSelect = 0;
 
 /*
     Integer variable to represent the current game phase
@@ -75,9 +76,9 @@ unsigned short int GamePhase = 1;
 // LemLib drivetrain configuration
 lemlib::Drivetrain drivetrain{
     &LMotors, // left drivetrain motors
-    &RMotors, // right drivetrain motors
+    &RMotors, // right drivetrain motor
     10.35, // track width
-    3.25, // wheel diameter
+    2.25, // wheel diameter
     450, // wheel rpm
     8 // chase power
 };
@@ -95,7 +96,7 @@ lemlib::OdomSensors sensors{
 lemlib::ControllerSettings linearController(
     10,   // proportional gain (kP)
     0,    // integral gain (kI)
-    35,   // derivative gain (kD)
+    32,   // derivative gain (kD)
     3,    // anti windup
     1,    // small error range, in inches
     100,  // small error range timeout, in milliseconds
@@ -106,15 +107,15 @@ lemlib::ControllerSettings linearController(
 
 // Angular Motion Controller
 lemlib::ControllerSettings angularController(
-    1,     // proportional gain (kP)
+    2,     // proportional gain (kP)
     0,  // integral gain (kI)
-    0,    // derivative gain (kD)
+    10,    // derivative gain (kD)
     3,     // anti windup
     1,     // small error range, in degrees
     100,   // small error range timeout, in milliseconds
     3,     // large error range, in degrees
     500,   // large error range timeout, in milliseconds
-    10     // maximum acceleration (slew)
+    5     // maximum acceleration (slew)
 );
 
 // Creates the LemLib chassis

diff --git a/src/main.cpp b/src/main.cpp
@@ -58,13 +58,14 @@ void initialize() {
 	*/
 
 	pros::Task terminalTask([&]() {
-        lemlib::Pose pose(0, 0, 0);
         while (true) {
             // print robot location to the brain screen
             std::cout << "X: " << chassis.getPose().x << std::endl; // x
             std::cout << "Y: " << chassis.getPose().y << std::endl; // y
             std::cout << "Theta: " << chassis.getPose().theta << std::endl; // heading
             std::cout << "Inertial: " << Inertial.get_heading() << std::endl; // heading
+			std::cout << "Kicker: " << KickerMotor.get_position() << std::endl;
+            std::cout << "----------------------------" << std::endl;
             // delay to save resources
             pros::delay(500);
         }
@@ -78,7 +79,7 @@ void initialize() {
 	switch (autonSelect) {
 		case 0:
 			// Odom Tests
-			chassis.setPose(lemlib::Pose(0, 0, 0)); // X, Y, Heading (degrees)
+			chassis.setPose(0, 0, 0); // X, Y, Heading (degrees)
 			break;
 		case 1:
 			// Left Quals

diff --git a/src/opcontrol.cpp b/src/opcontrol.cpp
@@ -20,8 +20,8 @@ float GetCurveOutput(int input) {
  */
 void opcontrol()
 {
-	// autonomous();
-	// /*
+	autonomous();
+	/*
 	ControllerDisplay();
 	short int leftAxis;
 	short int rightAxis;
@@ -86,6 +86,7 @@ void opcontrol()
 				kickerOn = !kickerOn;
 		} else {
 			if(Optical.get_hue() > 65 && Optical.get_hue() < 80) kickerOn = true;
+			// else if(KickerMotor.get_position() < 1280) kickerOn = true;
 			else kickerOn = false;
 		}
 
@@ -101,18 +102,16 @@ void opcontrol()
 
 		count++;
 
-		/*
 		if (count % 40 == 0) {
 			std::cout << "Hue: " << Optical.get_hue() << std::endl;
 			std::cout << "Saturation: " << Optical.get_saturation() << std::endl;
 			std::cout << "Brightness: " << Optical.get_brightness() << std::endl;
 			std::cout << "manualKicker: ";
 			std::cout << "-----------------------" << std::endl;
 		}
-		*/
 
 		// Creates a 20 millisecond delay between each loop of the driver control code to prevent the starving of PROS kernel resources
 		pros::delay(20);
 	}
-	// */
+	*/
 }