class MyRobot(MagicRobot):
solenoids = Solenoids
def createObjects(self):
self.shiftBaseSolenoid = wpilib.DoubleSolenoid(
rMap.conf_shifterSolenoid1, rMap.conf_shifterSolenoid2)
self.rightFrontBaseMotor = CANTalon(rMap.conf_rightFrontBaseTalon)
self.rightRearBaseMotor = CANTalon(rMap.conf_rightRearBaseTalon)
self.leftFrontBaseMotor = CANTalon(rMap.conf_leftFrontBaseTalon)
self.leftRearBaseMotor = CANTalon(rMap.conf_leftRearBaseTalon)
self.rightFrontBaseMotor.enableControl()
self.rightRearBaseMotor.enableControl()
self.rightRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower)
self.rightRearBaseMotor.set(rMap.conf_rightFrontBaseTalon)
self.leftFrontBaseMotor.enableControl()
self.leftRearBaseMotor.enableControl()
self.leftFrontBaseMotor.setInverted(True)
self.leftRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower)
self.leftRearBaseMotor.set(rMap.conf_leftFrontBaseTalon)
self.leftJoy = Joystick(rMap.conf_left_joy)
self.rightJoy = Joystick(rMap.conf_right_joy)
self.xbox = Joystick(rMap.conf_xbox)
self.robotDrive = wpilib.RobotDrive(self.leftFrontBaseMotor,
self.rightFrontBaseMotor)
def autonomous(self):
"""Drive left and right motors for two seconds, then stop."""
MagicRobot.autonomous(self)
def teleopInit(self):
MagicRobot.teleopInit(self)
def teleopPeriodic(self):
if self.isSimulation():
# easier to control in simulation
self.robotDrive.arcadeDrive(self.leftJoy)
else:
self.robotDrive.tankDrive(self.leftJoy.getY(),
self.rightJoy.getY())
try:
if self.leftJoy.getRawButton(7):
self.solenoids.setShift()
print("RED " + str(self.solenoids.setShift()))
except:
self.onException()
def test(self):
'''Runs during test mode'''
wpilib.LiveWindow.run()
def upadte_operator(self,
left_stick: Joystick,
right_stick: Joystick = None):
if right_stick is None:
self.arcade_mode = True
self.y_speed = -left_stick.getY()
self.z_speed = -left_stick.getZ()
else:
self.arcade_mode = True
self.left_speed = -left_stick.getY()
self.right_speed = -right_stick.getY()
class Attack3Joystick(Sensor):
"""
Sensor wrapper for the Attack 3 Joystick.
Has boolean attributes for buttons: trigger, button2-9
and double x_axis, y_axis for joystick position
"""
x_axis = y_axis = 0
trigger = button2 = button3 = \
button4 = button5 = button6 = \
button7 = button8 = button9 = \
button10 = button11 = False
def __init__(self, port):
"""
Initializes the joystick with some USB port.
"""
super().__init__()
self.j = Joystick(port)
def poll(self):
for i, state_id in enumerate(BUTTON_TABLE, 1):
self.update_state(state_id,
self.j.getRawButton(i))
self.x_axis = self.j.getX()
self.y_axis = self.j.getY()
class MyRobot(TimedRobot):
# Defines the channels that are used on the inputs. In the simulator, they have to match up with the physics.py file
# This is really useful when you have a variable used hundreds of times and you want to have it set so you can
# change it all in one go.
RLMotorChannel = 2
RRMotorChannel = 0
FLMotorChannel = 3
FRMotorChannel = 4
DriveStickChannel = 0
# RobotInit is where all of the things we are using is initialized.
def robotInit(self):
# Note the lack of the camera server initialization here.
# Initializing drive motors
RLMotor = Spark(self.RLMotorChannel)
RRMotor = Spark(self.RRMotorChannel)
FLMotor = Spark(self.FLMotorChannel)
FRMotor = Spark(self.FRMotorChannel)
# Grouping drive motors into left and right.
self.Left = SpeedControllerGroup(RLMotor, FLMotor)
self.Right = SpeedControllerGroup(RRMotor, FRMotor)
# Initializing drive Joystick.
self.DriveStick = Joystick(self.DriveStickChannel)
# Initializing drive function.
self.drive = DifferentialDrive(self.Left, self.Right)
# Sets the right side motors to be inverted.
self.drive.setRightSideInverted(rightSideInverted=True)
# Turns the drive off after .1 seconds of inactivity.
self.drive.setExpiration(0.1)
# Components is a dictionary that contains any variables you want to put into it. All of the variables put into
# components dictionary is given to the autonomous modes.
self.components = {"drive": self.drive}
# Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous
# modes should inherit.
self.automodes = autonomous.AutonomousModeSelector(
"Sim_Autonomous", self.components)
# Autonomous and teleop periodic both run the code on a fixed loop time. A part of TimedRobot.
def autonomousPeriodic(self):
# runs the autonomous modes when Autonomous is activated.
self.automodes.run()
def teleopPeriodic(self):
# Turns on drive safety and checks to se if operator control and the robot is enabled.
self.drive.setSafetyEnabled(True)
if self.isOperatorControl() and self.isEnabled():
# Drives the robot with controller inputs. You can use buttons with self.DriveStick.getRawButton(ButtonNum).
self.drive.arcadeDrive(self.DriveStick.getY(),
-self.DriveStick.getX(),
squareInputs=False)
class Attack3Joystick(Sensor):
"""
Sensor wrapper for the Attack 3 Joystick.
Has boolean attributes for buttons: trigger, button2-9
and double x_axis, y_axis for joystick position
"""
x_axis = y_axis = 0
trigger = button2 = button3 = \
button4 = button5 = button6 = \
button7 = button8 = button9 = \
button10 = button11 = False
def __init__(self, port):
"""
Initializes the joystick with some USB port.
"""
super().__init__()
self.j = Joystick(port)
def poll(self):
for i, state_id in enumerate(BUTTON_TABLE, 1):
self.update_state(state_id, self.j.getRawButton(i))
self.x_axis = self.j.getX()
self.y_axis = self.j.getY()
class XboxJoystick(Sensor):
"""
Sensor wrapper for the Xbox Controller.
Has boolean attributes for buttons: a/b/x/y/back/start_button,
l/r_shoulder
Attributes l/r_x/y_axis for thumbstick positions
trigger_pos and keypad_pos for trigger and keypad position
"""
l_x_axis = l_y_axis = r_x_axis = r_y_axis = 0
trigger_pos = keypad_pos = 0
a_button = b_button = x_button = y_button = False
l_shoulder = r_shoulder = back_button = start_button = False
def __init__(self, port):
"""
Initializes the joystick with some USB port.
"""
super().__init__()
self.j = Joystick(port)
def poll(self):
for i, state_id in enumerate(BUTTON_TABLE, 1):
self.update_state(state_id,
self.j.getRawButton(i))
# button index is offset by 1 due to wpilib 1-indexing
self.l_x_axis = self.j.getX()
self.l_y_axis = self.j.getY()
self.r_x_axis = self.j.getRawAxis(4)
self.r_y_axis = self.j.getRawAxis(5)
self.trigger_pos = self.j.getZ()
self.keypad_pos = self.j.getRawAxis(6)
class Joker(magicbot.MagicRobot):
drivePID: DriveController
chassis: Chassis
def createObjects(self):
""" Create motors, sensors and all your components here. """
self.chassis_left_master = WPI_TalonSRX(1)
self.chassis_left_slave = WPI_TalonSRX(2)
self.chassis_right_master = WPI_TalonSRX(3)
self.chassis_right_slave = WPI_TalonSRX(4)
self.chassis_gyro = AHRS.create_spi()
self.joystick = Joystick(0)
def disabledInit(self):
if self.drivePID.enabled:
self.drivePID.close()
def robotPeriodic(self):
if self.isAutonomousEnabled():
self.chassis.set_auto()
elif self.isOperatorControlEnabled():
self.chassis.set_teleop()
def teleopInit(self):
""" Called when teleop starts. """
NetworkTables.initialize(server="10.43.20.149")
self.sd = NetworkTables.getTable("SmartDashboard")
self.chassis.reset_encoders()
def teleopPeriodic(self):
""" Called on each iteration of the control loop. """
self.chassis.set_speed(-self.joystick.getY(), -self.joystick.getZ())
def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
# TODO reset heading
pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0.1, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
if joystick.getRawButtonPressed(3):
self.chassis.reset_odometry(
geometry.Pose2d(-3, 0, geometry.Rotation2d(
math.pi))) # the starting position on the field
self.target_estimator.reset()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.drive = Drive()
self.stick = Joystick(0)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
pass
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
self.drive.moveToPosition(10000, 'left')
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.stick.getY() * -1
rotation = self.stick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
def testInit(self):
pass
def testPeriodic(self):
pass
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# holds data from the FMS about the field elements.
self.fielddata = None
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data -- currently removed until we add an IF statement
# self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
## get field data
#if not self.fielddata:
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
#nearswitch, scale, farswitch = list(self.fielddata)
#if self.fielddata[0] == 'R':
#self.drive.arcade(.5, .2)
#else:
#self.drive.arcade(.5, -.2)
self.drive.arcade(.5, 0)
def teleopInit(self):
self.drive.driveLeftMaster.setQuadraturePosition(0, 0)
self.drive.driveRightMaster.setQuadraturePosition(0, 0)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
self.drive.arcade(speed, rotation)
self.cubeGrabber.grabberFunction()
def testInit(self):
pass
def testPeriodic(self):
pass
def handle_chassis_inputs(self, joystick: wpilib.Joystick) -> None:
throttle = scale_value(joystick.getThrottle(), 1, -1, 0, 1)
vx = 3 * throttle * rescale_js(-joystick.getY(), 0.1)
vz = 3 * throttle * rescale_js(-joystick.getTwist(), 0.1)
self.chassis.drive(vx, vz)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kElevator = robotmap.Elevator
self.kSticks = robotmap.Sticks
self.kClimber = robotmap.Climber
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = cubeGrabber(self)
self.elevator = Elevator(self)
self.climber = Climber(self)
self.sendableChooser()
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.autonomous = Autonomous(self)
self.autonomous.reset()
self.drive.autoInit()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#self.autonomous.testMove(self.autonomous.WALL_TO_SCALE, -1, False)
#self.autonomous.testAngle(-90, -1)
#self.elevator.setElevatorPosition(self.elevator.kScale)
#self.autonomous.start()
self.autonomous.run()
#self.elevator.setElevatorPosition(-20000)
#self.autonomous.telemetry()
def teleopInit(self):
self.drive.teleInit()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = (self.dStick.getY() * -1)**3
rotation = self.dStick.getTwist()/(1.1+self.dStick.getRawAxis(3))
# self.drive.moveSpeed(speed, speed)
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
#
self.elevator.elevatorFunction()
#self.elevator.telemetry()
self.climber.climberFunction()
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
class OI:
def __init__(self):
self.reversed = True
self.arcade_drive = True
self.turbo = False
self.controls = {
"reverse": 2, # top
"arcade_switch": 3,
"turbo": 1, # trigger
"reset": 4
}
self.driver_controller = Joystick(0)
self.left_power = 0
self.right_power = 0
def get_button_config_pressed(self, controller, button_name):
return controller.getRawButtonPressed(self.controls[button_name])
def get_button_config_held(self, controller, button_name):
return controller.getRawButton(self.controls[button_name])
def curve_input(self, i):
deadzone = 0.1
if abs(i) < deadzone:
return 0
return math.pow(i, 3) / 1.25
def reset(self):
self.left_power = 0
self.right_power = 0
self.reversed = False
self.arcade_drive = True
self.turbo = False
def execute(self):
if self.get_button_config_pressed(self.driver_controller, "reverse"):
self.reversed = not self.reversed
if self.get_button_config_pressed(self.driver_controller,
"arcade_switch"):
self.arcade_drive = not self.arcade_drive
self.turbo = self.get_button_config_held(self.driver_controller,
"turbo")
self.left_power = self.driver_controller.getY()
self.right_power = self.driver_controller.getTwist()
reversed_constant = -1 if self.reversed else 1
self.left_power = self.curve_input(self.left_power * reversed_constant)
self.right_power = self.curve_input(self.right_power *
reversed_constant)
if self.turbo:
self.left_power *= 1.25
self.right_power *= 1.25
if self.get_button_config_pressed(self.driver_controller, "reset"):
self.reset()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kSticks = robotmap.Sticks
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = Grabber(self)
# TODO: see examples/pacgoat/robot.py
# we need Classes defining autonomous commands to call for Forward
# and Reverse
#self.autoChooser = wpilib.SendableChooser()
#self.autoChooser.addDefault("Forward", Forward(self))
#self.autoChooser.addObject("Reverse", Reverse(self))
#wpilib.SmartDashboard.putData("Auto Mode", self.autoChooser)
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""
This function is run once each time the robot enters autonomous mode.
"""
# get field data
#self.fielddata = wpilib.DriverStation.getInstance().getGameSpecificMessage()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#nearswitch, scale, farswitch = list(self.fielddata)
#
# if nearswitch == 'R':
# self.drive.arcade(.5, .2)
# else:
# self.drive.arcade(.5, -.2)
pass
def teleopInit(self):
pass
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = self.dStick.getY() * -1
rotation = self.dStick.getTwist()
# self.drive.moveSpeed(speed, speed)
if self.isSimulation():
self.drive.arcade(speed, rotation)
else:
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
# TODO: need something like this in commands that autonomous or teleop
# would run to make sure an exception doesn't crash the code during
# competition.
#raise ValueError("Checking to see what happens when we get an exception")
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
