From 5a81ebce92533b337da579631ed4552861cae4fa Mon Sep 17 00:00:00 2001 From: Steel Ridge Robotics Date: Thu, 30 Jun 2022 14:25:13 -0700 Subject: [PATCH] Fixed Motion Magic Issue --- Larry/commands/turn_to_specific_point.py | 2 +- Larry/robotcontainer.py | 2 +- Larry/subsystems/swerve_wheel.py | 17 ++++++++++++----- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/Larry/commands/turn_to_specific_point.py b/Larry/commands/turn_to_specific_point.py index 30f2a6c..5d17259 100644 --- 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 \ No newline at end of file + return self.wheel.isNotrotating \ No newline at end of file diff --git a/Larry/robotcontainer.py b/Larry/robotcontainer.py index d21ce1d..cf06df4 100644 --- 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 index bd039ee..985456e 100644 --- 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