From 7e8e4fa5265d4c78e60d1b2e9cf7de12d4b00f6d Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Thu, 29 Feb 2024 12:07:22 -0300 Subject: [PATCH 1/5] add gearsbot example --- GearsBot/commands/autonomous.py | 42 ++++++++ GearsBot/commands/closeclaw.py | 34 +++++++ GearsBot/commands/drivestraight.py | 47 +++++++++ GearsBot/commands/openclaw.py | 28 ++++++ GearsBot/commands/pickup.py | 36 +++++++ GearsBot/commands/place.py | 35 +++++++ GearsBot/commands/preparetopickup.py | 35 +++++++ GearsBot/commands/setdistancetobox.py | 40 ++++++++ GearsBot/commands/setelevatorsetpoint.py | 33 ++++++ GearsBot/commands/setwristsetpoint.py | 29 ++++++ GearsBot/commands/tankdrive.py | 39 +++++++ GearsBot/constants.py | 85 ++++++++++++++++ GearsBot/robot.py | 66 ++++++++++++ GearsBot/robotcontainer.py | 49 +++++++++ GearsBot/subsystems/claw.py | 48 +++++++++ GearsBot/subsystems/drivetrain.py | 123 +++++++++++++++++++++++ GearsBot/subsystems/elevator.py | 65 ++++++++++++ GearsBot/subsystems/wrist.py | 62 ++++++++++++ 18 files changed, 896 insertions(+) create mode 100644 GearsBot/commands/autonomous.py create mode 100644 GearsBot/commands/closeclaw.py create mode 100644 GearsBot/commands/drivestraight.py create mode 100644 GearsBot/commands/openclaw.py create mode 100644 GearsBot/commands/pickup.py create mode 100644 GearsBot/commands/place.py create mode 100644 GearsBot/commands/preparetopickup.py create mode 100644 GearsBot/commands/setdistancetobox.py create mode 100644 GearsBot/commands/setelevatorsetpoint.py create mode 100644 GearsBot/commands/setwristsetpoint.py create mode 100644 GearsBot/commands/tankdrive.py create mode 100644 GearsBot/constants.py create mode 100644 GearsBot/robot.py create mode 100644 GearsBot/robotcontainer.py create mode 100644 GearsBot/subsystems/claw.py create mode 100644 GearsBot/subsystems/drivetrain.py create mode 100644 GearsBot/subsystems/elevator.py create mode 100644 GearsBot/subsystems/wrist.py diff --git a/GearsBot/commands/autonomous.py b/GearsBot/commands/autonomous.py new file mode 100644 index 00000000..f6a58a56 --- /dev/null +++ b/GearsBot/commands/autonomous.py @@ -0,0 +1,42 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import commands2.cmd +import preparetopickup +import ..constants +from subsystems.drivetrain import Drivetrain +from subsystems.claw import Claw +from subsystems.elevator import Elevator +from subsystems.wrist import Wrist + +class Autonomous(commands2.SequentialCommandGroup): + """The main autonomous command to pickup and deliver the soda to the box.""" + + def __init__(self, drive: Drivetrain, claw: Claw, wrist: Wrist, elevator: + Elevator) -> None: + """Create a new autonomous command.""" + super().__init__() + + self.addCommands( + preparetopickup.PrepareToPickup(claw, wrist, elevator), + pickup.Pickup(claw, wrist, elevator), + setdistancetobox.SetDistanceToBox( + constants.AutoConstants.kDistToBox1, drive + ), + # drivestraight.DriveStraight(4) # Use encoders if ultrasonic is broken + place.Place(claw, wrist, elevator), + setdistancetobox.SetDistanceToBox( + constants.AutoConstants.kDistToBox2, drive + ), + # drivestraight.DriveStraight(-2) # Use encoders if ultrasonic is broken + commands2.parallel( + setwristsetpoint.SetWristSetpoint( + constants.AutoConstants.kWristSetpoint, wrist + ), + closeclaw.CloseClaw(claw) + ) + ) diff --git a/GearsBot/commands/closeclaw.py b/GearsBot/commands/closeclaw.py new file mode 100644 index 00000000..93b01026 --- /dev/null +++ b/GearsBot/commands/closeclaw.py @@ -0,0 +1,34 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +from subsystems.claw import Claw +import ..robot + +class CloseClaw(commands2.Command): + """Closes the claw until the limit switch is tripped.""" + + def __init__(self, claw: Claw) -> None: + self.claw = claw + self.addRequirements(self.claw) + + def initialize(self) -> None: + # Called just before this Command runs the first time + self.claw.close() + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return self.claw.isGrabbing() + + def end(self, interrupted: bool) -> None: + # Called once after isFinished returns true + + # NOTE: Doesn't stop in simulation due to lower friction causing the + # can to fall out + there is no need to worry about stalling the motor + # or crushing the can. + + if !robot.MyRobot.isSimulation(): + self.claw.stop() diff --git a/GearsBot/commands/drivestraight.py b/GearsBot/commands/drivestraight.py new file mode 100644 index 00000000..ea7d6191 --- /dev/null +++ b/GearsBot/commands/drivestraight.py @@ -0,0 +1,47 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import wpimath.controller +import ..constants2 +from subsystems.drivetrain import Drivetrain + +class DriveStraight(commands2.PIDCommand): + """Drive the given distance straight (negative values go backwards). + Uses a local PID controller to run a simple PID loop that is only + enabled while this command is running. The input is the + averaged values of the left and right encoders. + """ + + def __init__(self, distance: float, drivetrain: Drivetrain) -> None: + """Create a new DriveStraight command. + + :param distance: The distance to drive + """ + super().__init__( + wpimath.controller.PIDController( + constants.DriveStraightConstants.kP, + constants.DriveStraightConstants.kI, + constants.DriveStraightConstants.kD, + ), + drivetrain.getDistance(), + distance, + lambda d: drivetrain.drive(d, d) + ) + + self.drivetrain = drivetrain + self.addRequirements(self.drivetrain) + self.getController().setTolerance(0.01) + + def initialize(self) -> None: + # Called just before this Command runs the first time + # Get everything in a safe starting state. + self.drivetrain.reset() + super().initialize() + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return self.getController().atSetpoint() diff --git a/GearsBot/commands/openclaw.py b/GearsBot/commands/openclaw.py new file mode 100644 index 00000000..abb9524e --- /dev/null +++ b/GearsBot/commands/openclaw.py @@ -0,0 +1,28 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +from subsystems.claw import Claw + +class OpenClaw(commands2.WaitCommand): + """Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!""" + def __init__(self, claw: Claw) -> None: + """Creates a new OpenClaw command. + + :param claw: The claw to use + """ + super().__init__(1) + self.claw = claw + self.addRequirements(self.claw) + + def initialize(self) -> None: + # Called just before this Command runs the first time + self.claw.open() + super().initialize() + + def end(self, interrupted: bool) -> None: + # Called once after isFinished returns true + self.claw.stop() diff --git a/GearsBot/commands/pickup.py b/GearsBot/commands/pickup.py new file mode 100644 index 00000000..29311250 --- /dev/null +++ b/GearsBot/commands/pickup.py @@ -0,0 +1,36 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import commands2.cmd +import closeclaw +import setwristsetpoint +import setelevatorsetpoint +import ..constants +from subsystems.claw import Claw +from subsystems.elevator import Elevator +from subsystems.wrist import Wrist + +class Pickup(commands2.SequentialCommandGroup): + """Pickup a soda can (if one is between the open claws) and get it in a + safe state to drive around.""" + + def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None: + """Create a new pickup command. + + :param claw: The claw subsystem to use + :param wrist: The wrist subsystem to use + :param elevator: The elevator subsystem to use + """ + self.addCommands(closeclaw.CloseClaw(claw)) + commands2.cmd.parallel( + setwristsetpoint.SetWristSetpoint( + constants.Position.Pickup.kWristSetpoint, wrist + ), + setelevatorsetpoint.SetElevatorSetpoint( + constants.Position.Pickup.kElevatorSetpoint, elevator + ), + ) diff --git a/GearsBot/commands/place.py b/GearsBot/commands/place.py new file mode 100644 index 00000000..1b6896e7 --- /dev/null +++ b/GearsBot/commands/place.py @@ -0,0 +1,35 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import setelevatorsetpoint +import setwristsetpoint +import openclaw +import ..constants +from subsystems.claw import Claw +from subsystems.elevator import Elevator +from subsystems.wrist import Wrist + +class Place(commands2.SequentialCommandGroup): + """Place a held soda can onto the platform.""" + + def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None: + """Create a new place command. + + :param claw: The claw subsystem to use + :param wrist: The wrist subsystem to use + :param elevator: The elevator subsystem to use + """ + super().__init__() + self.addCommands( + setelevatorsetpoint.SetElevatorSetpoint( + constants.Positions.Place.kElevatorSetpoint, elevator + ), + setwristsetpoint.SetWristSetpoint( + constants.Positions.Place.kWristSetpoint, wrist + ), + openclaw.OpenClaw(claw) + ) diff --git a/GearsBot/commands/preparetopickup.py b/GearsBot/commands/preparetopickup.py new file mode 100644 index 00000000..c292d45a --- /dev/null +++ b/GearsBot/commands/preparetopickup.py @@ -0,0 +1,35 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import commands2.cmd +import setwristsetpoint +import setelevatorsetpoint +import openclaw +from subsystems.elevator import Elevator +from subsystems.wrist import Wrist +from subsystem.claw import Claw + +class PrepareToPickup(commands2.SequentialCommandGroup): + def __init__(self, claw: Claw, wrist: Wrist, elevator: Elevator) -> None: + """Create a new prepare to pickup command. + + :param claw: The claw subsystem to use + :param wrist: The wrist subsystem to use + :param elevator: The elevator subsystem to use + """ + super().__init__() + self.addCommands( + openclaw.OpenClaw(claw), + commands2.cmd.parallel( + setwristsetpoint.SetWristSetpoint( + constants.Positions.Place.kWristSetpoint, wrist + ), + setelevatorsetpoint.SetElevatorSetpoint( + constants.Positions.Place.kElevatorSetpoint, elevator + ), + ) + ) diff --git a/GearsBot/commands/setdistancetobox.py b/GearsBot/commands/setdistancetobox.py new file mode 100644 index 00000000..29e305e0 --- /dev/null +++ b/GearsBot/commands/setdistancetobox.py @@ -0,0 +1,40 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import wpimath.controller +from subsystems.drivetrain import Drivetrain + +class SetDistanceToBox(commands2.PIDCommand): + """Drive until the robot is the given distance away from the box. Uses a local + PID controller to run a simple PID loop that is only enabled while this command is + running. The input is the averaged values of the left and right encoders. + """ + + def __init__(self, distance: float, drivetrain: Drivetrain) -> None: + """Create a new set distance to box command. + + :param distance: The distance away from the box to drive to + """ + super().__init__( + wpimath.controller.PIDController(-2, 0, 0), + drivetrain.getDistanceToObstacle(), + distance, + lambda d : drivetrain.drive(d, d) + ) + self.drivetrain = drivetrain + self.addRequirements(self.drivetrain) + self.getController().setTolerance(0.01) + + def initialize(self) -> None: + # Called just before this Command runs the first time + # Get everything in a safe starting state. + self.drivetrain.reset() + super().initialize() + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return self.getController().atSetpoint() diff --git a/GearsBot/commands/setelevatorsetpoint.py b/GearsBot/commands/setelevatorsetpoint.py new file mode 100644 index 00000000..eeb99cfa --- /dev/null +++ b/GearsBot/commands/setelevatorsetpoint.py @@ -0,0 +1,33 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +from subsystems.elevator import Elevator + +class SetElevatorSetpoint(commands2.Command): + """Move the elevator to a given location. This command finishes when it is + within the tolerance, but leaves the PID loop running to maintain the position. + Other commands using the elevator should make sure they disable PID! + """ + def __init__(self, setpoint: float, elevator: Elevator) -> None: + """Create a new SetElevatorSetpoint command. + + :param setpoint: The setpoint to set the elevator to + :param elevator: The elevator to use + """ + super().__init__() + self.elevator = elevator + self.setpoint = setpoint + self.addRequirements(self.elevator) + + def initialize(self) -> None: + # Called just before this Command runs the first time + self.elevator.setSetpoint(self.setpoint) + self.elevator.enable() + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return self.elevator.getController().atSetpoint() diff --git a/GearsBot/commands/setwristsetpoint.py b/GearsBot/commands/setwristsetpoint.py new file mode 100644 index 00000000..7fb6a921 --- /dev/null +++ b/GearsBot/commands/setwristsetpoint.py @@ -0,0 +1,29 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +from subsystems.wrist import Wrist + +class SetWristSetpoint(commands2.Command): + def __init__(self, setpoint: float, wrist: Wrist) -> None: + """Create a new SetWristSetpoint command. + + :param setpoint: The setpoint to set the wrist to + :param wrist: The wrist to use + """ + super().__init__() + self.wrist = wrist + self.setpoint = setpoint + self.addRequirements(self.wrist) + + def initialize(self) -> None: + # Called just before this Command runs the first time + self.wrist.setSetpoint(self.setpoint) + self.wrist.enable() + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return self.wrist.getController().atSetpoint() diff --git a/GearsBot/commands/tankdrive.py b/GearsBot/commands/tankdrive.py new file mode 100644 index 00000000..bde6805a --- /dev/null +++ b/GearsBot/commands/tankdrive.py @@ -0,0 +1,39 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import typing +import commands2 +from subsystems.drivetrain import Drivetrain + +class TankDrive(commands2.Command): + """Have the robot drive tank style.""" + + def __init__(self, left: typing.Callable[[], float], right: + typing.Callable[[], float], drivetrain: Drivetrain) -> None: + """Creates a new TankDrive command. + + :param left: The control input for the left side of the drive + :param right: The control input for the right sight of the drive + :param drivetrain: The drivetrain subsystem to drive + """ + super().__init__() + + self.drivetrain = drivetrain + self.left = left + self.right = right + self.addRequirements([self.drivetrain]) + + def execute(self) -> None: + # Called repeatedly when this Command is scheduled to run + self.drivetrain.drive(self.left(), self.right()) + + def isFinished(self) -> bool: + # Make this return true when this Command no longer needs to run execute() + return False + + def end(self, interrupted: bool) -> None: + # Called once after isFinished returns true + self.drivetrain.drive(0, 0) diff --git a/GearsBot/constants.py b/GearsBot/constants.py new file mode 100644 index 00000000..a55743a9 --- /dev/null +++ b/GearsBot/constants.py @@ -0,0 +1,85 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +""" +The constants module is a convenience place for teams to hold robot-wide +numerical or boolean constants. Don't use this for any other purpose! +""" + +import math + +class DriveConstants: + # The PWM IDs for the drivetrain motor controllers. + kLeftMotor1Port = 0 + kLeftMotor2Port = 1 + kRightMotor1Port = 2 + kRightMotor2Port = 3 + + # Encoders and their respective motor controllers. + kLeftEncoderPorts = (0, 1) + kRightEncoderPorts = (2, 3) + kLeftEncoderReversed = False + kRightEncoderReversed = True + + # Encoder counts per revolution/rotation. + kEncoderCPR = 1024 + kWheelDiameterInches = 6 + + # Assumes the encoders are directly mounted on the wheel shafts + kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + +class ClawConstants: + kMotorPort = 7 + kContactPort = 5 + +class WristConstants: + kMotorPort = 6 + + # These pid constants are not real, and will need to be tuned + kP = 0.1 + kI = 0.0 + kD = 0.0 + + kTolerance = 2.5 + kPotentiometerPort = 3; + +class ElevatorConstants: + kMotorPort = 5 + kPotentiometerPort = 2 + + # These pid constants are not real, and will need to be tuned + kP_real = 4 + kI_real = 0.007 + + kP_simulation = 18 + kI_simulation = 0.2 + + kD = 0.0 + + kTolerance = 0.005 + +class AutoConstants: + kDistToBox1 = 0.10 + kDistToBox2 = 0.60 + kWristSetpoint = -45.0 + +class DriveStraightConstants: + kP = 4.0 + kI = 0.0 + kD = 0.0 + +class Positions: + class Pickup: + kWristSetpoint = -45.0 + kElevatorSetpoint = 0.2 + + class Place: + kWristSetpoint = 0.0 + kElevatorSetpoint = 0.25 + + class PrepareToPickup: + kWristSetpoint = 0.0 + kElevatorSetpoint = 0.0 diff --git a/GearsBot/robot.py b/GearsBot/robot.py new file mode 100644 index 00000000..c3ecb437 --- /dev/null +++ b/GearsBot/robot.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import commands2 +import typing + +from robotcontainer import RobotContainer + + +class MyRobot(commands2.TimedCommandRobot): + """ + Command v2 robots are encouraged to inherit from TimedCommandRobot, which + has an implementation of robotPeriodic which runs the scheduler for you + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + def robotInit(self) -> None: + """ + This function is run when the robot is first started up and should be used for any + initialization code. + """ + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = RobotContainer() + + def disabledInit(self) -> None: + """This function is called once each time the robot enters Disabled mode.""" + pass + + def disabledPeriodic(self) -> None: + """This function is called periodically when disabled""" + pass + + def autonomousInit(self) -> None: + """This autonomous runs the autonomous command selected by your RobotContainer class.""" + self.autonomousCommand = self.container.getAutonomousCommand() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + def autonomousPeriodic(self) -> None: + """This function is called periodically during autonomous""" + pass + + def teleopInit(self) -> None: + # This makes sure that the autonomous stops running when + # teleop starts running. If you want the autonomous to + # continue until interrupted by another command, remove + # this line or comment it out. + if self.autonomousCommand: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + pass + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/GearsBot/robotcontainer.py b/GearsBot/robotcontainer.py new file mode 100644 index 00000000..0bde382b --- /dev/null +++ b/GearsBot/robotcontainer.py @@ -0,0 +1,49 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import constants +import commands2.button +import commands2.cmd + +class RobotContainer: + """ + This class is where the bulk of the robot should be declared. Since Command-based is a + "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` + periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + subsystems, commands, and button mappings) should be declared here. + """ + + def __init__(self) -> None: + # The robot's subsystems + self.robotDrive = DriveSubsystem() + self.robotArm = ArmSubsystem() + + # The driver's controller + self.driverController = commands2.button.CommandXboxController( + constants.kDriverControllerPort + ) + # self.driverController = wpilib.Joystick(constants.kDriverControllerPort) + + # Configure the button bindings + self.configureButtonBindings() + + # Set the default drive command + self.robotDrive.setDefaultCommand( + self.robotDrive.arcadeDriveCommand( + lambda: -self.driverController.getLeftY(), + lambda: -self.driverController.getRightX(), + ) + ) + + def configureButtonBindings(self) -> None: + """ + Use this method to define your button->command mappings. Buttons can be created by + instantiating a :GenericHID or one of its subclasses (Joystick or XboxController), + and then passing it to a JoystickButton. + """ + + def getAutonomousCommand(self) -> commands2.Command: + return commands2.cmd.none() diff --git a/GearsBot/subsystems/claw.py b/GearsBot/subsystems/claw.py new file mode 100644 index 00000000..88d39d03 --- /dev/null +++ b/GearsBot/subsystems/claw.py @@ -0,0 +1,48 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import commands2 +import ..constants + +class Claw(commands2.Subsystem): + """The claw subsystem is a simple system with a motor for opening and closing. + If using stronger motors, you should probably use a sensor so that the motors don't stall.""" + + def __init__(self) -> None: + """Create a new claw subsystem.""" + super().__init__() + + self.motor = wpilib.Victor(constants.ClawConstants.kMotorPort) + self.contact = wpilib.DigitalInput(constants.ClawConstants.kContactPort) + + # Let's name everything on the LiveWindow + addChild("Motor", self.motor) + addChild("Limit Switch", self.contact) + + def log(self) -> None: + wpilib.SmartDashboard.putData("Claw switch", self.contact) + + def open(self) -> None: + """Set the claw motor to move in the open direction.""" + self.motor.set(-1) + + def close(self) -> None: + """Set the claw motor to move in the close direction.""" + self.motor.set(1) + + def stop(self) -> None: + """Stops the claw motor from moving.""" + self.motor.set(0) + + def isGrabbing(self) -> bool: + """Return true when the robot is grabbing an object hard enough to + trigger the limit switch.""" + return self.contact.get() + + def periodic(self) -> None: + """Call log method every loop.""" + self.log() diff --git a/GearsBot/subsystems/drivetrain.py b/GearsBot/subsystems/drivetrain.py new file mode 100644 index 00000000..f011afaa --- /dev/null +++ b/GearsBot/subsystems/drivetrain.py @@ -0,0 +1,123 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import wpilib.drive +import wpiutil +import commands2 +import ..constants +import ..robot + +class Drivetrain(commands2.Subsystem): + """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.""" + + def __init__(self) -> None: + """Create a new drivetrain subsystem.""" + super().__init__() + + self.leftLeader = wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor1Port) + self.leftFollower = wpilib.PWMSparkMax(constants.DriveConstants.kLeftMotor2Port) + self.rightLeader = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor1Port) + self.rightFollower = wpilib.PWMSparkMax(constants.DriveConstants.kRightMotor2Port) + + self.drive = wpilib.drive.DifferentialDrive(self.leftLeader, self.rightLeader) + + self.leftEncoder = wpilib.Encoder( + constants.DriveConstants.kLeftEncoderPorts[0], + constants.DriveConstants.kLeftEncoderPorts[1], + constants.DriveConstants.kLeftEncoderReversed + ) + + self.rightEncoder = wpilib.Encoder( + constants.DriveConstants.kRightEncoderPorts[0], + constants.DriveConstants.kRightEncoderPorts[1], + constants.DriveConstants.kRightEncoderReversed + ) + + self.rangefinder = wpilib.AnalogInput(constants.DriveConstants.kRangeFinderPort) + self.gyro = wpilib.AnalogGyro(constants.DriveConstants.kAnalogGyroPort) + + wpiutil.SendableRegistry.addChild(self.drive, self.leftLeader) + wpiutil.SendableRegistry.addChild(self.drive, self.rightLeader) + + self.leftLeader.addFollower(self.leftFollower) + self.rightLeader.addFollower(self.rightFollower) + + # We need to invert one side of the drivetrain so that positive voltages + # result in both sides moving forward. Depending on how your robot's + # gearbox is constructed, you might have to invert the left side instead. + self.rightLeader.setInverted(true) + + # Encoders may measure differently in the real world and in + # simulation. In this example the robot moves 0.042 barleycorns + # per tick in the real world, but the simulated encoders + # simulate 360 tick encoders. This if statement allows for the + # real robot to handle this difference in devices. + if robot.MyRobot.isReal(): + self.leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse) + else: + # Circumference = diameter in feet * pi. 360 tick simulated encoders. + self.leftEncoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0) + self.rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0) + + # Let's name the sensors on the LiveWindow + self.addChild("Drive", self.drive); + self.addChild("Left Encoder", self.leftEncoder); + self.addChild("Right Encoder", self.rightEncoder); + self.addChild("Rangefinder", self.rangefinder); + self.addChild("Gyro", self.gyro); + + def log(self) -> None: + """The log method puts interesting information to the SmartDashboard.""" + wpilib.SmartDashboard.putNumber("Left Distance", self.leftEncoder.getDistance()) + wpilib.SmartDashboard.putNumber("Right Distance", self.rightEncoder.getDistance()) + wpilib.SmartDashboard.putNumber("Left Speed", self.leftEncoder.getRate()) + wpilib.SmartDashboard.putNumber("Right Speed", self.rightEncoder.getRate()) + wpilib.SmartDashboard.putNumber("Gyro", self.gyro.getAngle()) + + def drive(self, left: float, right: float) -> None: + """Tank style driving for the Drivetrain. + + :param left: Speed in range [-1,1] + :param right: Speed in range [-1,1] + """ + self.drive.tankDrive(left, right) + + def getHeading(self) -> float: + """Get the robot's heading. + + :returns: The robots heading in degrees. + """ + return self.gyro.getAngle() + + def reset(self) -> None: + """Reset the robots sensors to the zero states.""" + self.gyro.reset() + self.leftEncoder.reset() + self.rightEncoder.reset() + + def getDistance(self) -> float: + """Get the average distance of the encoders since the last reset. + + :returns: The distance driven (average of left and + right encoders). + """ + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2 + + def getDistanceToObstacle(self) -> float: + """Get the distance to the obstacle. + + :returns: The distance to the obstacle detected by the rangefinder. + """ + # Really meters in simulation since it's a rangefinder... + return self.rangefinder.getAverageVoltage() + + def periodic(self) -> None: + """Call log method every loop.""" + self.log() diff --git a/GearsBot/subsystems/elevator.py b/GearsBot/subsystems/elevator.py new file mode 100644 index 00000000..cf1905be --- /dev/null +++ b/GearsBot/subsystems/elevator.py @@ -0,0 +1,65 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import wpilib +import wpimath.controller +import commands2 +import ..constants +import ..robot + +class Elevator(commands2.PIDSubsystem): + def __init__(self) -> None: + """Create a new elevator subsystem.""" + super().__init__( + wpimath.controller.PIDController( + constants.ElevatorConstants.kP_real, + constants.ElevatorConstants.kI_real, + constants.ElevatorConstants.kD_real, + ) + ) + + self.motor = wpilib.Victor(constants.ElevatorConstants.kMotorPort) + + if robot.MyRobot.isSimulation(): # Check for simulation and update PID values + self.getController().setPID( + constants.ElevatorConstants.kP_simulation, + constants.ElevatorConstants.kI_simulation, + constants.ElevatorConstants.kD, + ) + self.getController().setTolerance(constants.ElevatorConstants.kTolerance) + + # Conversion value of potentiometer varies between the real world and simulation + if robot.MyRobot.isReal(): + self.pot = wpilib.AnalogPotentiometer( + constants.ElevatorConstants.kPotentiometerPort, -2.0 / 5 + ) + else: + # Defaults to meters + self.pot = wpilib.AnalogPotentiometer( + constants.ElevatorConstants.kPotentiometerPort + ) + + # Let's name everything on the LiveWindow + self.addChild("Motor", self.motor); + self.addChild("Pot", self.pot); + + def log(self) -> None: + """The log method puts interesting information to the SmartDashboard.""" + wpilib.SmartDashboard.putData("Elevator Pot", self.pot) + + def getMeasurement(self) -> None: + """Use the potentiometer as the PID sensor. This method is automatically + called by the subsystem.""" + return self.pot.get() + + def useOutput(self, output: float, setpoint: float) -> None: + """Use the motor as the PID output. This method is automatically called + by the subsystem.""" + self.motor.set(output) + + def periodic(self) -> None: + """Call log method every loop.""" + self.log() diff --git a/GearsBot/subsystems/wrist.py b/GearsBot/subsystems/wrist.py new file mode 100644 index 00000000..9be88bc4 --- /dev/null +++ b/GearsBot/subsystems/wrist.py @@ -0,0 +1,62 @@ +# +# Copyright (c) FIRST and other WPILib contributors. +# Open Source Software; you can modify and/or share it under the terms of +# the WPILib BSD license file in the root directory of this project. +# + +import commands2 +import wpilib +import wpimath.controller +import ..constants +import ..robot + +class Wrist(commands2.PIDSubsystem): + """The wrist subsystem is like the elevator, but with a rotational + joint instead of a linear joint.""" + + def __init__(self) -> None: + """Create a new wrist subsystem.""" + super().__init__( + wpimath.controller.PIDController( + constants.WristConstants.kP, + constants.WristConstants.kI, + constants.WristConstants.kD, + ) + ) + + self.getController().setTolerance(constants.WristConstants.kTolerance) + + self.motor = wpilib.Victor(constants.WristConstants.kMotorPort) + + # Conversion value of potentiometer varies between the real world and simulation + if robot.MyRobot.isReal(): + self.pot = wpilib.AnalogPotentiometer( + constants.ElevatorConstants.kPotentiometerPort, -270.0 / 5 + ) + else: + # Defaults to meters + self.pot = wpilib.AnalogPotentiometer( + constants.ElevatorConstants.kPotentiometerPort + ) + + # Let's name everything on the LiveWindow + self.addChild("Motor", self.motor) + self.addChild("Pot", self.pot) + + def log(self) -> None: + """The log method puts interesting information to the SmartDashboard.""" + wpilib.SmartDashboard.putData("Wrist Angle", self.pot) + + def getMeasurement(self) -> float: + """Use the potentiometer as the PID sensor. This method is automatically + called by the subsystem.""" + return self.pot.get() + + def useOutput(self, output: float, setpoint: float) -> None: + """Use the motor as the PID output. This method is automatically called + by the subsystem.""" + self.motor.set(output) + + def periodic(self) -> None: + """Call log method every loop.""" + self.log(); From ef35b2041fb9220997911c5ad096e5aeaa3a8ae7 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sun, 3 Mar 2024 00:26:53 -0300 Subject: [PATCH 2/5] add 2 more constants --- GearsBot/constants.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/GearsBot/constants.py b/GearsBot/constants.py index a55743a9..d3068949 100644 --- a/GearsBot/constants.py +++ b/GearsBot/constants.py @@ -24,6 +24,9 @@ class DriveConstants: kLeftEncoderReversed = False kRightEncoderReversed = True + kRangeFinderPort = 6 + kAnalogGyroPort = 1 + # Encoder counts per revolution/rotation. kEncoderCPR = 1024 kWheelDiameterInches = 6 From b06ccaddce210bd6abe2cca7af834190d6df2d5c Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sun, 3 Mar 2024 00:27:14 -0300 Subject: [PATCH 3/5] add the buttons and commands --- GearsBot/robotcontainer.py | 109 ++++++++++++++++++++++++++++++------- 1 file changed, 89 insertions(+), 20 deletions(-) diff --git a/GearsBot/robotcontainer.py b/GearsBot/robotcontainer.py index 0bde382b..03b55739 100644 --- a/GearsBot/robotcontainer.py +++ b/GearsBot/robotcontainer.py @@ -4,46 +4,115 @@ # the WPILib BSD license file in the root directory of this project. # +import wpilib import constants +import commands2 import commands2.button -import commands2.cmd +from subsystems.drivetrain import Drivetrain +from subsystems.elevator import Elevator +from subsystems.wrist import Wrist +from subsystems.claw import Claw +from commands.autonomous import Autonomous +from commands.setelevatorsetpoint import SetElevatorSetpoint +from commands.setwristsetpoint import SetWristSetpoint +from commands.openclaw import OpenClaw +from commands.closeclaw import CloseClaw +from commands.tankdrive import TankDrive +from commands.pickup import Pickup +from commands.place import Place class RobotContainer: - """ - This class is where the bulk of the robot should be declared. Since Command-based is a + """This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` periodic methods (other than the scheduler calls). Instead, the structure of the robot (including subsystems, commands, and button mappings) should be declared here. """ def __init__(self) -> None: - # The robot's subsystems - self.robotDrive = DriveSubsystem() - self.robotArm = ArmSubsystem() + """The container for the robot. Contains subsystems, OI devices, and commands.""" + # The robot's subsystems and commands are defined here... + self.drivetrain = Drivetrain() + self.elevator = Elevator() + self.wrist = Wrist() + self.claw = Claw() - # The driver's controller - self.driverController = commands2.button.CommandXboxController( - constants.kDriverControllerPort + self.joystick = wpilib.XboxController(0) + + self.autonomousCommand = Autonomous( + self.drivetrain, self.claw, self.wrist, self.elevator ) - # self.driverController = wpilib.Joystick(constants.kDriverControllerPort) - # Configure the button bindings - self.configureButtonBindings() + # Put Some buttons on the SmartDashboard + wpilib.SmartDashboard.putData( + "Elevator Bottom", SetElevatorSetpoint(0, self.elevator) + ) + wpilib.SmartDashboard.putData( + "Elevator Top", SetElevatorSetpoint(0.25, self.elevator) + ) + wpilib.SmartDashboard.putData( + "Wrist Horizontal", SetWristSetpoint(0, self.wrist) + ) + wpilib.SmartDashboard.putData( + "Raise Wrist", SetWristSetpoint(-45, self.wrist) + ) + wpilib.SmartDashboard.putData( + "Open Claw", OpenClaw(self.claw) + ) + wpilib.SmartDashboard.putData( + "Close Claw", CloseClaw(self.claw) + ) + wpilib.SmartDashboard.putData( + "Deliver Soda", + Autonomous(self.drivetrain, self.claw, self.wrist, self.elevator) + ) - # Set the default drive command - self.robotDrive.setDefaultCommand( - self.robotDrive.arcadeDriveCommand( - lambda: -self.driverController.getLeftY(), - lambda: -self.driverController.getRightX(), - ) + # Assign default commands + self.drivetrain.setDefaultCommand( + TankDrive( + lambda: -self.joystick.getLeftY(), + lambda: -self.joystick.getRightY(), + self.drivetrain + ), ) + # Show what command your subsystem is running on the SmartDashboard + wpilib.SmartDashboard.putData(self.drivetrain) + wpilib.SmartDashboard.putData(self.elevator) + wpilib.SmartDashboard.putData(self.wrist) + wpilib.SmartDashboard.putData(self.claw) + + # Configure the button bindings + self.configureButtonBindings() + def configureButtonBindings(self) -> None: - """ - Use this method to define your button->command mappings. Buttons can be created by + """Use this method to define your button->command mappings. Buttons can be created by instantiating a :GenericHID or one of its subclasses (Joystick or XboxController), and then passing it to a JoystickButton. """ + # Create some buttons + dpadUp = commands2.button.JoystickButton(self.joystick, 5) + dpadRight = commands2.button.JoystickButton(self.joystick, 6) + dpadDown = commands2.button.JoystickButton(self.joystick, 7) + dpadLeft = commands2.button.JoystickButton(self.joystick, 8) + l2 = commands2.button.JoystickButton(self.joystick, 9) + r2 = commands2.button.JoystickButton(self.joystick, 10) + l1 = commands2.button.JoystickButton(self.joystick, 11) + r1 = commands2.button.JoystickButton(self.joystick, 12) + + # Connect the buttons to commands + dpadUp.onTrue(SetElevatorSetpoint(0.25, self.elevator)) + dpadDown.onTrue(SetElevatorSetpoint(0.0, self.elevator)) + dpadRight.onTrue(CloseClaw(self.claw)) + dpadLeft.onTrue(OpenClaw(self.claw)) + + r1.onTrue(PrepareToPickup(self.claw, self.wrist, self.elevator)) + r2.onTrue(Pickup(self.claw, self.wrist, self.elevator)) + l1.onTrue(Place(self.claw, self.wrist, self.elevator)) + l2.onTrue(Autonomous(self.drivetrain, self.claw, self.wrist, self.elevator)) def getAutonomousCommand(self) -> commands2.Command: + """Use this to pass the autonomous command to the main :class:`.Robot` class. + + :returns: the command to run in autonomous + """ return commands2.cmd.none() From da0e89760d1b2da9925ebe7e767f79583e257eb7 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sun, 3 Mar 2024 00:27:41 -0300 Subject: [PATCH 4/5] add GearsBot to test file --- run_tests.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/run_tests.sh b/run_tests.sh index 9d7481f3..2752351d 100755 --- a/run_tests.sh +++ b/run_tests.sh @@ -25,6 +25,7 @@ BASE_TESTS=" FlywheelBangBangController FrisbeeBot GameData + GearsBot GettingStarted Gyro GyroDriveCommands From 5497693ea43d99165e93dbf8847d6425923fa614 Mon Sep 17 00:00:00 2001 From: Guilherme Samuel Date: Sun, 3 Mar 2024 00:27:53 -0300 Subject: [PATCH 5/5] fix import --- GearsBot/commands/autonomous.py | 4 ++-- GearsBot/commands/closeclaw.py | 4 ++-- GearsBot/commands/pickup.py | 8 ++++---- GearsBot/commands/place.py | 8 ++++---- GearsBot/commands/preparetopickup.py | 8 ++++---- GearsBot/subsystems/claw.py | 2 +- GearsBot/subsystems/drivetrain.py | 11 +++++++---- GearsBot/subsystems/elevator.py | 4 ++-- GearsBot/subsystems/wrist.py | 4 ++-- 9 files changed, 28 insertions(+), 25 deletions(-) diff --git a/GearsBot/commands/autonomous.py b/GearsBot/commands/autonomous.py index f6a58a56..256d31c9 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 93b01026..739aaf5d 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 29311250..8a85aabc 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 1b6896e7..59b94968 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 c292d45a..ef830aad 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 88d39d03..9cd7bcd6 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 f011afaa..7b05a0fa 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 cf1905be..9b5193e2 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 9be88bc4..01eec992 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