Merge pull request #1 from Team-4536/cleanup

deleted some code and runs sim

src/robot.py

@@ -3,7 +3,6 @@
 import profiler
 import robotHAL
 import wpilib
-from lightControl import LightControl
 from ntcore import NetworkTableInstance
 from pathplannerlib.controller import PIDConstants, PPHolonomicDriveController
 from PIDController import PIDController, PIDControllerForArm, updatePIDsInNT
@@ -16,12 +15,6 @@
 
 
 class RobotInputs():
-    TARGET_NONE = 0
-    TARGET_LEFT = 1
-    TARGET_RIGHT = 2
-    TARGET_SUBWOOFER = 3
-    TARGET_SOURCE = 4
-
     def __init__(self) -> None:
         self.driveCtrlr = wpilib.XboxController(0)
         self.armCtrlr = wpilib.XboxController(1)
@@ -62,7 +55,6 @@ def robotInit(self) -> None:
 
         self.driveGyroYawOffset = 0.0 # the last angle that drivers reset the field oriented drive to zero at
 
-
         self.autoSideChooser = wpilib.SendableChooser()
         wpilib.SmartDashboard.putData('auto side chooser', self.autoSideChooser)
 
@@ -84,21 +76,7 @@ def robotPeriodic(self) -> None:
 
         self.hal.publish(self.table)
 
-        self.drive.updateOdometry(self.hal)
-
-        pose = self.drive.odometry.getPose()
-        self.table.putNumber("odomX", pose.x )
-        self.table.putNumber("odomY", pose.y)
-        self.odomField.setRobotPose(pose)
-
-        self.table.putBoolean("ctrl/absOn", self.abs)
-        self.table.putNumber("ctrl/absOffset", self.driveGyroYawOffset)
-        self.table.putNumber("ctrl/driveX", self.input.driveX)
-        self.table.putNumber("ctrl/driveY", self.input.driveY)
-
-
         updatePIDsInNT()
-        self.table.putNumber("Offset yaw", -self.hal.yaw + self.driveGyroYawOffset)
 
     def teleopInit(self) -> None:
         pass
@@ -111,41 +89,16 @@ def teleopPeriodic(self) -> None:
         turnScalar = 6
         driveVector = Translation2d(self.input.driveX * speedControlEdited, self.input.driveY * speedControlEdited)
         turnVector = Translation2d(self.input.turningY, self.input.turningX) #for pid only
-        #absolute drive
-        if self.abs:
-            driveVector = driveVector.rotateBy(Rotation2d(-self.hal.yaw + self.driveGyroYawOffset))
-
-        #disable pid when stick moved
-        if (self.input.turningX != 0 and self.rightStickToggle == False) or self.input.lineUpWithSubwoofer:
-            self.PIDtoggle = False
-
-        self.drive.update(self.time.dt, self.hal, speed)
 
         self.hardware.update(self.hal, self.time)
 
     def autonomousInit(self) -> None:
         # when simulating, initalize sim to have a preloaded ring
         if isinstance(self.hardware, RobotSimHAL):
-            pass
-
-        self.holonomicController = PPHolonomicDriveController(
-            PIDConstants(1, 0, 0),
-            PIDConstants(self.turnPID.kp, self.turnPID.ki, self.turnPID.kd,),
-            5.0,
-            self.drive.modulePositions[0].distance(Translation2d()))
-
-        self.auto, initialPose = self.autoSubsys.autoInit(self)
-
-        self.driveGyroYawOffset = initialPose.rotation().radians()
-        self.hardware.resetGyroToAngle(initialPose.rotation().radians())
-        self.hardware.update(self.hal, self.time)
-        self.drive.resetOdometry(initialPose, self.hal)
-        self.holonomicController.reset(initialPose, ChassisSpeeds())
-
+            pass        
 
     def autonomousPeriodic(self) -> None:
         self.hal.stopMotors()
-        self.auto.run(self)
         self.hardware.update(self.hal, self.time)
 
     def disabledInit(self) -> None:

src/robotHAL.py

@@ -28,10 +28,6 @@ class RobotHAL():
     def __init__(self) -> None:
         self.prev = RobotHALBuffer()
 
-    def resetGyroToAngle(self, ang: float) -> None:
-        self.gyro.reset()
-        self.gyro.setAngleAdjustment(-math.degrees(ang))
-
     def update(self, buf: RobotHALBuffer, time: TimeData) -> None:
         prev = self.prev
         self.prev = copy.deepcopy(buf)

src/simHAL.py

@@ -4,7 +4,6 @@
 from ntcore import NetworkTableInstance
 from real import angleWrap, lerp
 from robotHAL import RobotHALBuffer
-from swerveDrive import SwerveDrive
 from timing import TimeData
 from wpimath.geometry import Rotation2d, Translation2d
 
