Fixing disabled shooting motor for testing, adding an odometry board.

sonic-howl · Apr 15, 2024 · 29db3c9 · 29db3c9
1 parent 3f6f9ec
commit 29db3c9
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 29 deletions.
diff --git a/src/launcher.py b/src/launcher.py
@@ -10,10 +10,10 @@ def __init__(self):
 
     self.feed = rev.CANSparkMax(12, rev.CANSparkLowLevel.MotorType.kBrushless)
 
-    if wpilib.RobotBase.isReal():
-      self.elevator = rev.CANSparkMax(13, rev.CANSparkLowLevel.MotorType.kBrushed)
-    else:
-      self.elevator = rev.CANSparkMax(13, rev.CANSparkLowLevel.MotorType.kBrushless)
+    # if wpilib.RobotBase.isReal():
+    #   self.elevator = rev.CANSparkMax(13, rev.CANSparkLowLevel.MotorType.kBrushed)
+    # else:
+    #   self.elevator = rev.CANSparkMax(13, rev.CANSparkLowLevel.MotorType.kBrushless)
 
     # Limit switches
     self.upperswitch = wpilib.DigitalInput(1)
@@ -28,7 +28,7 @@ def stop(self):
     self.motor2.set(0.0)
 
     self.feed.set(0.0)
-    self.elevator.set(0.0)
+    # self.elevator.set(0.0)
     self.firetimer = 75  # In robot cycles
 
   def shoot(self, fire: float):
@@ -38,8 +38,8 @@ def shoot(self, fire: float):
       # Stop unrelated motors
       # self.elevator.set(0.0)
       # Spool up rollers
-      # self.motor1.set(-fire)
-      # self.motor2.set(fire)
+      self.motor1.set(-fire)
+      self.motor2.set(fire)
       self.firetimer = self.firetimer - 1
 
       # Feed ring into launcher when we are at the lowered position
@@ -53,8 +53,8 @@ def unjams(self):
     # Stop unrelated motors
     # self.elevator.set(0.0)
     # Reverse rollers
-    # self.motor1.set(0.25)
-    # self.motor2.set(-0.25)
+    self.motor1.set(0.25)
+    self.motor2.set(-0.25)
     self.feed.set(-0.5)
 
   ### Elevator is currently not physically implemented. Might be added later. ###
@@ -102,8 +102,8 @@ def unjams(self):
 
   def pickup(self):
     # # Stop unrelated motors
-    # self.motor1.set(0.0)
-    # self.motor2.set(0.0)
+    self.motor1.set(0.0)
+    self.motor2.set(0.0)
     # self.elevator.set(0.0)
     # Stop when note reaches limit switch in the feed mechanism
     if not self.feedswitch.get():
@@ -114,8 +114,8 @@ def pickup(self):
 
   def eject(self):
     # Stop unrelated motors
-    # self.motor1.set(0.0)
-    # self.motor2.set(0.0)
+    self.motor1.set(0.0)
+    self.motor2.set(0.0)
     # self.elevator.set(0.0)
     # Push note back out the pickup
     self.feed.set(-0.5)
diff --git a/src/robot.py b/src/robot.py
@@ -177,25 +177,25 @@ def autonomousInit(self):
         wpimath.geometry.Pose2d(kX / inchToM, kY / inchToM, kRotation)
       )
 
-    self.autoMoveCounter = 75
-    self.autoShootCounter = 100
+    # self.autoMoveCounter = 75
+    # self.autoShootCounter = 100
     # else the gyro is probably broken if it is still calibrating, degraded operations tbd.
 
   def autonomousPeriodic(self):
     """This function is called periodically during autonomous."""
-    if self.autoShootCounter >= 0:
-      self.autoShootCounter = self.autoShootCounter - 1
-      self.launcher.shoot(1.0)
-    elif self.autoShootCounter < 0 and self.autoMoveCounter >= 0:
-      self.autoMoveCounter = self.autoMoveCounter - 1
-      self.launcher.stop()
-      if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kBlue:
-        self.drivebase.setvelocity(-0.3, 0.0, 0.0)
-      else:
-        self.drivebase.setvelocity(0.3, 0.0, 0.0)
-    else:
-      self.drivebase.stop()
-      self.launcher.stop()
+    # if self.autoShootCounter >= 0:
+    #   self.autoShootCounter = self.autoShootCounter - 1
+    #   self.launcher.shoot(1.0)
+    # elif self.autoShootCounter < 0 and self.autoMoveCounter >= 0:
+    #   self.autoMoveCounter = self.autoMoveCounter - 1
+    #   self.launcher.stop()
+    #   if wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kBlue:
+    #     self.drivebase.setvelocity(-0.3, 0.0, 0.0)
+    #   else:
+    #     self.drivebase.setvelocity(0.3, 0.0, 0.0)
+    # else:
+    #   self.drivebase.stop()
+    #   self.launcher.stop()
 
     self.setOutputs()
 
@@ -219,7 +219,7 @@ def teleopPeriodic(self):
       )
 
     magnitude = abs(self.strafe) + abs(self.drive) + abs(self.turn)
-    if utils.utils.dz(magnitude) > 0:
+    if utils.utils.dz(magnitude) > 0.025:
       if (
         wpilib.DriverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed
         and self.drivebase.field_oriented