diff --git a/GearsBot/commands/autonomous.py b/GearsBot/commands/autonomous.py index f6a58a5..256d31c 100644 --- a/GearsBot/commands/autonomous.py +++ b/GearsBot/commands/autonomous.py @@ -6,8 +6,8 @@ import commands2 import commands2.cmd -import preparetopickup -import ..constants +from . import preparetopickup +import constants from subsystems.drivetrain import Drivetrain from subsystems.claw import Claw from subsystems.elevator import Elevator diff --git a/GearsBot/commands/closeclaw.py b/GearsBot/commands/closeclaw.py index 93b0102..739aaf5 100644 --- a/GearsBot/commands/closeclaw.py +++ b/GearsBot/commands/closeclaw.py @@ -6,7 +6,7 @@ import commands2 from subsystems.claw import Claw -import ..robot +import robot class CloseClaw(commands2.Command): """Closes the claw until the limit switch is tripped.""" @@ -30,5 +30,5 @@ def end(self, interrupted: bool) -> None: # can to fall out + there is no need to worry about stalling the motor # or crushing the can. - if !robot.MyRobot.isSimulation(): + if not robot.MyRobot.isSimulation(): self.claw.stop() diff --git a/GearsBot/commands/pickup.py b/GearsBot/commands/pickup.py index 2931125..8a85aab 100644 --- a/GearsBot/commands/pickup.py +++ b/GearsBot/commands/pickup.py @@ -6,10 +6,10 @@ import commands2 import commands2.cmd -import closeclaw -import setwristsetpoint -import setelevatorsetpoint -import ..constants +import constants +from . import closeclaw +from . import setwristsetpoint +from . import setelevatorsetpoint from subsystems.claw import Claw from subsystems.elevator import Elevator from subsystems.wrist import Wrist diff --git a/GearsBot/commands/place.py b/GearsBot/commands/place.py index 1b6896e..59b9496 100644 --- a/GearsBot/commands/place.py +++ b/GearsBot/commands/place.py @@ -5,10 +5,10 @@ # import commands2 -import setelevatorsetpoint -import setwristsetpoint -import openclaw -import ..constants +import constants +from . import openclaw +from . import setelevatorsetpoint +from . import setwristsetpoint from subsystems.claw import Claw from subsystems.elevator import Elevator from subsystems.wrist import Wrist diff --git a/GearsBot/commands/preparetopickup.py b/GearsBot/commands/preparetopickup.py index c292d45..ef830aa 100644 --- a/GearsBot/commands/preparetopickup.py +++ b/GearsBot/commands/preparetopickup.py @@ -6,12 +6,12 @@ import commands2 import commands2.cmd -import setwristsetpoint -import setelevatorsetpoint -import openclaw +from . import setwristsetpoint +from . import setelevatorsetpoint +from . import openclaw from subsystems.elevator import Elevator from subsystems.wrist import Wrist -from subsystem.claw import Claw +from subsystems.claw import Claw class PrepareToPickup(commands2.SequentialCommandGroup): def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None: diff --git a/GearsBot/subsystems/claw.py b/GearsBot/subsystems/claw.py index 88d39d0..9cd7bcd 100644 --- a/GearsBot/subsystems/claw.py +++ b/GearsBot/subsystems/claw.py @@ -6,7 +6,7 @@ import wpilib import commands2 -import ..constants +import constants class Claw(commands2.Subsystem): """The claw subsystem is a simple system with a motor for opening and closing. diff --git a/GearsBot/subsystems/drivetrain.py b/GearsBot/subsystems/drivetrain.py index f011afa..7b05a0f 100644 --- a/GearsBot/subsystems/drivetrain.py +++ b/GearsBot/subsystems/drivetrain.py @@ -8,10 +8,10 @@ import wpilib.drive import wpiutil import commands2 -import ..constants -import ..robot +import constants +import robot -class Drivetrain(commands2.Subsystem): +class Drivetrain(commands2.SubsystemBase): """The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis. These include four drive motors, a left and right encoder and a gyro.""" @@ -25,7 +25,10 @@ def __init__(self) -> None: self.rightLeader = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port) self.rightFollower = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port) - self.drive = wpilib.drive.DifferentialDrive(self.leftLeader, self.rightLeader) + self.drive = wpilib.drive.DifferentialDrive( + lambda s : self.leftLeader.set(s), + lambda s : self.rightLeader.set(s) + ) self.leftEncoder = wpilib.Encoder( constants.DriveConstants.kLeftEncoderPorts[0], diff --git a/GearsBot/subsystems/elevator.py b/GearsBot/subsystems/elevator.py index cf1905b..9b5193e 100644 --- a/GearsBot/subsystems/elevator.py +++ b/GearsBot/subsystems/elevator.py @@ -7,8 +7,8 @@ import wpilib import wpimath.controller import commands2 -import ..constants -import ..robot +import constants +import robot class Elevator(commands2.PIDSubsystem): def __init__(self) -> None: diff --git a/GearsBot/subsystems/wrist.py b/GearsBot/subsystems/wrist.py index 9be88bc..01eec99 100644 --- a/GearsBot/subsystems/wrist.py +++ b/GearsBot/subsystems/wrist.py @@ -7,8 +7,8 @@ import commands2 import wpilib import wpimath.controller -import ..constants -import ..robot +import constants +import robot class Wrist(commands2.PIDSubsystem): """The wrist subsystem is like the elevator, but with a rotational