@@ -30,107 +29,3 @@ def __init__(self):
     def update(self, buf: RobotHALBuffer, time: TimeData) -> None:
         # prev = self.prev
         self.prev = copy.deepcopy(buf)
-
-        # UPDATE WHEEL VELOCITIES
-        prefs = ["FL", "FR", "BL", "BR"]
-        for i in range(4):
-            self.driveVels[i] = lerp(self.driveVels[i], buf.driveVolts[i] * 1/0.2, 0.2)
-            self.steerVels[i] = lerp(self.steerVels[i], buf.steeringVolts[i] * 20, 0.3)
-            self.table.putNumber(prefs[i] + "SimSteerVel", self.steerVels[i])
-            self.table.putNumber(prefs[i] + "SimDriveVel", self.driveVels[i])
-
-        # UPDATE HAL BUFFER WITH WHEEL DATA, CALCULATE YAW DELTA
-        angleDeltaSum = 0.0
-        for i in range(4):
-            buf.steeringPositions[i] += self.steerVels[i] * time.dt
-            buf.driveSpeedMeasured[i] = self.driveVels[i]
-
-            driveDist = self.driveVels[i] * time.dt
-            buf.drivePositions[i] += driveDist
-
-            new = Translation2d(driveDist, 0)
-            new = new.rotateBy(Rotation2d(buf.steeringPositions[i]))
-            new += SwerveDrive.modulePositions[i]
-            old = SwerveDrive.modulePositions[i]
-            delta = angleWrap(math.atan2(new.y, new.x) - math.atan2(old.y, old.x))
-            angleDeltaSum += delta
-        self.yaw += angleDeltaSum / 4
-        buf.yaw = self.yaw
-
-        # SIMULATE RING POSITION, TRANSITION, AND SENSOR READINGS
-        if self.ringPos == 0:
-            buf.intakeSensor = False
-            buf.shooterSensor = False
-            if buf.intakeSpeeds[0] > 0.001:
-
-                if self.ringTransitionStart == -1:
-
-                    self.ringTransitionStart = time.timeSinceInit
-                else:
-                    if (time.timeSinceInit - self.ringTransitionStart) > 0.4:
-                        self.ringPos = 1
-                        self.ringTransitionStart = -1
-            else:
-                self.ringTransitionStart = -1
-        elif self.ringPos == 1:
-            buf.intakeSensor = True
-            buf.shooterSensor = False
-            if buf.shooterIntakeSpeed > 0.001:
-
-                if self.ringTransitionStart == -1:
-
-                    self.ringTransitionStart = time.timeSinceInit
-                else:
-                    if (time.timeSinceInit - self.ringTransitionStart) > 0.4:
-                        self.ringPos = 2
-                        self.ringTransitionStart = -1
-            else:
-                self.ringTransitionStart = -1
-        elif self.ringPos == 2:
-            buf.intakeSensor = False
-            buf.shooterSensor = True
-            if buf.shooterIntakeSpeed > 0.001:
-
-                if self.ringTransitionStart == -1:
-
-                    self.ringTransitionStart = time.timeSinceInit
-                else:
-                    if (time.timeSinceInit - self.ringTransitionStart) > 0.2:
-                        self.ringPos = 0
-                        self.ringTransitionStart = -1
-            else:
-                self.ringTransitionStart = -1
-        self.table.putNumber("ringPos", self.ringPos)
-        self.table.putNumber("ringTransitionStart", self.ringTransitionStart)
-
-        # SHOOTER VELOCITY, ARM POSITION
-        self.shooterAngVel = lerp(self.shooterAngVel, buf.shooterSpeed * 1/0.00181, 0.4)
-        self.shooterAimVel = lerp(self.shooterAimVel, buf.shooterAimSpeed * 1, 0.1)
-        self.shooterAimVel -= 0.005 * math.cos(self.shooterAimPos + 0.1)
-
-        self.shooterAimPos += self.shooterAimVel
-        self.shooterAimPos = max(0, min(self.shooterAimPos, 1.9))
-
-        buf.shooterAimPos = self.shooterAimPos
-        buf.shooterAngVelocityMeasured = self.shooterAngVel
-
-        # TODO: cam sim
-
-
-    # TODO: gyro yaw sim
-    def resetGyroToAngle(self, ang: float) -> None:
-        self.yaw = ang
-
-    def resetCamEncoderPos(self, nPos: float) -> None:
-        pass
-
-    def resetClimbEncoderPos(self, nPos: float) -> None:
-        pass
-
-    def resetAimEncoderPos(self, nPos: float) -> None:
-        pass
-
-    def setLEDs(self, r: int, g: int, b: int, w: int = 0, startIdx: int = 0, count: int = 0) -> None:
-        pass
-
-    # TODO: test to verify that this and the actual hal have the same signature