Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Event/reading (#27)
Browse files Browse the repository at this point in the history
* adding a auto to just shoot and showing all on dashboard

* everything bropkrhbhgar

* gfhyuj

* WORKING

* feild relative works

* works

* savestate 0

* savestate 1

* addRequirements commands, correctly invert controller controls, use optimizedDesiredState

* disable this rateLimit if statement, fixes j hook problem

* get reset yaw angle from nt value

* move auto mode selector to Auto/Mode, add Auto/Angle value

---------

Co-authored-by: kredcool <[email protected]>
Co-authored-by: dubluayaychtee <[email protected]>
  • Loading branch information
3 people authored Mar 19, 2024
1 parent 0dd7ea6 commit 3eab8f8
Show file tree
Hide file tree
Showing 17 changed files with 221 additions and 80 deletions.
11 changes: 11 additions & 0 deletions Pipfile
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
[[source]]
url = "https://pypi.org/simple"
verify_ssl = true
name = "pypi"

[packages]

[dev-packages]

[requires]
python_version = "3.11"
48 changes: 48 additions & 0 deletions rio/autonomous/flywithwires.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
import commands2
import wpilib

from subsystems.drivesubsystem import DriveSubsystem


class FlyWithWires(commands2.CommandBase):
def __init__(
self, driveSubsystem: DriveSubsystem, xSpeed=0, ySpeed=0, rot=0, time=-1
) -> None:
super().__init__()

self.driveSubsystem = driveSubsystem
self.addRequirements((self.driveSubsystem))

# time and derection vars
self.time = time
self.xSpeed = xSpeed
self.ySpeed = ySpeed
self.rot = rot

# Timer
self.backgroundTimer = wpilib.Timer()
self.backgroundTimer.start()

print("initalized")

def initialize(self) -> None:
self.backgroundTimer.reset()

def execute(self) -> None:
self.driveSubsystem.drive(
self.xSpeed,
self.ySpeed,
self.rot,
False,
True,
)
print("running")

def end(self, interrupted: bool) -> None:
self.driveSubsystem.drive(0, 0, 0, False, True)
print("ended")

def isFinished(self) -> bool:
if self.time != -1 and self.backgroundTimer.hasElapsed(self.time):
return True
print("finished")
39 changes: 39 additions & 0 deletions rio/autonomous/forwardDrive.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
import commands2

from subsystems.drivesubsystem import DriveSubsystem
from subsystems.intake import IntakeSubsystem
from subsystems.shooter import Shooter

from commands.FlyWheelSpeed import FlyWheelSpeed
from commands.setIntakeSpeed import SetIntakeSpeed

from autonomous.flywithwires import FlyWithWires


class ForwardDrive(commands2.SequentialCommandGroup):
def __init__(
self,
driveSubsystem: DriveSubsystem,
shooterSubsytem: Shooter,
intakeSubsystem: IntakeSubsystem,
) -> None:
"""
This shoots a note and drives backwards
this wants the note to start in the intake (folded up)
with the limit switch pressed
"""

super().__init__(
FlyWheelSpeed(1, shooterSubsytem, False), # Power up the flywheels
SetIntakeSpeed(-0.6, intakeSubsystem), # Load magazine
commands2.WaitCommand(
3
), # this needs to be here because it won't hold speed otherwise
FlyWheelSpeed(0.0, shooterSubsytem), # Stop flywheel
SetIntakeSpeed(0, intakeSubsystem).withTimeout(3), # Stop intake
FlyWithWires(driveSubsystem, 1, 0, 0, 2), # driving
)


NAME = "Forward Drive"
load = lambda bot: ForwardDrive(bot.robotDrive, bot.shooter, bot.intake)
4 changes: 4 additions & 0 deletions rio/autonomous/grabNote.py
Original file line number Diff line number Diff line change
Expand Up @@ -63,3 +63,7 @@ def __init__(
# ================= #
PoseReset(_drive),
)


NAME = "Grab Note"
load = lambda bot: GrabNote(bot.limelight, bot.shooter, bot.intake, bot.robotDrive)
11 changes: 9 additions & 2 deletions rio/autonomous/noAuto.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,5 +5,12 @@

class NoAuto(commands2.SequentialCommandGroup):
def __init__(self, _drive: DriveSubsystem):
self.drivetrain = _drive
super().__init__(ResetYaw(_drive))
"""
This doesn't do anything besides
reset yaw
"""
super().__init__(ResetYaw(_drive, 0))


NAME = "No Auto"
load = lambda bot: NoAuto(bot.robotDrive)
49 changes: 49 additions & 0 deletions rio/autonomous/shoot.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
import commands2

from subsystems.drivesubsystem import DriveSubsystem
from subsystems.intake import IntakeSubsystem
from subsystems.shooter import Shooter

from commands.rotateIntake import RotateIntake
from commands.FlyWheelSpeed import FlyWheelSpeed
from commands.setIntakeSpeed import SetIntakeSpeed
from commands.ResetYaw import ResetYaw
from commands.zeroPose import zeroPose

from constants import IntakeConstants


class Shoot(commands2.SequentialCommandGroup):
def __init__(
self,
_drive: DriveSubsystem,
_intake: IntakeSubsystem,
_shooter: Shooter,
):
"""
This just shoots the note,
useful for being with teams
with super autos
"""
super().__init__(
# Resets Yaw relative to the robot's starting position
ResetYaw(_drive),
zeroPose(_drive),
# ============ #
# SPEAKER SHOT #
# ============ #
RotateIntake(
IntakeConstants.BlowPos, _intake
), # Put intake fully inside (if it wasn't already)
print("intake rotatered"),
FlyWheelSpeed(1.0, _shooter, False), # Power up the flywheels (?)
print("wheels r fly"),
SetIntakeSpeed(-0.6, _intake), # Load magazine? (but without ending)
commands2.WaitCommand(1),
FlyWheelSpeed(0.0, _shooter), # Stop flywheel
SetIntakeSpeed(0, _intake), # Stop intake)
)


NAME = "Shoot"
load = lambda bot: Shoot(bot.robotDrive, bot.intake, bot.shooter)
9 changes: 7 additions & 2 deletions rio/commands/ResetYaw.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
import logging

from subsystems.drivesubsystem import DriveSubsystem
from ntcore import NetworkTableInstance


class ResetYaw(commands2.Command):
Expand All @@ -12,12 +13,16 @@ def __init__(
):
super().__init__()

# Configure networktables
self.nt = NetworkTableInstance.getDefault()
self.sd = self.nt.getTable("SmartDashboard")

# local subsystem instance
self.subsystem = _drivetrain
self.angle = angle

def initialize(self):
self.subsystem.resetYaw(self.angle)
print("i b setn' 0 t' " + str(self.sd.getNumber("Auto/Angle", 1)))
self.subsystem.resetYaw(self.sd.getNumber("Auto/Angle", 1))

def execute(self):
pass
Expand Down
39 changes: 0 additions & 39 deletions rio/commands/crashDrive.py

This file was deleted.

5 changes: 3 additions & 2 deletions rio/commands/loadMagazine.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import logging
import commands2
import rev

from subsystems.shooter import Shooter
from subsystems.intake import IntakeSubsystem
Expand All @@ -22,7 +23,7 @@ def __init__(self, _shooter: Shooter, _intake: IntakeSubsystem):

def initialize(self):
logging.info("Spinning up...")
self.shooter.setIdleBrake()
self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kBrake)

