diff --git a/dashboard b/dashboard index 77cbcb3..cd10fe4 160000 --- a/dashboard +++ b/dashboard @@ -1 +1 @@ -Subproject commit 77cbcb31069a65928a351ea3de3349e5cf9a67ee +Subproject commit cd10fe409bae4576d11e59ede1df51922f7a1e73 diff --git a/rio/constants.py b/rio/constants.py index 901a241..c7f1667 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -166,7 +166,8 @@ class SuperStrucConstants: # angles for shooter ShootPos = 321 LoadPos = 204 - ClimbPos = 277 + ClimbPos = 285 + HarmonyPos = 316 # CANSpark IDS rotateID = 11 @@ -200,8 +201,8 @@ class IntakeConstants: # conversion factor kLiftConversion = 1 # Configured feb 12 by joe - SuckPos = 0.541 - BlowPos = 0.005 + SuckPos = 0.645 + BlowPos = 0.1 # lift pid kLiftP = 3.3 kLiftI = 0.0000001 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 3b8a73c..8f9ebd7 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -180,11 +180,6 @@ def configureButtonBindings(self) -> None: ) ) - # set Shooter to climb position - self.driverController.y().onTrue( - ShooterROT(SuperStrucConstants.ClimbPos, self.shooter) - ) - # Deliver to amp (button a), part a self.driverController.a().onTrue( commands2.SequentialCommandGroup( @@ -220,18 +215,10 @@ def configureButtonBindings(self) -> None: # Climbing self.driverController.pov(0).whileTrue( - Climb( - 1, - self.climber, - self.shooter, - ) + ShooterROT(SuperStrucConstants.HarmonyPos, self.shooter) ) self.driverController.pov(180).whileTrue( - Climb( - -1, - self.climber, - self.shooter, - ) + ShooterROT(SuperStrucConstants.ClimbPos, self.shooter) ) self.driverController.leftBumper().whileTrue( @@ -279,8 +266,8 @@ def configureButtonBindings(self) -> None: # shooter keybinds # shooter movement - self.opController.button(3).whileTrue(manualROT(0.5, self.shooter)) - self.opController.button(4).whileTrue(manualROT(-0.5, self.shooter)) + self.opController.button(3).whileTrue(manualROT(0.2, self.shooter)) + self.opController.button(4).whileTrue(manualROT(-0.2, self.shooter)) # climber self.opController.button(5).whileTrue(Climb(0.4, self.climber, self.shooter)) diff --git a/rio/simgui-window.json b/rio/simgui-window.json index c93cc85..29e2c34 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -26,7 +26,7 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "169,184" + "Size": "283,140" }, "###Joysticks": { "Collapsed": "0", diff --git a/rio/subsystems/intake.py b/rio/subsystems/intake.py index 96a6613..b5b26f3 100644 --- a/rio/subsystems/intake.py +++ b/rio/subsystems/intake.py @@ -87,7 +87,7 @@ def periodic(self) -> None: self.sd.putNumber( "Thermals/Intake/Rotate", self.liftMotor.getMotorTemperature() ) - # print(self.liftEncoder.getPosition()) + logging.debug(f" Lift encoder is {self.liftEncoder.getPosition()}") def intake(self, speed): self.intakeMotor.set(speed)