Candle lights

build.gradle

@@ -2,7 +2,7 @@ plugins {
     id "java"
     id 'java-library'
     id 'maven-publish'
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
     id 'com.google.protobuf' version '0.8.19'
 }
 

src/main/java/com/team766/robot/OI.java

@@ -6,14 +6,15 @@
 import com.team766.hal.RobotProvider;
 import com.team766.logging.Category;
 import com.team766.robot.constants.InputConstants;
-import com.team766.robot.constants.InputConstants.IntakeState;
 import com.team766.robot.procedures.*;
 import com.team766.robot.mechanisms.Drive;
+import com.team766.robot.mechanisms.Intake;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the code that allow control of the robot.
@@ -32,11 +33,13 @@ public class OI extends Procedure {
 	private double LeftJoystick_Z = 0;
 	private double LeftJoystick_Theta = 0;
 	private boolean isCross = false;
-	private IntakeState intakeState = IntakeState.IDLE;
 
 	private static final double HighConeArm1 = 0;
 	private static final double CHA2 = 0;
 	private static final double CMA1 = 0;
+
+	private int state = 0;
+	private boolean ignoreState = false;
 	// enum generalControl{
 	// 	CONE_HIGH_NODE,
 	// 	CUBE_HIGH_NODE,
@@ -65,21 +68,57 @@ public OI() {
 
 	public void run(Context context) {
 		context.takeOwnership(Robot.drive);
-		context.takeOwnership(Robot.intake);
-		context.takeOwnership(Robot.arms);
-		context.takeOwnership(Robot.storage);
+		// context.takeOwnership(Robot.intake);
 		context.takeOwnership(Robot.gyro);
-		context.takeOwnership(Robot.grabber);
-
-		// CameraServer.startAutomaticCapture();
-
-		Robot.arms.resetFirstEncoders();
-		Robot.arms.resetSecondEncoders();
+		context.takeOwnership(Robot.lights);
 
 		while (true) {
 			context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData());
 			RobotProvider.instance.refreshDriverStationData();
 
+			//TODO: ADD CODE DEPENDING ON WHAT THE STATE SHOULD BE FOR WHICH BUTTONS
+			if(DriverStation.getMatchTime() < 30 && DriverStation.getMatchTime() > 29){
+				Robot.lights.rainbow();
+				log("" + DriverStation.getMatchTime());
+				ignoreState = true;
+			}
+
+			if(DriverStation.getMatchTime() <29 && DriverStation.getMatchTime() > 28.7){
+				Robot.lights.clearAnimation();
+				ignoreState = false;
+			}
+			switch (state){
+				case 1:
+					if(ignoreState){ break;}
+					Robot.lights.signalCube();
+					break;
+				case 2:
+					if(ignoreState){ break;}
+					Robot.lights.signalCone();
+					break;
+				case 3:
+					if(ignoreState){ break;}
+					Robot.lights.rainbow();
+					break;
+				case 4:
+					if(ignoreState){ break;}
+					Robot.lights.hybridScore();
+					break;
+				case 5:
+					if(ignoreState){ break;}
+					Robot.lights.midScore();
+					break;
+				case 6:
+					if(ignoreState){ break;}
+					Robot.lights.highScore();
+					break;
+				default:
+					if(!ignoreState){
+					Robot.lights.resetLights();
+					}
+			}
+
+
 			// Add driver controls here - make sure to take/release ownership
 			// of mechanisms when appropriate.
 
@@ -96,6 +135,9 @@ public void run(Context context) {
 			// 	SmartDashboard.putString("Alliance", "NULLLLLLLLL");
 			// }
 
+			if(DriverStation.isTeleop() && DriverStation.getMatchTime() < 30){
+				Robot.lights.signalMalfunction();
+			}
 
 			if (leftJoystick.getButtonPressed(InputConstants.RESET_GYRO)) {
 				Robot.gyro.resetGyro();
@@ -138,43 +180,25 @@ public void run(Context context) {
 
 			// Sets intake state based on button pressed
 			if (controlPanel.getButtonPressed(InputConstants.INTAKE)) {
-				if (intakeState == IntakeState.IDLE) {
-					Robot.intake.startIntake();
-					Robot.storage.beltIn();
-					intakeState = IntakeState.SPINNINGFWD;
-				} else {
-					Robot.intake.stopIntake();
-					Robot.storage.beltIdle();
-					intakeState = IntakeState.IDLE;
-				}
+				// if (Robot.intake.getState() == Intake.State.IDLE) {
+				// 	Robot.intake.in();
+				// } else {
+				// 	Robot.intake.stop();
+				//}
 			}
-			/* if (controlPanel.getButtonPressed(InputConstants.INTAKE_PISTONLESS)){
-				if (intakeState == IntakeState.IDLE){
-					Robot.intake.intakePistonless();
-					Robot.storage.beltIn();
-					intakeState = IntakeState.SPINNINGREV;
-				} else {
-					Robot.intake.stopIntake();
-					Robot.storage.beltIdle();
-					intakeState = IntakeState.IDLE;
-				}
-			} */
+
 			if (controlPanel.getButtonPressed(InputConstants.OUTTAKE)) {
-				if (intakeState == IntakeState.IDLE) {
-					Robot.intake.reverseIntake();
-					Robot.storage.beltOut();
-					intakeState = IntakeState.SPINNINGREV;
-				} else {
-					Robot.intake.stopIntake();
-					Robot.storage.beltIdle();
-					intakeState = IntakeState.IDLE;
-				}
+				// if (Robot.intake.getState() == Intake.State.IDLE) {
+				// 	Robot.intake.out();
+				// } else {
+				// 	Robot.intake.stop();
+				// }
 			} 
 
 			// Sets the wheels to the cross position if the cross button is pressed
 			if (rightJoystick.getButtonPressed(InputConstants.CROSS_WHEELS)) {
 				if (!isCross) {
-					context.startAsync(new setCross());
+					context.startAsync(new SetCross());
 				}
 				isCross = !isCross;
 			}
@@ -187,11 +211,10 @@ public void run(Context context) {
 				context.takeOwnership(Robot.drive);
 				// If a button is pressed, drive is just fine adjustment
 				if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) {
-					Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (-leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (-rightJoystickX * FINE_DRIVING_COEFFICIENT));
+					Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT));
 				} else {
-          // On deafault, controls the robot field oriented
-          // Need negatives here, controls backwards otherwise (most likely specific to CLR)
-					Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (-leftJoystickX), (leftJoystickY), (-rightJoystickX));
+          	// On deafault, controls the robot field oriented
+					Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX), (leftJoystickY), (rightJoystickX));
 				}
 			} else if (!isCross) {
 				Robot.drive.stopDrive();			
@@ -203,73 +226,43 @@ public void run(Context context) {
 
 			if (controlPanel.getButtonPressed(InputConstants.CONE_HIGH)) {
 				log("Arm cone high");
-				Robot.arms.stowed = false;
-				Robot.arms.pidForArmOne(-17.379);
-				Robot.arms.pidForArmTwo(-56.61); //previously -66
 			}
 			if (controlPanel.getButtonPressed(InputConstants.CONE_MID)) {
 				log("Arm cone mid");
-				Robot.arms.stowed = false;
-				Robot.arms.pidForArmOne(2.7765);
-				Robot.arms.pidForArmTwo(-83.813); //previously -93
 			}
 			if (controlPanel.getButtonPressed(InputConstants.ARM_READY)) {
 				log("Arm ready");
-				Robot.arms.stowed = false;
-				Robot.arms.pidForArmOne(10.269);
-				Robot.arms.pidForArmTwo(-90);
 			}
 			if (controlPanel.getButtonPressed(InputConstants.HUMANPLAYER_PICKUP)) {
-				if (Robot.arms.getSecondJointPosition() < -100) {
-					log("Move arms to Ready position before moving to Pickup");
-				} else {
-					log("Arm pickup");
-					Robot.arms.stowed = false;
-					Robot.arms.pidForArmOne(22.73);
-					Robot.arms.pidForArmTwo(-62.529); // previously -72.529
-				}
 			}
 			if (controlPanel.getButtonPressed(InputConstants.UNSTOWED)) {
 				log("Arm unstowed");
-				Robot.arms.stowed = true;
-				Robot.arms.pidForArmOne(10.269);
-				Robot.arms.pidForArmTwo(-157.387);
 			}
 			/* if(controlPanel.getButton(InputConstants.IN_CHASSIS)){
 				Robot.arms.pidForArmOne(22.73);
 				Robot.arms.pidForArmTwo(-73.664);
 			} */
 			if (controlPanel.getButton(InputConstants.NUDGE_UP)) {
 				log("Arm nudge up");
-				Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2up());
 			}
 
 			if (controlPanel.getButton(InputConstants.NUDGE_DOWN)) {
 				log("Arm nudge down");
-				Robot.arms.pidForArmTwo(Robot.arms.nudgeArm2down());
 			}
 
 			if (controlPanel.getButton(InputConstants.GRAB_IN)) {
-				Robot.grabber.grabberPickUp();
 			} else if (rightJoystick.getButton(InputConstants.GRABBER_RELEASE)) {
-				Robot.grabber.grabberLetGo();
 			} else {
-				Robot.grabber.grabberStop();
 			} 
 
 			if (controlPanel.getButtonPressed(InputConstants.BRAKE)) {
-				Robot.arms.brake();
 			} else if (controlPanel.getButtonPressed(InputConstants.COAST)) {
-				Robot.arms.coast();
 			}
 
 			if (leftJoystick.getButtonPressed(InputConstants.ARM_STOP)) {
-				Robot.arms.armStop();
 			}
 
 			if (controlPanel.getButtonPressed(13)) {
-				Robot.arms.resetFirstEncoders();
-				Robot.arms.resetSecondEncoders();
 			}
 		}
 	}