def execute(self):
self.intake.intake(-0.6) # Slowly eject
Expand All @@ -41,7 +42,7 @@ def isFinished(self):
def end(self, interrupted: bool):
self.intake.intake(0)
# self.shooter.setFlyWheelSpeed(0)
self.shooter.setIdleCoast()
self.shooter.setIdleMode(rev._rev.CANSparkBase.IdleMode.kCoast)

if not interrupted:
logging.info(f"Done, magazine loaded")
Expand Down
3 changes: 2 additions & 1 deletion rio/commands/poseReset.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,10 @@ def __init__(

# local subsystem instance
self.drivetrain = _drivetrain
self.addRequirements((self.drivetrain))

def initialize(self):
# what do I put here? :3
# what do I put here? :3c
pass

def execute(self):
Expand Down
10 changes: 5 additions & 5 deletions rio/constants.py
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,10 @@ class DriveConstants:

# Distance between front and back wheels on robot
kModulePositions = [
Translation2d(kWheelBase / 2, kTrackWidth / 2),
Translation2d(kWheelBase / 2, -kTrackWidth / 2),
Translation2d(-kWheelBase / 2, kTrackWidth / 2),
Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
Translation2d(kWheelBase / 2, kTrackWidth / 2),
Translation2d(kWheelBase / 2, -kTrackWidth / 2),
]
kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions)

Expand All @@ -63,7 +63,7 @@ class DriveConstants:
kFrontRightTurningCanId = 2
kRearRightTurningCanId = 4

kGyroReversed = False
kGyroReversed = True


class ModuleConstants:
Expand Down Expand Up @@ -179,11 +179,11 @@ class IntakeConstants:

# inversion
kLiftInversion = False
kIntakeInversion = False
kIntakeInversion = True

# conversion factor
kLiftConversion = 360 # Configured feb 12 by joe
SuckPos = 197 # 347
SuckPos = 120
BlowPos = 1
# lift pid
kLiftP = 0.075
Expand Down
Loading

0 comments on commit 3eab8f8

Please sign in to comment.