Transfer files

Larry/.deploy_cfg

@@ -0,0 +1,3 @@
+[auth]
+hostname = 6343
+

Larry/PID_Tuning_Results.txt

@@ -0,0 +1,17 @@
+18917 -->> 100%
+11620 -->> 60%
+
+kF = 0.0528227194492255
+Original_Error_=_652
+
+(10%*1023)/(error)=_starting_P_value
+102.3/652=0.1569018404907975
+0.067258382642998
+
+kF = 0.05282272
+kP = 0.3 (it oscillated too much if we went higher)
+kD = 3
+I_Zone = 130
+kI = 0.002
+
+This gets fairly close to zero. Gets 1 - 2 units off.

Larry/Updated_PID.txt

@@ -0,0 +1,7 @@
+Talon Fx 4,5
+
+kF Same as others
+kI Zone = 150
+kP = 0.2
+kI = 0.004
+kD = 2

Larry/commands/drive_single_module.py

@@ -0,0 +1,29 @@
+import typing
+import commands2
+from subsystems.swerve_drive import SwerveDrive
+import wpilib
+import constants
+import conversions
+import math
+import ctre
+
+class DriveSingleModule(commands2.CommandBase):
+    def __init__(self,  swerveDrive: SwerveDrive, x: typing.Callable[[], float], y: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.drive = swerveDrive
+        #self.units = conversions.convertDegreesToTalonFXUnits(conversions.convertJoystickInputToDegrees(x(), y()))
+        self.x = x
+        self.y = y
+        self.addRequirements([self.drive])
+
+    def execute(self) -> None:
+        self.angle = conversions.convertJoystickInputToDegrees(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.magnitude = math.hypot(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.drive.turnWheel(self.drive.rightRearSwerveModule, self.angle, self.magnitude)
+
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False

Larry/commands/drive_with_controller.py

@@ -0,0 +1,57 @@
+import typing
+import commands2
+from subsystems.swerve_drive import SwerveDrive
+import constants
+import conversions
+import math
+import wpilib
+
+class DriveWithController(commands2.CommandBase):
+    def __init__(self,  swerveDrive: SwerveDrive, x: typing.Callable[[], float], y: typing.Callable[[], float], rightx: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.drive = swerveDrive
+        self.x = x
+        self.y = y
+        self.rightx = rightx
+        self.addRequirements([self.drive])
+        self.drive.reset()
+
+    def execute(self) -> None:
+        self.angle = conversions.convertJoystickInputToDegrees(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.magnitude = math.hypot(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        #self.magnitude *= 0.5
+        #self.angle -= self.drive.getGyroAngle()
+
+        if self.magnitude >= 1.0:
+            self.magnitude = 1.0
+
+        if self.magnitude == 0.0:
+            # only rotation
+            self.drive.turnInPlace(conversions.deadband(self.rightx(), constants.kdeadband))
+        else:
+            self.drive.translate(self.angle, self.magnitude)
+            """
+            if self.magnitude != 0.0 and self.rightx != 0.0:
+                # checks if both joysticks are being used
+                self.drive.moveWhileSpinning(self.x(), self.y(), self.rightx())
+            else:
+                # no rotation wanted
+                self.drive.translate(self.angle, self.magnitude)
+                """
+
+        self.drive.showWheelStats()
+        wpilib.SmartDashboard.putNumber(" Turn Power -", conversions.deadband(self.rightx(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber(" Angle -", self.angle)
+        wpilib.SmartDashboard.putNumber(" Magnitude -", self.magnitude)
+        wpilib.SmartDashboard.putNumber(" X -", conversions.deadband(self.x(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber(" Y -", conversions.deadband(self.y(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber(" Gyro -", self.drive.getGyroAngle())
+
+
+
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False

Larry/commands/joysticks.py

@@ -0,0 +1,34 @@
+import commands2
+import wpilib
+import typing
+from subsystems.swerve_wheel import SwerveWheel
+import conversions
+import constants
+class Joysticks(commands2.CommandBase):
+
+    def __init__(self, robotcontainer: SwerveWheel, LeftX: typing.Callable[[], float], LeftY: typing.Callable[[], float], RightX: typing.Callable[[], float], RightY: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.robotcontainer = robotcontainer
+
+        self.leftx = LeftX
+        self.lefty = LeftY
+        self.rightx = RightX
+        self.righty = RightY
+        #self.degrees = conversions.Conversions.convertJoystickInputToDegrees(LeftX(), LeftY())
+
+        self.addRequirements((self.robotcontainer))
+
+    def execute(self) -> None:
+        #self.degrees = conversions.Conversions.convertJoystickInputToDegrees(self.leftx(), self.lefty())
+        wpilib.SmartDashboard.putNumber("   LeftX - ", conversions.deadband(self.leftx(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber("   Left Y - ", conversions.deadband(self.lefty(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber("   RightX - ", conversions.deadband(self.rightx(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber("   Right Y - ", conversions.deadband(self.righty(), constants.kdeadband))
+        #wpilib.SmartDashboard.putNumber("   Left Degrees - ", self.degrees)
+
+
+    def end(self, interrupted: bool) -> None:
+        print()
+
+    def isFinished(self) -> bool:
+        return False

Larry/commands/move_in_place.py

@@ -0,0 +1,24 @@
+import typing
+import commands2
+from subsystems.swerve_drive import SwerveDrive
+import wpilib
+import conversions
+import constants
+
+class MoveInPlace(commands2.CommandBase):
+    def __init__(self,  swerveDrive: SwerveDrive, rightx: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.drive = swerveDrive
+        #self.units = conversions.convertDegreesToTalonFXUnits(conversions.convertJoystickInputToDegrees(x(), y()))
+        self.x = rightx
+        self.addRequirements([self.drive])
+
+    def execute(self) -> None:
+        self.drive.turnInPlace(conversions.deadband(self.x(), constants.kdeadband))
+        wpilib.SmartDashboard.putNumber("Turn Power -", conversions.deadband(self.x(), constants.kdeadband))
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False

Larry/commands/translate.py

@@ -0,0 +1,26 @@
+import typing
+import commands2
+from subsystems.swerve_drive import SwerveDrive
+import constants
+import conversions
+import math
+
+class Translate(commands2.CommandBase):
+    def __init__(self,  swerveDrive: SwerveDrive, x: typing.Callable[[], float], y: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.drive = swerveDrive
+        self.x = x
+        self.y = y
+        self.addRequirements([self.drive])
+
+    def execute(self) -> None:
+        self.angle = conversions.convertJoystickInputToDegrees(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.magnitude = math.hypot(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.drive.translate(self.angle, self.magnitude)
+
+
+    def end(self, interrupted: bool) -> None:
+        self.drive.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False

Larry/commands/turn_to_specific_point.py

@@ -0,0 +1,73 @@
+import typing
+import commands2
+from subsystems.swerve_wheel import SwerveWheel
+import wpilib
+import constants
+import conversions
+import math
+import ctre
+
+class TurnToSpecificPoint(commands2.CommandBase):
+    def __init__(self,  wheel: SwerveWheel, x: typing.Callable[[], float], y: typing.Callable[[], float]) -> None:
+        super().__init__()
+        self.wheel = wheel
+        #self.units = conversions.convertDegreesToTalonFXUnits(conversions.convertJoystickInputToDegrees(x(), y()))
+        self.x = x
+        self.y = y
+        self.addRequirements([self.wheel])
+
+    def execute(self) -> None:
+        self.angle = conversions.convertJoystickInputToDegrees(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        self.units = conversions.convertDegreesToTalonFXUnits(self.angle)
+        self.magnitude = math.hypot(conversions.deadband(self.x(), constants.kdeadband), conversions.deadband(self.y(), constants.kdeadband))
+        if self.magnitude >= 1.0:
+            self.magnitude = 1.0
+        # we calculate what we need to rotate the wheel to, keeping in mind the gear ratios
+
+        #insert translate code here
+        currentAngle = conversions.convertTalonFXUnitsToDegrees(self.wheel.directionMotor.getSelectedSensorPosition())
+        if math.fabs(self.angle) >= 180.0:
+            opposAngle = math.fabs(self.angle) - 180.0
+        else:
+            opposAngle = math.fabs(self.angle) + 180.0
+        wpilib.SmartDashboard.putNumber(" Original Angle -", self.angle)
+        wpilib.SmartDashboard.putNumber(" Abs Opposit Angle -", opposAngle)
+        if self.magnitude != 0.0:
+            if math.fabs(currentAngle - self.angle) <= math.fabs(currentAngle - opposAngle):
+                #turn to the original angle
+                if self.angle == 0.0:
+                    if (2048*constants.ksteeringGearRatio) - self.units < self.wheel.directionMotor.getSelectedSensorPosition():
+                        self.wheel.turn(2048*constants.ksteeringGearRatio)
+                    else:
+                        self.wheel.turn(0.0)
+                else:
+                    self.wheel.turn(self.units*constants.ksteeringGearRatio)
+                #move command
+                self.wheel.move(self.magnitude)
+            else:
+                #turn to the other angle
+                if self.angle == 0.0:
+                    if (2048*constants.ksteeringGearRatio) - conversions.convertDegreesToTalonFXUnits(opposAngle) < self.wheel.directionMotor.getSelectedSensorPosition():
+                        self.wheel.turn(2048*constants.ksteeringGearRatio)
+                    else:
+                        self.wheel.turn(0.0)
+                else:
+                #change direction of the speed motor
+                    self.wheel.turn(conversions.convertDegreesToTalonFXUnits(opposAngle)*constants.ksteeringGearRatio)
+                #move command
+                self.wheel.move(-self.magnitude)
+
+        self.wheel.showStats()
+        wpilib.SmartDashboard.putNumber("Setpoint -", self.units)
+        wpilib.SmartDashboard.putNumber("Angle -", self.angle)
+        wpilib.SmartDashboard.putNumber("Input to wheel -", self.units*constants.ksteeringGearRatio)
+        wpilib.SmartDashboard.putNumber("Magnitude -", self.magnitude)
+        wpilib.SmartDashboard.putNumber("Difference fro 2048 -", float((2048*constants.ksteeringGearRatio) - self.wheel.directionMotor.getSelectedSensorPosition()))
+
+
+    def end(self, interrupted: bool) -> None:
+        self.wheel.stopAllMotors()
+
+    def isFinished(self) -> bool:
+        return False
+

Larry/constants.py

@@ -0,0 +1,35 @@
+import math
+
+# motors
+kleftFrontSpeedID = 0
+kleftRearSpeedID = 1
+krightFrontSpeedID = 2
+krightRearSpeedID = 3
+
+kleftFrontDirectionID = 4
+kleftRearDirectionID = 5
+krightFrontDirectionID = 6
+krightRearDirectionID = 7
+
+
+kdriverControllerPort = 0
+kdeadband = 0.15
+
+#Encoders
+ktimeoutMs = 10
+kF = 0.05282272 #Feed forward
+kP = 0.2 #Proportional
+kI = 0.004 #Integral
+kD = 2 #Derivative
+kIzone = 150
+kcruiseVel = 10567.0 #Cruise Velocity at 50% of max
+kcruiseAccel = 10567.0 #Cruise Acceleration same as velocity
+kticksPerRev = 2048
+kSlotIdx = 0
+kPIDLoopIdx = 0 
+
+ksteeringGearRatio = 150/7
+
+klength = 29
+kwidth = 29
+kr = math.sqrt((klength**2)+(kwidth**2)) # ** is the exponent operator

Larry/conversions.py

@@ -0,0 +1,38 @@
+import math
+
+def convertDegreesToTalonFXUnits(num: float) -> float:
+    conversionFactor = 2048/360 # 1 revolution of the falcon 500 is 2048 units
+    num *= conversionFactor
+    return num
+
+def convertTalonFXUnitsToDegrees(num: float) -> float:
+    num *= (360/2048)
+    return num
+
+def sign(num) -> int:
+    if num > 0:
+        # positive
+        return 1
+    elif num < 0:
+        # negative
+        return -1
+    else:
+        # zero
+        return 0
+
+def convertJoystickInputToDegrees(x: float, y: float): # this allows us to use the x and y values on the joystick and convert it into degrees
+    if sign(x) == -1:
+        return float(math.degrees(math.atan2(x, -y)) + 360.0) # this will make sure that it gives us a number between 0 and 360
+    else:
+        if float(math.degrees(math.atan2(x, -y))) == 360.0: #This makes sure that if we get 360.0 degrees, it will be zero
+            return 0.0
+        else:
+            return float(math.degrees(math.atan2(x, -y))) # the degrees, the joystick up is zero and the values increase clock-wise
+
+def deadband(value: float, band: float):
+     # this makes sure that joystick drifting is not an issue. 
+     # It takes the small values and forces it to be zero if smaller than the band value
+    if math.fabs(value) <= band:
+        return 0
+    else:
+        return value

Larry/robot.py

@@ -0,0 +1,37 @@
+import wpilib
+from robotcontainer import RobotContainer
+import commands2
+#vision setup
+
+class MyRobot(commands2.TimedCommandRobot):
+   def robotInit(self) -> None:
+        #initialize robotcontainer
+        self.container = RobotContainer()
+        #autonomous
+        #self.autonomousCommand = self.container.getAutonomousCommand()
+        #launch camera
+
+   def disabledInit(self) -> None:
+      """Things to do once disabled (only occurs once when disabled)"""
+
+   def disabledPeriodic(self) -> None:
+      """Things to do when disabled (repeated)"""
+
+   def autonomousInit(self) -> None:
+
+      """self.autonomousCommand = self.container.getAutonomousCommand()
+
+      if self.autonomousCommand:
+         self.autonomousCommand.schedule()"""
+
+   def teleopInit(self) -> None:
+      """
+      if self.autonomousCommand:
+          self.autonomousCommand.cancel()
+      """
+   def testInit(self) -> None:
+      #cancel all running commands at the once you enter test mode
+      commands2.CommandScheduler.getInstance().cancelAll()
+
+if __name__ == "__main__":
+   wpilib.run(MyRobot)