src/main/java/com/team766/robot/Robot.java

@@ -4,20 +4,16 @@
 
 public class Robot {
 	// Declare mechanisms here
-	public static Intake intake; 
-	public static Storage storage;
+	// public static Intake intake; 
 	public static Drive drive;
-	public static Arms arms;
 	public static Gyro gyro;
-	public static Grabber grabber;
+	public static Lights lights;
 
 	public static void robotInit() {
 		// Initialize mechanisms here
-		intake = new Intake();
-		storage = new Storage();
+		// intake = new Intake();
 		drive = new Drive();
-		arms = new Arms();
 		gyro = new Gyro();
-		grabber = new Grabber();
+		lights = new Lights();
 	}
 }

src/main/java/com/team766/robot/constants/ConfigConstants.java

@@ -0,0 +1,41 @@
+package com.team766.robot.constants;
+
+/** Constants used for reading values from the config file. */
+public final class ConfigConstants {
+	// utility class
+	private ConfigConstants() {}
+
+	// drive config values
+	public static final String DRIVE_DRIVE_FRONT_RIGHT = "drive.DriveFrontRight";
+	public static final String DRIVE_DRIVE_FRONT_LEFT = "drive.DriveFrontLeft";
+	public static final String DRIVE_DRIVE_BACK_RIGHT = "drive.DriveBackRight";
+	public static final String DRIVE_DRIVE_BACK_LEFT = "drive.DriveBackLeft";
+
+	public static final String DRIVE_STEER_FRONT_RIGHT = "drive.SteerFrontRight";
+	public static final String DRIVE_STEER_FRONT_LEFT = "drive.SteerFrontLeft";
+	public static final String DRIVE_STEER_BACK_RIGHT = "drive.SteerBackRight";
+	public static final String DRIVE_STEER_BACK_LEFT = "drive.SteerBackLeft";
+
+	// intake config values
+	public static final String INTAKE_MOTOR = "intake.motor";
+
+	// wrist config values
+	public static final String WRIST_MOTOR = "wrist.motor";
+	public static final String WRIST_PGAIN = "wrist.pGain";
+	public static final String WRIST_IGAIN = "wrist.iGain";
+	public static final String WRIST_DGAIN = "wrist.dGain";
+	public static final String WRIST_FFGAIN = "wrist.ffGain";
+
+	// elevator config values
+	public static final String ELEVATOR_LEFT_MOTOR = "elevator.leftMotor";
+	public static final String ELEVATOR_RIGHT_MOTOR = "elevator.rightMotor";
+	public static final String ELEVATOR_PGAIN = "elevator.pGain";
+	public static final String ELEVATOR_IGAIN = "elevator.iGain";
+	public static final String ELEVATOR_DGAIN = "elevator.dGain";
+	public static final String ELEVATOR_FFGAIN = "elevator.ffGain";
+	public static final String ELEVATOR_MAX_VELOCITY = "elevator.maxVelocity";
+	public static final String ELEVATOR_MIN_OUTPUT_VELOCITY = "elevator.minOutputVelocity";
+	public static final String ELEVATOR_MAX_ACCEL = "elevator.maxAccel";
+}
+
+

src/main/java/com/team766/robot/constants/OdometryInputConstants.java

@@ -14,8 +14,8 @@ public final class OdometryInputConstants {
 	public static final double GEAR_RATIO = 6.75;
 	//Unique to the type of swerve module we have. For every revolution of the wheel, the encoders will increase by 2048.
 	public static final int ENCODER_TO_REVOLUTION_CONSTANT = 2048;
-	//The distance between the centers of the wheels on each side of the robot. This was measured as 22 inches, then converted to meters.
-	public static final double DISTANCE_BETWEEN_WHEELS = 22 * 2.54 / 100;
+	//The distance between the centers of the wheels on each side of the robot. This was measured as 20.5 inches, then converted to meters.
+	public static final double DISTANCE_BETWEEN_WHEELS = 20.5 * 2.54 / 100;
 	//How often odometry updates, in seconds.
 	public static final double RATE_LIMITER_TIME = 0.05;