Fixed Motion Magic Issue

SteelRidgeRobotics · Jun 30, 2022 · 5a81ebc · 5a81ebc
1 parent 24b71ab
commit 5a81ebc
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 7 deletions.
diff --git a/Larry/commands/turn_to_specific_point.py b/Larry/commands/turn_to_specific_point.py
@@ -22,4 +22,4 @@ def end(self, interrupted: bool) -> None:
         self.wheel.stopAllMotors()
 
     def isFinished(self) -> bool:
-        return self.wheel.rotating
+        return self.wheel.isNotrotating
diff --git a/Larry/robotcontainer.py b/Larry/robotcontainer.py
@@ -44,7 +44,7 @@ def __init__(self) -> None:
         ##self.swerveDrive.setDefaultCommand(DriveSwerveWheelByJoysticks(self.swerveDrive, lambda: self.driverController.getLeftX(), lambda: self.driverController.getRightY()))
     def configureButtonBindings(self):
         """This is where our trigger bindings for commands go"""
-        (JoystickButton(self.driverController, XboxController.Button.kA).whenPressed(TurnToSpecificPoint(self.swerveDrive, 204800)))
+        (JoystickButton(self.driverController, XboxController.Button.kA).whenPressed(TurnToSpecificPoint(self.swerveDrive, 5000)))
     """    
     def getAutonomousCommand(self) -> commands2.Command:
         return self.chooser.getSelected()

diff --git a/Larry/subsystems/swerve_wheel.py b/Larry/subsystems/swerve_wheel.py
@@ -32,31 +32,38 @@ def __init__(self) -> None:
         self.directionMotor.configMotionCruiseVelocity(kcruiseVel, ktimeoutMs)
         self.directionMotor.configMotionAcceleration(kcruiseAccel, ktimeoutMs)
 
-        self.directionMotor.setNeutralMode(ctre.NeutralMode.Brake)
+        self.directionMotor.setNeutralMode(ctre.NeutralMode.Coast)
 
-        self.directionMotor.setSelectedSensorPosition(0.0, kPIDLoopIdx, ktimeoutMs)
+        #self.directionMotor.setSelectedSensorPosition(0.0, kPIDLoopIdx, ktimeoutMs)
 
         wpilib.SmartDashboard.putNumber(" P -", kP)
         wpilib.SmartDashboard.putNumber(" I -", kI)
         wpilib.SmartDashboard.putNumber(" D -", kD)
         wpilib.SmartDashboard.putNumber(" F -", kF)
         wpilib.SmartDashboard.putNumber(" Sensor Position -", self.directionMotor.getSelectedSensorPosition())
-    def turn(self, joystick_input: float):
+        self.isNotrotating = True
+
+    def turn(self, set_point: float):
+        self.isNotrotating = False
         current_pos = self.directionMotor.getSelectedSensorPosition()
-        self.directionMotor.set(ctre.TalonFXControlMode.MotionMagic, current_pos)
-        self.rotating = True
+        self.directionMotor.set(ctre.TalonFXControlMode.MotionMagic, int(set_point))
         wpilib.SmartDashboard.putNumber("Sensor - ", current_pos)
+
     def move(self, joystick_input: float):
         self.speedMotor.set(ctre.TalonFXControlMode.PercentOutput, 0.1*joystick_input)
+
     def stopAllMotors(self):
         self.directionMotor.set(ctre.TalonFXControlMode.PercentOutput, 0.0)
         self.speedMotor.set(ctre.TalonFXControlMode.PercentOutput, 0.0)
+
     def showStats(self):
         wpilib.SmartDashboard.putNumber(" P -", kP)
         wpilib.SmartDashboard.putNumber(" I -", kI)
         wpilib.SmartDashboard.putNumber(" D -", kD)
         wpilib.SmartDashboard.putNumber(" F -", kF)
         wpilib.SmartDashboard.putNumber(" Sensor Position -", self.directionMotor.getSelectedSensorPosition())
+        wpilib.SmartDashboard.putBoolean(" Is Not Moving? -", self.isNotrotating)
+
 
         """
         # This allows us to change the values for the PIDF Controller