class Arcade(Subsystem):
"""
Subsystem to control the motors for ArcadeDrive
"""
def __init__(self):
super().__init__("Arcade")
# motors and the channels they are connected to
self.frontLeftMotor = wpilib.PWMVictorSPX(0)
self.rearLeftMotor = wpilib.PWMVictorSPX(1)
self.frontRightMotor = wpilib.PWMVictorSPX(2)
self.rearRightMotor = wpilib.PWMVictorSPX(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
self.drive.setSafetyEnabled(False)
def speed(self, xSpeed, zRotation):
self.drive.arcadeDrive(xSpeed, zRotation)
def speed2(self, leftSpeed, rightSpeed):
self.drive.tankDrive(leftSpeed, rightSpeed)
def initDefaultCommand(self):
self.setDefaultCommand(ThrottleMixer())
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
self.frontLeftMotor = wpilib.Talon(1)
self.rearLeftMotor = wpilib.Talon(12)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(5)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
def teleopInit(self):
'''Executed at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
'''Runs the motors with tank steering'''
self.myRobot.tankDrive(self.leftStick.getY() * -1,
self.rightStick.getY() * -1)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
self.myRobot.tankDrive(self.leftStick.getY() * -1,
self.rightStick.getY() * -1)
class MyRobot(wpilib.SampleRobot):
# Defines the channels on the RoboRio that the motors are plugged into. There can be up to eight.
FLChannel = 3
FRChannel = 4
RLChannel = 1
RRChannel = 2
# Defines the order that the sticks that are plugged in are assigned.
DriveStickChannel = 0
# ExtraStickChannel = 1
def robotInit(self):
# Launches the camera server so that we can have video through any cameras on the robot.
wpilib.CameraServer.launch()
# Defines the motors that will actually be on the robot for use in the drive function.
self.FLMotor = wpilib.Spark(self.FLChannel)
self.FRMotor = wpilib.Spark(self.FRChannel)
self.RLMotor = wpilib.Spark(self.RLChannel)
self.RRMotor = wpilib.Spark(self.RRChannel)
# Puts the motors into groups so that they fit the parameters of the function.
self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor)
self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor)
# The drive function that tells the computer what kind of drive to use and where the motors are.
self.drive = DifferentialDrive(self.LMG, self.RMG)
# Tells the computer how long to wait without input to turn off the motors
self.drive.setExpiration(0.1)
# Defines the Joystick that we will be using for driving.
self.DriveStick = wpilib.Joystick(self.DriveStickChannel)
self.components = {"drive": self.drive}
self.automodes = robotpy_ext.autonomous.AutonomousModeSelector(
"autonomous", self.components)
def autonomous(self):
self.automodes.run()
def operatorControl(self):
# Enables the safety on the drive. Very important. DO NOT FORGET!
self.drive.setSafetyEnabled(True)
# Checks to see if the robot is activated and that operator control is active, so your robot does not move
# when it is not supposed to.
while self.isOperatorControl() and self.isEnabled():
# drives the robot with the arcade drive, which uses one joystick and is a bit easier to use.
# It is a part of DifferentialDrive
self.drive.arcadeDrive(self.DriveStick.getY(),
-self.DriveStick.getX(),
squareInputs=False)
wpilib.Timer.delay(0.005)
class DriveTrain(Subsystem):
def __init__(self):
# Ensures a single-time initialization
Subsystem.__init__(self, "DriveTrain")
# Front Motor Controllers
self.front_cont_right = ctre.WPI_TalonSRX(3)
self.rear_cont_right = ctre.WPI_TalonSRX(2)
self.right = wpilib.SpeedControllerGroup(self.front_cont_right,
self.rear_cont_right)
# Rear Motor Controllers
self.front_cont_left = ctre.WPI_TalonSRX(4)
self.rear_cont_left = ctre.WPI_TalonSRX(5)
self.left = wpilib.SpeedControllerGroup(self.front_cont_left,
self.rear_cont_left)
# Drive Type
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# enable safety
self.drive.setSafetyEnabled(False)
def engageDrive(self, speed, rotation):
self.drive.arcadeDrive(speed, rotation)
#print(speed)
def initDefaultCommand(self):
self.setDefaultCommand(Drive())
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 MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
#self.myRobot = wpilib.RobotDrive(0, 1)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
def autonomousInit(self):
'''
:return:
'''
pass
def autonomousPeriodic(self):
pass
def teleopInit(self):
"""Executed at the start of teleop mode
:return:
"""
pass
def teleopPeriodic(self):
"""Runs the motors with tank steering
:return:
"""
self.myRobot.tankDrive(self.leftStick.getY(), self.rightStick.getY())
class DriveArcade(wpilib.command.Command):
""" Command to control drive using gamepad in arcade mode. """
diffDrive: DifferentialDrive
gamePad: GenericHID
nominal: float
def __init__(self, drive: Drive, leftMotors: SpeedControllerGroup, rightMotors: SpeedControllerGroup, gamePad: GenericHID):
super().__init__("DriveArcade")
self.requires(drive)
self.gamePad = gamePad
self.diffDrive = DifferentialDrive(leftMotors, rightMotors)
self.nominal = 3.0 / 8.0
self.deadband = 0.15 # XBox360 controller has a lot of slop
def tweak(self, val, scale):
tweaked: float = DifferentialDrive.applyDeadband(val, self.deadband)
if tweaked == 0.0:
# User input value in deadband, just return 0.0
return tweaked
tweaked *= scale * (1 - self.nominal)
if tweaked > 0:
tweaked = (tweaked * tweaked) + self.nominal
else:
tweaked = (tweaked * -tweaked) - self.nominal
return tweaked
def initialize(self):
self.diffDrive.setSafetyEnabled(True)
def execute(self):
throttle: float = self.tweak(-self.gamePad.getY(GenericHID.Hand.kLeft), 1.0)
rotation: float = self.tweak(self.gamePad.getX(GenericHID.Hand.kRight), 0.5)
#print("Throttle", throttle, "Rotation", rotation)
self.diffDrive.arcadeDrive(throttle, rotation, False)
def isFinished(self):
return False
def interrupted(self):
self.diffDrive.stopMotor()
self.diffDrive.setSafetyEnabled(False)
def end(self):
self.interrupted()
class DriveTrain(Subsystem):
def __init__(self):
super().__init__('DriveTrain')
map = wpilib.command.Command.getRobot().robotMap
#create motors here
motors = {}
for name in map.CAN.driveMotors:
motors[name] = wpilib.command.Command.getRobot(
).motorHelper.createMotor(map.CAN.driveMotors[name])
print(motors['leftMotor'].getSensorCollection())
self.motors = motors
self.drive = DifferentialDrive(motors['leftMotor'],
motors['rightMotor'])
self.drive.setSafetyEnabled(False)
self.minSpeedChange = 0.001
self.timeRate = 0.2
self.desired = {}
self.desired['left'] = 0
self.desired['right'] = 0
self.current = {}
self.current['left'] = 0
self.current['right'] = 0
self.lastUpdateTime = time.time()
self.deadband = 0.1
def setDeadband(self, deadband):
self.drive.deadband = deadband
def move(self, spd, rot):
#print("Spd" ,spd, "rot", rot)
self.drive.arcadeDrive(spd, rot, True)
#print("Left Position: %f"%(self.motors['leftMotor'].getSelectedSensorPosition(0)))
#print("Right Position: %f"%(self.motors['rightMotor'].getSelectedSensorPosition(0)))
def initDefaultCommand(self):
self.setDefaultCommand(commands.driveControlled.DriveControlled())
def moveTank(self, left, right):
self.drive.tankDrive(left, right, True)
class MyRobot(wpilib.TimedRobot):
RLMotorChannel = 1
RRMotorChannel = 2
FLMotorChannel = 3
FRMotorChannel = 4
DriveStickChannel = 0
def robotInit(self):
RLMotor = wpilib.Spark(self.RLMotorChannel)
RRMotor = wpilib.Spark(self.RRMotorChannel)
FLMotor = wpilib.Spark(self.FLMotorChannel)
FRMotor = wpilib.Spark(self.FRMotorChannel)
self.Left = wpilib.SpeedControllerGroup(RLMotor, FLMotor)
self.Right = wpilib.SpeedControllerGroup(RRMotor, FRMotor)
self.DriveStick = wpilib.Joystick(self.DriveStickChannel)
self.drive = DifferentialDrive(self.Left, self.Right)
self.drive.setExpiration(0.1)
def operatorControl(self):
self.drive.setSafetyEnabled(True)
while self.isEnabled() and self.isOperatorControl():
self.drive.arcadeDrive(
self.DriveStick.getX(),
self.DriveStick.getY(),
squareInputs=True
)
f
class Apollo(wpilib.TimedRobot):
def __init__(self):
super().__init__()
self.joystick = wpilib.XboxController(0)
self.left_drive_motor = wpilib.Talon(0)
self.left_drive_motor_2 = wpilib.Talon(1)
self.right_drive_motor = wpilib.Talon(2)
self.right_drive_motor_2 = wpilib.Talon(3)
self.left_drive_motor_group = wpilib.SpeedControllerGroup(
self.left_drive_motor, self.left_drive_motor_2)
self.right_drive_motor_group = wpilib.SpeedControllerGroup(
self.right_drive_motor, self.right_drive_motor_2)
self.drive = DifferentialDrive(self.left_drive_motor_group,
self.right_drive_motor_group)
self.drive_rev = False
self.lift_motor = wpilib.Spark(4)
self.lift_motor_2 = wpilib.Spark(5)
self.lift_motor_group = wpilib.SpeedControllerGroup(
self.lift_motor, self.lift_motor_2)
self.lift_motor_speed = 0
self.lock_controls = False
self.front_motor = wpilib.Spark(6)
self.front_motor_2 = wpilib.Spark(7)
self.front_motor_2.setInverted(True)
self.front_motor_group = wpilib.SpeedControllerGroup(
self.front_motor, self.front_motor_2)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.encoder = wpilib.Encoder(aChannel=0, bChannel=1)
def reset(self):
self.drive_rev = False
while self.lift_motor_speed > 0:
self.lift_motor_speed -= 0.002
self.lift_motor_group.set(self.lift_motor_speed)
if self.lift_motor_speed < 0:
self.lift_motor_speed = 0
self.lift_motor_group.set(self.lift_motor_speed)
self.front_speed = 0
self.front_motor_group.set(self.front_speed)
self.lock_controls = False
self.hatch_solenoid.set(0)
self.encoder.reset()
def drive_control(self):
lh_y = self.joystick.getY(self.joystick.Hand.kLeft) * (1 / 2)
rh_x = self.joystick.getX(self.joystick.Hand.kRight) * (1 / 2)
if self.joystick.getStickButtonPressed(self.joystick.Hand.kLeft) or \
self.joystick.getStickButtonPressed(self.joystick.Hand.kRight):
self.drive_rev = not self.drive_rev
if self.drive_rev:
self.drive.arcadeDrive(lh_y, rh_x, True)
else:
self.drive.arcadeDrive(lh_y * -1, rh_x, True)
def lift_control(self):
low_volt = 0.2
if self.joystick.getYButtonPressed():
self.lock_controls = not self.lock_controls
if not self.lock_controls:
if self.joystick.getTriggerAxis(self.joystick.Hand.kLeft) > 0.9:
# self.lift_motor_speed = 0.6
if self.lift_motor_speed == 0.0 or self.lift_motor_speed == low_volt:
self.lift_motor_speed = 0.6
elif int(abs(self.encoder.getRate())) == 0:
self.lift_motor_speed += 0.01
elif (self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9)\
and self.joystick.getBButton():
self.lift_motor_speed -= 0.01 if self.lift_motor_speed > 0.0 else 0.0
elif self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9:
self.lift_motor_speed = low_volt
elif (self.lift_motor_speed > low_volt
and abs(self.encoder.getRate()) > 0):
self.lift_motor_speed -= 0.0025
elif (0.0 < self.lift_motor_speed < low_volt):
self.lift_motor_speed -= 0.0025 if self.lift_motor_speed > 0.0 else 0.00
elif self.lift_motor_speed < 0:
self.lift_motor_speed = 0
self.lift_motor_group.set(self.lift_motor_speed)
def grab_control(self):
if self.joystick.getBumper(self.joystick.Hand.kLeft) and not \
self.joystick.getBumper(self.joystick.Hand.kRight):
self.front_motor_group.set(0.33)
elif self.joystick.getBumperPressed(self.joystick.Hand.kRight) and not \
self.joystick.getBumper(self.joystick.Hand.kLeft):
self.front_motor_group.set(-0.7)
elif self.joystick.getBumperReleased(self.joystick.Hand.kLeft) or \
self.joystick.getBumperReleased(self.joystick.Hand.kRight):
self.front_motor_group.set(0)
def hatch_control(self):
if self.joystick.getXButtonPressed():
self.hatch_solenoid.set(2)
elif self.joystick.getXButtonReleased():
self.hatch_solenoid.set(1)
def robotInit(self):
self.reset()
wpilib.CameraServer.launch('vision.py:main')
def robotPeriodic(self):
pass
def autonomousInit(self):
self.reset()
def autonomousPeriodic(self):
self.drive_control()
self.lift_control()
self.grab_control()
self.hatch_control()
def disabledInit(self):
self.reset()
def disabledPeriodic(self):
self.reset()
def teleopInit(self):
self.reset()
self.drive.setSafetyEnabled(True)
def teleopPeriodic(self):
self.drive_control()
self.lift_control()
self.grab_control()
self.hatch_control()
class Drivetrain:
navx = navx.AHRS
left_motor_master = WPI_TalonFX
left_motor_slave = WPI_TalonFX
left_motor_slave2 = WPI_TalonFX
right_motor_master = WPI_TalonFX
right_motor_slave = WPI_TalonFX
right_motor_slave2 = WPI_TalonFX
shifter_solenoid = Solenoid
arcade_cutoff = tunable(0.1)
angle_correction_cutoff = tunable(0.05)
angle_correction_factor_low_gear = tunable(0.1)
angle_correction_factor_high_gear = tunable(0.1)
angle_correction_max = tunable(0.2)
little_rotation_cutoff = tunable(0.1)
def __init__(self):
self.pending_differential_drive = None
self.force_differential_drive = False
self.pending_gear = LOW_GEAR
self.pending_position = None
self.pending_reset = False
self.og_yaw = None
self.pending_manual_drive = None
self.is_manual_mode = False
# Set encoders
self.left_motor_master.configSelectedFeedbackSensor(
ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
self.right_motor_master.configSelectedFeedbackSensor(
ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
self.left_motor_master.setSensorPhase(True)
# Set slave motors
self.left_motor_slave.set(ctre.TalonFXControlMode.Follower,
self.left_motor_master.getDeviceID())
self.left_motor_slave2.set(ctre.TalonFXControlMode.Follower,
self.left_motor_master.getDeviceID())
self.right_motor_slave.set(ctre.TalonFXControlMode.Follower,
self.right_motor_master.getDeviceID())
self.right_motor_slave2.set(ctre.TalonFXControlMode.Follower,
self.right_motor_master.getDeviceID())
# Set up drive control
self.robot_drive = DifferentialDrive(self.left_motor_master,
self.right_motor_master)
self.robot_drive.setDeadband(0)
self.robot_drive.setSafetyEnabled(False)
def is_left_encoder_connected(self):
return self.left_motor_master.getPulseWidthRiseToRiseUs() != 0
def is_right_encoder_connected(self):
return self.right_motor_master.getPulseWidthRiseToRiseUs() != 0
def reset_position(self):
self.left_motor_master.setQuadraturePosition(0, TALON_TIMEOUT)
self.right_motor_master.setQuadraturePosition(0, TALON_TIMEOUT)
def get_position(self):
'''
Returns averaged quadrature position in inches.
'''
left_position = self.get_left_encoder()
right_position = self.get_right_encoder()
position = (((left_position + right_position) / 2) *
(1 / UNITS_PER_REV) * DISTANCE_PER_REV)
return position
def drive(self, *args):
self.differential_drive(*args)
def differential_drive(self,
y,
rotation=0,
squared=True,
force=False,
quick_turn=False,
use_curvature=False):
if not self.force_differential_drive:
self.pending_differential_drive = DifferentialDriveConfig(
y=y,
rotation=rotation,
squared=squared,
quick_turn=quick_turn,
use_curvature=use_curvature)
self.force_differential_drive = force
def turn(self, rotation=0, force=False):
self.differential_drive(0, rotation, squared=False, force=force)
def reset_angle_correction(self):
self.navx.reset()
def angle_corrected_differential_drive(self, y, rotation=0):
'''
Heading must be reset first. (drivetrain.reset_angle_correction())
'''
# Scale angle to reduce max turn
rotation = util.scale(rotation, -1, 1, -0.65, 0.65)
# Scale y-speed in high gear
if self.pending_gear == HIGH_GEAR:
y = util.scale(y, -1, 1, -0.75, 0.75)
use_curvature = False
quick_turn = False
# Small rotation at lower speeds - and also do a quick_turn instead of
# the normal curvature-based mode.
if abs(y) <= self.little_rotation_cutoff:
rotation = util.abs_clamp(rotation, 0, 0.7)
quick_turn = True
# NEVER USE CURVATURE
# Curvature drive for high gear and high speedz
# if self.pending_gear == HIGH_GEAR and abs(y) >= 0.5:
# use_curvature = True
# Angle correction
if abs(rotation) <= self.angle_correction_cutoff:
heading = self.navx.getYaw()
if not self.og_yaw:
self.og_yaw = heading
factor = self.angle_correction_factor_high_gear if \
self.pending_gear == HIGH_GEAR else \
self.angle_correction_factor_low_gear
rotation = util.abs_clamp(-factor * (heading - self.og_yaw), 0,
self.angle_correction_max)
else:
self.og_yaw = None
self.differential_drive(y,
rotation,
quick_turn=quick_turn,
use_curvature=use_curvature)
def shift_low_gear(self):
self.pending_gear = LOW_GEAR
def shift_high_gear(self):
self.pending_gear = HIGH_GEAR
def shift_toggle(self):
if self.pending_gear == HIGH_GEAR:
self.pending_gear = LOW_GEAR
else:
self.pending_gear = HIGH_GEAR
def manual_drive(self, left, right):
self.pending_manual_drive = [left, right]
def get_left_encoder(self):
return -self.left_motor_master.getQuadraturePosition()
def get_right_encoder(self):
return self.right_motor_master.getQuadraturePosition()
def get_left_encoder_meters(self):
return self.get_left_encoder() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_right_encoder_meters(self):
return self.get_right_encoder() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_left_encoder_velocity(self):
return -self.left_motor_master.getQuadratureVelocity()
def get_right_encoder_velocity(self):
return self.right_motor_master.getQuadratureVelocity()
def get_left_encoder_velocity_meters(self):
return self.get_left_encoder_velocity() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_right_encoder_velocity_meters(self):
return self.get_right_encoder_velocity() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def set_manual_mode(self, is_manual):
self.is_manual_mode = is_manual
if not is_manual:
self.pending_manual_drive = None
def execute(self):
# print('exec drivetrain', self.pending_differential_drive)
# print('dist_traveled_meters', self.get_left_encoder_meters(),
# self.get_right_encoder_meters())
# Shifter
self.shifter_solenoid.set(self.pending_gear)
# Manual
if self.is_manual_mode:
if self.pending_manual_drive:
left, right = self.pending_manual_drive
# left = self.robot_drive.applyDeadband(left, DEADBAND)
# right = self.robot_drive.applyDeadband(right, DEADBAND)
self.left_motor_master.set(-left)
self.right_motor_master.set(right)
self.pending_manual_drive = None
return
# Drive
if self.pending_differential_drive:
if self.pending_differential_drive.use_curvature and \
abs(self.pending_differential_drive.y) > \
self.arcade_cutoff and \
USE_CURVATURE_DRIVE:
self.robot_drive.curvatureDrive(
self.pending_differential_drive.y,
-self.pending_differential_drive.rotation,
isQuickTurn=self.pending_differential_drive.quick_turn)
else:
self.robot_drive.arcadeDrive(
self.pending_differential_drive.y,
-self.pending_differential_drive.rotation,
squareInputs=self.pending_differential_drive.squared)
self.pending_differential_drive = None
self.force_differential_drive = False
def on_disable(self):
self.robot_drive.arcadeDrive(0, 0)
def get_state(self):
return {
'pending_gear': self.pending_gear,
'pending_differential_drive': self.pending_differential_drive
}
def put_state(self, state):
self.pending_gear = state['pending_gear']
self.pending_differential_drive = DifferentialDriveConfig._make(
state['pending_differential_drive'])
def limelight_turn(self):
self.llt = NetworkTables.getTable('limelight')
self.tv = self.llt.getNumber('tv', 0)
self.tx = self.llt.getNumber('tx', 0)
if self.tv:
self.turn(self.get_position() + self.tx)
class Gemini(wpilib.TimedRobot):
def __init__(self):
""" Initialization of internal class variables and software-bases only """
super().__init__()
# global button status list construction
self.buttonToggleStatus = [
False, False, False, False, False, False, False
]
from networktables import NetworkTables
# connection for logging & Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
def robotInit(self):
''' Initialization of robot systems. '''
logging.info(
'-( ͡° ͜ʖ ͡°)╯╲_-^o=o\ \"Don\'t mind me, just walking the robot.\"'
)
from wpilib.drive import DifferentialDrive
from ctre import WPI_TalonSRX, WPI_VictorSPX
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
# lift encoder construction
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder construction
self.liftArmEncoder = wpilib.Encoder(5, 6)
# drive train motor groups assignment
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group assignment
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# lift motor system initialization
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motor system initialization
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor initialization
self.cargo = WPI_VictorSPX(5)
# game and joystick controller construction
# joystick - 0, 1 | controller - 2
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
self.buttonBox = wpilib.Joystick(3)
# pneumatic and compressor system initialization
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
# Smart Dashboard and NetworkTables initialization and construction
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
# proximity detection sensors
self.Hall = wpilib.DigitalInput(7)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
# timer construction
self.timer = wpilib.Timer()
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
from sensors import REV_Color_Sensor_V2
# Initialization and configuration of I2C interface with color sensor.
self.colorSensor = REV_Color_Sensor_V2(wpilib.I2C.Port.kOnboard)
def autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# pre-auto timer configuration
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
def autonomousPeriodic(self):
'''' Called periodically during autonomous. '''
if self.DS.getGameSpecificMessage():
# Test Methods
if self.DS.getGameSpecificMessage() is 'encoder_test':
# Drives robot set encoder distance away
self.rightPos = fabs(
self.frontRightMotor.getQuadraturePosition())
self.leftPos = fabs(
self.frontLeftMotor.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
if self.DS.getGameSpecificMessage() is 'diagnostics':
# Smart Dashboard Tests
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber(
"Right Encoder Speed: ",
abs(self.frontRightMotor.getQuadratureVelocity()))
self.sd.putNumber(
"Left Encoder Speed: ",
abs(self.frontLeftMotor.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ",
self.liftEncoder.getDistance())
if self.DS.getGameSpecificMessage() is 'pressure':
self.Compressor.start()
elif self.Compressor.enabled() is True:
self.Compressor.stop()
if not self.DS.getGameSpecificMessage(
) is 'pressure' and not self.DS.getGameSpecificMessage(
) is 'encoder_test':
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString(
"Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString("Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))
class MyRobot(wpilib.IterativeRobot):
# Defines the channels that are used on the inputs.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.
FLChannel = 4
FRChannel = 2
RLChannel = 3
RRChannel = 1
DriveStickChannel = 0
# ExtraStickChannel = 1
# RobotInit is where everything is initialized.
def robotInit(self):
# Initializes the network table
# NetworkTables.initialize(server="10.56.13.2")
# self.table = NetworkTables.getTable("limelight")
# f is a control constant that will be used to change variable values quickly
# self.f = 1
# self.ControlConstant = -0.1 * self.f
# self.minCommand = -0.05 * self.f
# self.Blue = (RGB value or blue color)
# self.Green = (RGB value of green color)
# self.Red = (RGB value of red color)
# self.Yellow = (RGB value of Yellow color)
# Defines the Joystick that we will be using for driving.
self.DriveStick = wpilib.Joystick(self.DriveStickChannel)
# Initializing drive motors
self.FLMotor = wpilib.Spark(self.FLChannel)
self.FRMotor = wpilib.Spark(self.FRChannel)
self.RLMotor = wpilib.Spark(self.RLChannel)
self.RRMotor = wpilib.Spark(self.RRChannel)
self.LoaderGrab = ctre.VictorSPX(1)
# Puts the motors into groups so that they fit the parameters of the function.
self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor)
self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor)
# The drive function that tells the computer what kind of drive to use and where the motors are.
self.drive = DifferentialDrive(self.LMG, self.RMG)
# self.ColorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# 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("autonomous", self.components)
self.timer = wpilib.Timer()
# def autonomousPeriodic(self):
# Runs the autonomous mode selector.
# self.automodes.run()
def operatorControl(self):
self.drive.setSafetyEnabled(True)
while self.isOperatorControlEnabled():
# Checks to see if you are holding button 2 and if so automatically aims the robot. Else, normal drive.
# if self.DriveStick.getrawButton(2):
# Tx and Ty are variables marking the angle to the target.
# If they are 0 then the calibrated crosshair is right on the target.
# tx = self.table.getNumber("tx")
# ty = self.table.getNumber("ty")
# Angle to the target based off of the angle the camera is mounted at and the angle the camera measures.
# targetAngle = 20 + ty
# Distance from the target as calculated off of the equation
# (TargetHeight - CameraHeight) / Tan(MountAngle + TargetAngle) = D
# For a more in-depth explanation look a the limelight docs at docs.limelightvision.io
# distance = 8 / tan(targetAngle)
# The error in your robot's heading based off of the angle to the target.
# headingError = tx
# SteerAdjust = 0
# Turns the robot if it is more than one degree off.
# if tx > 1.0:
# SteerAdjust = self.ControlConstant * headingError + self.minCommand
# elif tx < -1.0:
# SteerAdjust = self.ControlConstant * headingError - self.minCommand
# Uses the calculations to turn the robot.
# self.drive.tankDrive(
# SteerAdjust,
# -SteerAdjust,
# squareInputs=False
# )
# else:
# drives the robot with the arcade drive, which uses one joystick and is a bit easier to use. It is a
# part of DifferentialDrive
self.drive.arcadeDrive(
self.DriveStick.getY(),
self.DriveStick.getX(),
squareInputs=True
)
wpilib.Timer.delay(.005)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(3)
self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(5)
self.frontRightMotor = wpilib.Spark(0)
self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(2)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.middleLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.middleRightMotor, self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = 'LRL'
# joysticks 1 & 2 on the driver station
self.Stick1 = wpilib.XboxController(0)
self.Stick2 = wpilib.Joystick(1)
self.aSolenoidLow = wpilib.DoubleSolenoid(2,3)
self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1)
self.iSolenoid = wpilib.DoubleSolenoid(4,5)
def autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance().getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
def autonomousPeriodic(self):
if self.gameData[0:1] == "L":
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5,0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(0.7,0.7)
while timer.get() < 3:
self.myRobot.tankDrive(0,0)
timer.reset()
timer.start()
while timer.get() < 0.7: # Right = 0.68 //original
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.myRobot.tankDrive(-0.7,0.7) # Turning right, even though it looks like turning left
while timer.get() < 1.2:
self.myRobot.tankDrive(0,0)
while timer.get() < 2:
self.myRobot.tankDrive(0.7,0.7)
while timer.get() < 2.5:
self.myRobot.tankDrive(0,0)
while timer.get() < 5: # Outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
self.iSolenoid.set(1)
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
''' Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
elif self.gameData[0:1] == 'R':
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5,0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(0.7,0.7)
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
'''Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
def disabledInit(self):
self.myRobot.tankDrive(0,0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
minv = 0.4
maxv = 0.6
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sp = forward - backward
steering = self.Stick1.getX(0)
mod = minv + maxv*((1-abs(sp))**2)
r = (steering**3)*mod
if sp >= 0:
self.myRobot.tankDrive(sp*0.85 - r, sp*0.85 + r)
else:
self.myRobot.tankDrive(sp*0.85 + r, (sp*0.85 - r))
# intake and outtake
if self.Stick2.getRawButton(11)==True: # intake
self.ihigh_motor.set(0.6)
self.ilow_motor.set(-0.95)
if self.Stick2.getRawButton(11)==False and self.Stick2.getRawButton(12)==False:
self.ihigh_motor.set(0)
self.ilow_motor.set(0)
if self.Stick2.getRawButton(12)==True: # outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
if self.Stick2.getRawButton(9) == True:
self.aSolenoidLow.set(1)
self.iSolenoid.set(1)
if self.Stick2.getRawButton(10) == True:
self.aSolenoidLow.set(2)
if self.Stick2.getRawButton(7) == True:
self.aSolenoidHigh.set(1)
if self.Stick2.getRawButton(8) == True:
self.aSolenoidHigh.set(2)
if self.Stick2.getRawButton(3) == True:
self.iSolenoid.set(1) # push intake
if self.Stick2.getRawButton(4) == True:
self.iSolenoid.set(2) # pull intake
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""Robot initialization function"""
LEFT = 1
RIGHT = 2
CENTER1 = 3
CENTER2 = 4
# object that handles basic drive operations
self.leftTalon = ctre.WPI_TalonSRX(LEFT)
self.rightTalon = ctre.WPI_TalonSRX(RIGHT)
self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1)
self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2)
self.left = wpilib.SpeedControllerGroup(self.leftTalon)
self.right = wpilib.SpeedControllerGroup(self.rightTalon)
self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1)
self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
# self.leftStick = wpilib.Joystick(0)
# self.rightStick = wpilib.Joystick(1)
self.DEADZONE = 0.4
self.LEFT = GenericHID.Hand.kLeft
self.RIGHT = GenericHID.Hand.kRight
self.driver = wpilib.XboxController(0)
def autonomousInit(self):
self.myRobot.tankDrive(0.8, 0.8)
def autonomousPeriodic(self):
self.myRobot.tankDrive(1, 0.5)
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def setCenters(self, speed_value):
self.center1.set(speed_value)
self.center2.set(-speed_value)
def deadzone(self, val, deadzone):
if abs(val) < deadzone:
return 0
return val
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
right = self.driver.getY(self.RIGHT)
left = self.driver.getY(self.LEFT)
self.myRobot.tankDrive(right, left)
center_speed = self.driver.getX(self.RIGHT)
self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class MyRobot(wp.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = wp.VictorSP(1) #motor initialization
self.leftMotor2 = wp.VictorSP(3)
self.leftMotor3 = wp.VictorSP(5)
self.rightMotor1 = wp.VictorSP(2)
self.rightMotor2 = wp.VictorSP(4)
self.rightMotor3 = wp.VictorSP(6)
self.armMotor = wp.VictorSP(7)
self.leftIntakeMotor = wp.VictorSP(8)
self.rightIntakeMotor = wp.VictorSP(9)
self.leftSide = wp.SpeedControllerGroup(
self.leftMotor1, self.leftMotor2,
self.leftMotor3) #speed controller groups
self.rightSide = wp.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2,
self.rightMotor3)
self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor,
self.rightIntakeMotor)
self.myRobot = DifferentialDrive(self.leftSide, self.rightSide)
self.myRobot.setExpiration(0.1)
self.leftStick = wp.Joystick(0)
self.rightStick = wp.Joystick(1)
self.armStick = wp.XboxController(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(2, 3)
self.leftEncd = wp.Encoder(0, 1)
self.armPot = wp.AnalogPotentiometer(0, 270, -135)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
wp.SmartDashboard.putNumber("Straight Position", 15000)
wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000)
wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000)
wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000)
wp.SmartDashboard.putNumber("Left Switch Angle", 90)
wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000)
wp.SmartDashboard.putNumber("Switch Score Arm Position", 70)
wp.SmartDashboard.putNumber("Front Position 1", 74)
wp.SmartDashboard.putNumber("Front Position 2", 16)
wp.SmartDashboard.putNumber("Front Position 3", -63)
wp.SmartDashboard.putNumber("lvl2 Position 1", 60)
wp.SmartDashboard.putNumber("lvl2 Position 3", -50)
wp.SmartDashboard.putNumber("lvl3 Position 3", -38)
wp.SmartDashboard.putNumber("lvl3 Position 1", 45)
wp.SmartDashboard.putBoolean("switchScore?", False)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Side Switch Auto", 2)
self.chooser.addObject("Right Side Switch Auto (switch)", 3)
self.chooser.addObject("Center Auto", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotPeriodic(self):
wp.SmartDashboard.putNumber("Gyro:", round(self.gyro.getAngle(), 2))
wp.SmartDashboard.putNumber("Right Encoder:", self.rightEncd.get())
wp.SmartDashboard.putNumber("Left Encoder:", self.leftEncd.get())
wp.SmartDashboard.putNumber("Arm Postition", self.armPot.get())
calGyro = wp.SmartDashboard.getBoolean("calGyro:", True)
resetGyro = wp.SmartDashboard.getBoolean("resetGyro:", True)
encodeReset = wp.SmartDashboard.getBoolean("resetEnc:", True)
self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True)
self.straightPos = wp.SmartDashboard.getNumber("Straight Position",
4000)
self.leftAutoPos1 = wp.SmartDashboard.getNumber(
"Left Switch Pos 1", 4000)
self.leftAutoPos2 = wp.SmartDashboard.getNumber(
"Left Switch Angle", 90)
self.leftAutoPos3 = wp.SmartDashboard.getNumber(
"Left Switch Pos 2", 4000)
self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78)
self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.frontArmPos3 = wp.SmartDashboard.getNumber(
"Front Position 3", -63)
self.frontArmLvl2Pos1 = wp.SmartDashboard.getNumber(
"lvl2 Position 1", 68)
self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber(
"lvl2 Position 3", -50)
self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber(
"lvl3 Position 3", -38)
self.frontArmLvl3Pos1 = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
if (resetGyro):
self.gyro.reset()
wp.SmartDashboard.putBoolean("resetGyro:", False)
if (calGyro):
self.gyro.calibrate()
wp.SmartDashboard.putBoolean("calGyro:", False)
if (encodeReset):
self.rightEncd.reset()
self.leftEncd.reset()
wp.SmartDashboard.putBoolean("resetEnc:", False)
self.auto = self.chooser.getSelected()
def autonomousInit(self):
self.auto = self.chooser.getSelected()
self.leftMotorVal = 0
self.rightMotorVal = 0
self.gyroFuncGain = 40
self.angleFuncGain = 40
self.leftMotVal = 0
self.rightMotVal = 0
self.armSet = 0
self.intakeSet = 0
self.posGain = 18
self.armSpeed = 1
self.armPos = self.armPot.get()
self.startTime = tm.time()
self.gamePlacement = wp.DriverStation.getInstance(
).getGameSpecificMessage()
self.straightPos = wp.SmartDashboard.getNumber("Straight Position",
16000)
self.leftAutoPos1 = wp.SmartDashboard.getNumber(
"Left Switch Pos 1", 4000)
self.leftAutoPos2 = wp.SmartDashboard.getNumber(
"Left Switch Angle", 90)
self.leftAutoPos3 = wp.SmartDashboard.getNumber(
"Left Switch Pos 2", 4000)
self.leftMiddleAutoStraight = wp.SmartDashboard.getNumber(
"leftMiddleAutoStraight", 21)
self.leftMiddleAutoLateral = wp.SmartDashboard.getNumber(
"leftMiddleAutoLateral", 21)
self.switchScoreArmPos = wp.SmartDashboard.getNumber(
"Switch Score Arm Position", 70)
self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78)
self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.frontArmPos3 = wp.SmartDashboard.getNumber(
"Front Position 3", -63)
self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True)
self.StartButtonPos = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
self.stage1 = True
self.stage2 = False
self.stage3 = False
self.stage4 = False
self.stage5 = False
self.stage6 = False
def autonomousPeriodic(self):
#autonomous code will go here
armPos = self.armPot.get()
self.gamePlacement = wp.DriverStation.getInstance(
).getGameSpecificMessage()
if (self.auto == 1):
if (abs(self.rightEncd.get()) < self.straightPos and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.switchScore and self.gamePlacement != ""
and self.gamePlacement == "RRR"
or self.gamePlacement == "RRL"
or self.gamePlacement == "RLR"
or self.gamePlacement == "RLL"):
self.intakeSet = -1
else:
self.instakeSet = 0
if (self.auto == 2):
#Guidlines for switch placement on left side:
#check switch placement
#Move Forward
#Turn right 90 degrees
#Move Forward to wall
#place Cube
if (self.gamePlacement != "" and self.gamePlacement == "LLL"
or self.gamePlacement == "LRL"
or self.gamePlacement == "LLR"
or self.gamePlacement == "LRR"):
if (abs(self.rightEncd.get()) < self.leftAutoPos1
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() < self.leftAutoPos2 + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), self.leftAutoPos2,
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftAutoPos3
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), self.leftAutoPos2, 0.65,
self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) <= self.leftAutoPos1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.auto == 3):
if (self.gamePlacement != "" and self.gamePlacement == "RRR"
or self.gamePlacement == "RRL"
or self.gamePlacement == "RLR"
or self.gamePlacement == "RLL"):
if (abs(self.rightEncd.get()) < self.leftAutoPos1
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1),
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftAutoPos3
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65,
self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) <= self.leftAutoPos1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.auto == 5):
if (self.gamePlacement != "" and self.gamePlacement == "LLL"
or self.gamePlacement == "LRL"
or self.gamePlacement == "LLR"
or self.gamePlacement == "LRR"):
if (abs(self.rightEncd.get()) <
(self.leftMiddleAutoStraight / 2) and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1),
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftMiddleAutoLateral
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65,
self.gyroFuncGain)
self.stage4 = True
elif (self.gyro.getAngle() < (0) + 0.1 and self.stage4):
self.stage3 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), 0, self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage5 = True
elif (abs(self.rightEncd.get()) <
(self.leftMiddleAutoStraight / 1.50) and self.stage5):
self.stage4 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.armSet = 0
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) < self.straightPos
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.armSet = 0
if (abs(self.rightEncd.get()) > (self.straightPos - 1000)):
self.intakeSet = -1
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
self.leftIntakeMotor.set(self.intakeSet)
self.rightIntakeMotor.set(self.intakeSet * -1)
self.armMotor.set(self.armSet)
self.myRobot.tankDrive(self.rightMotVal, self.leftMotVal)
def teleopInit(self):
self.myRobot.setSafetyEnabled(True)
self.pastFrontFlipButton = False
self.flip = True
self.armSet = 0
self.shiftSet = 1
self.posGain = 27
self.posGain2 = 27
self.armSpeed = 1
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastLBumper = False
self.lastRBumper = False
self.lastStartButton = False
self.XButtonPos = wp.SmartDashboard.getNumber("Front Position 1", 77.6)
self.YButtonPos = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.BButtonPos = wp.SmartDashboard.getNumber("Front Position 3",
-62.8)
self.AButtonPos = wp.SmartDashboard.getNumber("lvl2 Position 1", 30)
self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber(
"lvl2 Position 3", -50)
self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber(
"lvl3 Position 3", -38)
self.StartButtonPos = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
#wp.SmartDashboard.getNumber("Back Position 1", -90)
#wp.SmartDashboard.getNumber("Back Position 2", -45)
#wp.SmartDashboard.getNumber("Back Position 3", -30)
def teleopPeriodic(self):
#Joystick Variables
leftJoyValY = self.leftStick.getY()
rightJoyValY = self.rightStick.getY()
armJoyValY = self.armStick.getRawAxis(3)
frontFlipButton = self.rightStick.getRawButton(2)
armPos = self.armPot.get()
highShiftButton = self.rightStick.getRawButton(4)
lowShiftButton = self.rightStick.getRawButton(5)
self.compressorButtonOn = self.rightStick.getRawButton(9)
self.compressorButtonOff = self.rightStick.getRawButton(8)
self.intakeInButton = self.armStick.getRawButton(7)
self.intakeOutButton = self.armStick.getRawButton(8)
self.buttonX = self.armStick.getRawButton(1)
self.buttonY = self.armStick.getRawButton(4)
self.buttonB = self.armStick.getRawButton(3)
self.buttonA = self.armStick.getRawButton(2)
self.buttonStart = self.armStick.getRawButton(10)
self.buttonLBumper = self.armStick.getRawButton(5)
self.buttonRBumper = self.armStick.getRawButton(6)
#Automatic arm positioning
if (self.buttonX == True and self.lastXButton == False):
self.lastXButton = True
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = False
if (self.buttonY == True and self.lastYButton == False):
self.lastXButton = False
self.lastYButton = True
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = False
if (self.buttonB == True and self.lastBButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = True
self.lastAButton = False
self.lastStartButton = False
if (self.buttonA and self.lastAButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = True
self.lastStartButton = False
if (self.buttonStart and self.lastStartButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = True
if (armJoyValY > 0.075 or armJoyValY < -0.075):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
if (self.lastXButton):
if (armPos < (self.XButtonPos - 1) or armPos >
(self.XButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.XButtonPos,
self.armSpeed, self.posGain)
else:
self.lastXButton = False
elif (self.lastYButton):
if (armPos < (self.YButtonPos - 1) or armPos >
(self.YButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.YButtonPos,
self.armSpeed, self.posGain)
else:
self.lastYButton = False
elif (self.lastBButton):
if (armPos < (self.BButtonPos - 1) or armPos >
(self.BButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.BButtonPos,
self.armSpeed, self.posGain)
else:
self.lastBButton = False
elif (self.lastAButton):
if (armPos < (self.AButtonPos - 1) or armPos >
(self.AButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.AButtonPos,
self.armSpeed, self.posGain)
else:
self.lastAButton = False
elif (self.lastStartButton):
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.lastBButton = False
else:
self.armSet = (armJoyValY * 0.75)
#Intake Motor Control
if (self.intakeInButton):
self.intakeSet = 1
elif (self.intakeOutButton):
self.intakeSet = -1
else:
self.intakeSet = 0
if (highShiftButton):
self.shiftSet = 1
elif (lowShiftButton):
self.shiftSet = 2
#FrontFlip
if (self.pastFrontFlipButton == False and frontFlipButton):
self.flip = not self.flip
self.pastFrontFlipButton = frontFlipButton
leftMotVal, rightMotVal = rf.flip(self.flip, leftJoyValY, rightJoyValY)
self.armMotor.set(self.armSet)
self.shifter.set(self.shiftSet)
self.leftIntakeMotor.set(self.intakeSet)
self.rightIntakeMotor.set(self.intakeSet * -1)
self.myRobot.tankDrive(rightMotVal, leftMotVal)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
#wpilib.DriverStation.reportWarning(str(self.myRobot.isRightSideInverted()),False)
def PID(self):
error = self.setpoint - self.gyro.getAngle()
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
self.rcw = self.P * error + self.I * self.integral + self.D * derivative
def autonomousInit(self):
self.myRobot.setSafetyEnabled(False)
def autonomousPeriodic(self):
pass
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
self.myRobot.arcadeDrive(self.rightStick.getY(),
-self.rightStick.getZ() * .4)
self.smartdash.putNumber('gyrosensor', self.gyro.getAngle())
if (self.rightStick.getRawButton(4)):
self.ll.putNumber('ledMode', 1)
if (self.rightStick.getRawButton(3)):
self.right.set(.5)
if (self.trigger.get()):
if (self.gearshift.get() == 1):
self.gearshift.set(2)
elif (self.gearshift.get() == 2):
self.gearshift.set(1)
if (self.rightStick.getRawButton(2)):
self.tx = self.ll.getNumber("tx", None)
if (not type(self.tx) == type(0.0)):
self.tx = 0.0
if (self.tx > 1.0):
self.myRobot.arcadeDrive(0, self.tx * .03)
elif (self.tx < -1.0):
self.myRobot.tankDrive(0, self.tx * .03)
else:
self.myRobot.tankDrive(0, 0)
self.pdp.getVoltage()
class Chassis:
# Variable Injection
right_master: WPI_TalonSRX
left_master: WPI_TalonSRX
right_slave: WPI_TalonSRX
left_slave: WPI_TalonSRX
gyro: AHRS
# enabled = will_reset_to(False)
def __init__(self):
self.teleop = False
self.auto = False
self.z_speed = 0
self.y_speed = 0
def setup(self):
# Setting Masters
self.left_slave.follow(self.left_master)
self.right_slave.follow(self.right_master)
# Setting safety
self.left_master.setSafetyEnabled(False)
self.left_slave.setSafetyEnabled(False)
self.right_master.setSafetyEnabled(False)
self.right_slave.setSafetyEnabled(False)
# Setting inversion
self.left_master.setInverted(False)
self.left_slave.setInverted(False)
self.right_master.setInverted(False)
self.right_slave.setInverted(False)
# Setting the Encoders and configuring the Talons
self.right_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder)
self.left_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder)
self.left_master.configVoltageCompSaturation(11)
self.right_master.configVoltageCompSaturation(11)
self.left_master.enableVoltageCompensation(True)
self.right_master.enableVoltageCompensation(True)
# Creating a drive
self.drive = DifferentialDrive(self.left_master, self.right_master)
self.drive.setSafetyEnabled(True)
def set_teleop(self):
""" Sets current mode for teleop. """
self.teleop = True
self.auto = False
def set_auto(self):
""" Sets current mode for auto. """
self.teleop = False
self.auto = True
def set_motors_values(self, left_power: float, right_power: float):
""" Sets the current motor power. """
# print("motors", left_power, right_power)
self.left_master.set(left_power)
self.right_master.set(right_power)
def get_right_encoder(self):
""" Returns the right encoder position. """
return self.right_master.getSelectedSensorPosition()
def get_left_encoder(self):
""" Returns the left encoder position. """
return -self.left_master.getSelectedSensorPosition()
def set_right_motor(self, power: float):
""" Sets the velocity of the right motor. """
self.right_master.set(power)
def set_left_motor(self, power: float):
""" Sets the velocity of the left motor. """
self.left_master.set(power)
def set_speed(self, y_speed, z_speed):
""" Sets the speed of the robot. """
self.y_speed = y_speed
self.z_speed = z_speed
def set_drive(self, y_speed, z_speed):
""" Sets the speed of the robot. """
self.drive.arcadeDrive(y_speed, z_speed)
def reset_encoders(self):
""" Resets the values of both left and right encoders. """
self.left_master.setSelectedSensorPosition(0)
self.right_master.setSelectedSensorPosition(0)
def get_angle(self):
""" Returns Current robot angle. """
return self.gyro.getAngle()
def get_encoder_ticks(self):
""" Returns the current encoder ticks. """
left_pos = self.left_master.getSelectedSensorPosition()
right_pos = self.right_master.getSelectedSensorPosition()
return left_pos, right_pos
def disable(self):
""" Disables the chassis. """
self.teleop = False
self.auto = False
def reset_navx(self):
""" Resets the navx. """
self.gyro.zeroYaw()
def is_auto(self):
""" Returns True if current 'state' is auto, else returns False"""
return self.auto
def execute(self):
if self.teleop:
self.drive.arcadeDrive(self.y_speed, self.z_speed)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
''' Initialization of robot objects. '''
''' Talon SRX Initialization '''
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
''' Motor Groups '''
# drive train motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
''' Victor SPX Initialization '''
# lift motors
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motors
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor
self.cargo = WPI_VictorSPX(5)
''' Encoders '''
# drive train encoders
self.rightEncoder = self.frontRightMotor
self.leftEncoder = self.frontLeftMotor
# lift encoder
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder
self.liftArmEncoder = wpilib.Encoder(5, 6, True)
''' Sensors '''
# Hall Effect Sensor
self.minHall = wpilib.DigitalInput(7)
self.maxHall = wpilib.DigitalInput(4)
self.limitSwitch = wpilib.DigitalInput(3)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' Controller Initialization and Mapping '''
# joystick - 0, 1 | controller - 2
self.joystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
''' Pneumatic Button Status '''
self.clawButtonStatus = Toggle(self.xbox, 2)
self.gearButtonStatus = Toggle(self.joystick, 1)
self.ejectorPinButtonStatus = Toggle(self.xbox, 1)
self.compressorButtonStatus = Toggle(self.xbox, 9)
self.liftHeightButtonStatus = Toggle(self.xbox, 3)
''' Pneumatic Initialization '''
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
''' Smart Dashboard '''
# connection for logging & Smart Dashboard
logging.basicConfig(level=logging.DEBUG)
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
# Smart Dashboard classes
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
''' Timer '''
self.timer = wpilib.Timer()
''' Camera '''
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
''' PID settings for lift '''
self.kP = 0.03
self.kI = 0.0
self.kD = 0.0
self.kF = 0.1
self.PIDLiftcontroller = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.liftEncoder,
output=self)
self.PIDLiftcontroller.setInputRange(0, 400)
self.PIDLiftcontroller.setOutputRange(-0.5, 0.5)
self.PIDLiftcontroller.setAbsoluteTolerance(1.0)
self.PIDLiftcontroller.setContinuous(True)
self.encoderRate = 0
def pidWrite(self, output):
self.encoderRate = output
def robotCode(self):
if self.liftHeightButtonStatus.on:
self.PIDLiftcontroller.setSetpoint(200)
self.liftToHeight = True
elif self.liftHeightButtonStatus.off:
self.PIDLiftcontroller.setSetpoint(0)
self.liftToHeight = False
def hatchOne():
if self.liftEncoder.getDistance() < 80: # Hatch 2
self.lift.set(0.3)
elif self.liftEncoder.getDistance() >= 80:
self.lift.set(0.07)
def hatchTwo():
if self.liftEncoder.getDistance() < 275: # Hatch 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 275:
self.lift.set(0.07)
def cargoOne():
if self.liftEncoder.getDistance() < 150: # Cargo 1
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 150:
self.lift.set(0.05)
def cargoTwo():
if self.liftEncoder.getDistance() < 320: # Cargo 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 320:
self.lift.set(0.05)
def cargoShip():
if self.liftEncoder.getDistance() < 280: # Cargo ship
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 280:
self.lift.set(0.07)
# ''' Button Box Level Mapping '''
# if self.buttonStatusOne.on:
# # hatchOne()
# cargoOne()
# elif self.buttonStatusTwo.on: # comment out for hatch
# cargoTwo()
# elif self.buttonStatusThree.on:
# # hatchTwo()
# cargoShip()
if self.minHall.get() is False:
self.liftEncoder.reset()
if self.limitSwitch.get() is False:
self.liftArmEncoder.reset()
''' Smart Dashboard '''
# compressor state
if self.Compressor.enabled() is True:
self.sd.putString("Compressor Status: ", "Enabled")
elif self.Compressor.enabled() is False:
self.sd.putString("Compressor Status: ", "Disabled")
''' Pneumatics Dashboard States '''
# gear state
if self.DoubleSolenoidOne.get() == 1:
self.sd.putString("Gear Shift: ", "HIGH SPEED!!!")
elif self.DoubleSolenoidOne.get() == 2:
self.sd.putString("Gear Shift: ", "Low")
# ejector state
if self.DoubleSolenoidThree.get() == 2:
self.sd.putString("Ejector Pins: ", "Ejected")
elif self.DoubleSolenoidThree.get() == 1:
self.sd.putString("Ejector Pins: ", "Retracted")
# claw state
if self.DoubleSolenoidTwo.get() == 2:
self.sd.putString("Claw: ", "Open")
elif self.DoubleSolenoidTwo.get() == 1:
self.sd.putString("Claw: ", "Closed")
''' Ultrasonic Range Detection '''
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
if 0.142 <= self.ultraValue <= 0.146:
self.sd.putString("PLAYER STATION RANGE: ", "YES!!!!")
else:
self.sd.putString("PLAYER STATION RANGE: ", "NO!")
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
if 0.70 <= self.cargoUltraValue <= 1.56:
self.sd.putString("HATCH RANGE: ", "HATCH IN RANGE")
else:
self.sd.putString("HATCH RANGE: ", "NOT IN RANGE")
''' Pneumatics Toggles '''
# Compressor
if self.compressorButtonStatus.on:
self.Compressor.start()
elif self.compressorButtonStatus.off:
self.Compressor.stop()
# Claw Toggle
if self.clawButtonStatus.on:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward) # open claw
elif self.clawButtonStatus.off:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse) # close claw
# Ejector Pins Toggle
if self.ejectorPinButtonStatus.on:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward) # eject
elif self.ejectorPinButtonStatus.off:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse) # retract
# Gear Shift Toggle
if self.gearButtonStatus.on:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward) # shift right
elif self.gearButtonStatus.off:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse) # shift left
''' Victor SPX (Lift, Lift Arm, Cargo) '''
# lift control
if self.liftHeightButtonStatus.get() is False:
if self.xbox.getRawButton(5): # hold
self.lift.set(0.05)
elif self.xbox.getRawAxis(3): # up
self.lift.set(self.xbox.getRawAxis(3) * 0.85)
elif self.xbox.getRawAxis(2): # down
self.lift.set(-self.xbox.getRawAxis(2) * 0.45)
else:
self.lift.set(0)
# else:
# if self.liftToHeight is True:
# self.PIDLiftcontroller.enable()
# self.liftHeight = self.encoderRate
# self.lift.set(self.liftHeight)
# else:
# self.PIDLiftcontroller.disable()
# self.lift.set(0)
# # four-bar control
# if self.xbox.getRawButton(6): # hold
# self.liftArm.set(0.12)
# elif not self.xbox.getRawButton(6):
# self.liftArm.set(-self.xbox.getRawAxis(1) * 0.35)
# else:
# self.liftArm.set(0)
# cargo intake control
if self.xbox.getRawButton(7): # hold
self.cargo.set(0.12)
elif self.xbox.getRawAxis(5): # take in
self.cargo.set(self.xbox.getRawAxis(5) * 0.75)
# controller mapping for arcade steering
self.driveAxis = self.joystick.getRawAxis(1)
self.rotateAxis = self.joystick.getRawAxis(2)
# drives drive system using tank steering
if self.DoubleSolenoidOne.get() == 1: # if on high gear
self.divisor = 1.0 # 90% of high speed
self.turnDivisor = 0.8
elif self.DoubleSolenoidOne.get() == 2: # if on low gear
self.divisor = 0.85 # normal slow speed
self.turnDivisor = 0.75
else:
self.divisor = 1.0
if self.driveAxis != 0:
self.leftSign = self.driveAxis / fabs(self.driveAxis)
else:
self.leftSign = 0
if self.rotateAxis != 0:
self.rightSign = self.rotateAxis / fabs(self.rotateAxis)
else:
self.rightSign = 0
self.drive.arcadeDrive(-self.driveAxis * self.divisor,
self.rotateAxis * 0.75)
def autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# timer config
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
self.liftArmEncoder.reset()
def autonomousPeriodic(self):
self.sd.putBoolean("LIFT RESET ", self.minHall.get())
''' Called periodically during autonomous. '''
'''Test Methods'''
def encoder_test():
''' Drives robot set encoder distance away '''
self.rightPos = fabs(self.rightEncoder.getQuadraturePosition())
self.leftPos = fabs(self.leftEncoder.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
def Diagnostics():
''' Smart Dashboard Tests'''
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber("Right Encoder Speed: ",
abs(self.rightEncoder.getQuadratureVelocity()))
self.sd.putNumber("Left Encoder Speed: ",
abs(self.leftEncoder.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ", self.liftEncoder.getDistance())
def Pressure():
self.Compressor.start()
''' Test Execution '''
if self.DS.getGameSpecificMessage() == "pressure":
Pressure()
elif self.DS.getGameSpecificMessage() == "diagnostics":
Diagnostics()
self.robotCode()
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
self.robotCode()
class Drivetrain:
def __init__(self):
self.lDriveF = WPI_TalonSRX(TALON_DRIVE_LF)
self.lDriveR = WPI_TalonSRX(TALON_DRIVE_LR)
self.rDriveF = WPI_TalonSRX(TALON_DRIVE_RF)
self.rDriveR = WPI_TalonSRX(TALON_DRIVE_RR)
#self.rDriveF.setInverted(True)
#self.rDriveR.setInverted(True)
self.lSpeedGroup = wpilib.SpeedControllerGroup(self.lDriveF, self.lDriveR)
self.rSpeedGroup = wpilib.SpeedControllerGroup(self.rDriveF, self.rDriveR)
self.drive = DifferentialDrive(self.lSpeedGroup, self.rSpeedGroup)
self.drive.setSafetyEnabled(False)
self.drivePID = ProtoPID(1/5.0, 0.0, 0.0, 0.0, 0.02)
def arcadeDrive(self, speed: float, angle: float) -> None:
#print(f'arcade drive: {speed}, {angle}')
self.drive.setSafetyEnabled(True)
self.drive.arcadeDrive(speed, angle)
'''
encoder functions
'''
def resetEncoders(self) -> None:
self.lDriveF.setSelectedSensorPosition(0)
self.rDriveR.setSelectedSensorPosition(0)
def getDistance(self) -> float:
return (-self.lDriveF.getSelectedSensorPosition(0) + \
self.rDriveR.getSelectedSensorPosition(0)) \
/ 2 / DRIVE_ENCODER_COUNTS_PER_FOOT
def getTurningDistance(self) -> float:
return (fabs(-lDriveF.getSelectedSensorPosition(0)) + \
abs(rDriveR.getSelectedSensorPosition(0))) \
/ 2 / DRIVE_ENCODER_COUNTS_PER_FOOT
def getRDistance(self) -> float:
return self.rDriveR.getSelectedSensorPosition(0) / DRIVE_ENCODER_COUNTS_PER_FOOT
def getLDistance(self) -> float:
return -self.lDriveF.getSelectedSensorPosition(0) / DRIVE_ENCODER_COUNTS_PER_FOOT
def getRVelocity(self) -> float:
return self.rDriveR.getSelectedSensorVelocity(0)
def getLVelocity(self) -> float:
return -self.lDriveR.getSelectedSensorVelocity(0)
def getYaw(self) -> float:
pass
def encoderWrite(self, rightDistance, leftDistance) -> None:
pass
'''
auton functions
'''
def autonDrivetrain(self, rVelocity: float, lVelocity: float, rDistance: float,
lDistance: float) -> bool:
self.drive.setSafetyEnabled(False)
if fabs(self.getRDistance()) < rDistance:
self.rDriveF.set(-rVelocity)
self.rDriveR.set(-rVelocity)
elif fabs(self.getRDistance()) < rDistance:
self.rDriveF.set(-.1)
self.rDriveR.set(-.1)
else:
self.rDriveF.set(0)
self.rDriveR.set(0)
if fabs(self.getLDistance()) < (lDistance - .2):
self.lDriveF.set(lVelocity)
self.lDriveR.set(lVelocity)
elif fabs(self.getLDistance()) < lDistance:
self.lDriveF.set(.1)
self.lDriveR.set(.1)
else:
self.lDriveF.set(0)
self.lDriveR.set(0)
if fabs(self.getRDistance()) >= rDistance and fabs(self.getLDistance()) >= lDistance:
return True
else:
return False
def autonLimeDrive(self, speed: float, angle: float, area: float) -> bool:
self.drive.arcadeDrive(speed, angle)
if area > 8:
return True
else:
return False
def autonTurning(self, distance: float) -> bool:
angle = 0
if distance > 0:
angle = .5
else:
angle = -.5
if self.getTurningDistance() < distance:
self.drive.arcadeDrive(0, angle)
else:
self.drive.arcadeDrive(0, 0)
return True
return False
def autonStraight(self, distance: float) -> bool:
speed = 0
if distance > 0:
speed = .6
else:
speed = -.6
if self.getDistance() < distance:
self.drive.arcadeDrive(speed , 0)
else:
self.drive.arcadeDrive(0,0)
return True
return False
def autonPID(self, distance: float) -> bool:
if fabs(self.getDistance() < fabs(distance)):
command = self.drivePID.compute(self.getDistance(), distance)
bias = 0
if distance > 0:
bias = .22
else:
bias = -.22
self.drive.arcadeDrive(command + bias, 0)
else:
self.drive.arcadeDrive(0,0)
return True
def velocityMultiplier(self, firstV: float, secondV: float,
firstEncoderSpeed: float, secondEncoderSpeed: float) -> float:
ratio = firstV / secondV
vRatio = firstEncoderSpeed / secondEncoderSpeed
if (vRatio / ratio) < 1:
return (ratio / vRatio) * firstV
else:
return 1.0 * firstV
'''teleop PID drive for testing'''
def pidDrive(self, angle: float, distance: float) -> float:
command = self.drivePID.compute(self.getDistance(), distance)
bias = 0
if distance > 0:
bias = .22
else:
bias = -.22
self.drive.arcadeDrive(command + bias, angle)
class Apollo(wpilib.TimedRobot):
def robotInit(self):
self.xbox = wpilib.XboxController(0)
self.lift_lock = Toggle(self.xbox, 4, 0.5)
self.left_drive_motor_group = wpilib.SpeedControllerGroup(
wpilib.Talon(0), wpilib.Talon(1))
self.right_drive_motor_group = wpilib.SpeedControllerGroup(
wpilib.Talon(2), wpilib.Talon(3))
self.drive = DifferentialDrive(self.left_drive_motor_group,
self.right_drive_motor_group)
self.drive_rev = False
self.speedRatio = 0.5
self.lift_motor = wpilib.SpeedControllerGroup(wpilib.Spark(4),
wpilib.Spark(5))
self.lift_motor_speed = 0.0
# ball grab motors, need to spin in opposite directions
self.front_motor_1 = wpilib.Spark(6)
self.front_motor_2 = wpilib.Spark(7)
self.front_motor_2.setInverted(True)
self.front_motor = wpilib.SpeedControllerGroup(self.front_motor_1,
self.front_motor_2)
self.hatch_solenoid = wpilib.DoubleSolenoid(
0, 1) # pneumatic channels 0 & 1
self.encoder = wpilib.Encoder(aChannel=0, bChannel=1) # DIO 0 & 1
wpilib.CameraServer.launch(
'vision.py:main') # setup cameras: usb1 & usb2
self.xbox_axis = {} # init xbox axis dict
self.debug = False # enable to print debug info
self.reset() # i.e. ensure defaults are set
self.loops = 0 # counter for program loops
self.timer = wpilib.Timer() # init timer
def reset(self):
# safely lower lift
while self.lift_motor_speed > 0.0:
self.lift_motor_speed -= 0.002 if self.lift_motor_speed > 0.0 else 0.0
self.lift_motor.set(self.lift_motor_speed)
self.front_motor.set(0)
self.hatch_solenoid.set(0)
self.encoder.reset()
# init xbox axis info
for i in range(0, self.xbox.getAxisCount()):
self.xbox_axis[i] = self.xbox.getRawAxis(i)
def drive_control(self):
if self.xbox.getAButtonPressed():
if self.speedRatio == 2 / 3:
self.speedRatio = 0.5
elif self.speedRatio == 0.5:
self.speedRatio = 2 / 3
lh_y = self.xbox.getY(self.xbox.Hand.kLeft
) * self.speedRatio # incr speed from 1/2 to 2/3
rh_x = self.xbox.getX(self.xbox.Hand.kRight) * 2 / 3
if self.xbox.getStickButtonPressed(self.xbox.Hand.kLeft) \
or self.xbox.getStickButtonPressed(self.xbox.Hand.kRight):
self.drive_rev = not self.drive_rev
if self.drive_rev:
self.drive.arcadeDrive(lh_y, rh_x, True)
else:
self.drive.arcadeDrive(lh_y * -1, rh_x, True)
def button_status(self):
# output to logs button or axis # and value
for i in range(0, self.xbox.getAxisCount()):
cur_axis = self.xbox.getRawAxis(i)
if self.xbox_axis[i] != cur_axis:
self.xbox_axis[i] = cur_axis
self.logger.info("Axis {} = {}".format(i, cur_axis))
for j in range(1, self.xbox.getButtonCount() + 1):
if self.xbox.getRawButtonPressed(j):
self.logger.info("Button {} pressed".format(j))
def lift_control(self):
high_volt = 0.9 # volt high limit
init_volt = 0.45 # voltage to start raising
low_volt = 0.2 # volt low limit
volt_rate_raise = 0.01 # step voltage down
volt_rate_low = 0.005 # step voltage up
def trigger_pressed(hand):
# wrapper case to detect a Xbox Trigger hard press
return self.xbox.getTriggerAxis(hand) > 0.9
def auto_lower():
# redundant logic to reach max holding voltage while falling
if (round(self.lift_motor_speed, 2) > low_volt) and (int(
abs(self.encoder.getRate())) > 0):
self.lift_motor_speed -= volt_rate_low
# when voltage is below lower limit, then slowly reduce to zero
# slow fall to bottom out
elif (0.0 < round(self.lift_motor_speed, 2) < low_volt) and (int(
abs(self.encoder.getRate())) == 0):
self.lift_motor_speed -= volt_rate_low if round(
self.lift_motor.get(), 2) > 0.0 else 0.00
# fail-safe for when the speed goes negative
# shouldn't happen, but was left in case
elif self.lift_motor_speed < 0:
self.lift_motor_speed = 0
if self.lift_lock.on:
# disable trigger input and allow the voltage to reduce to holding state
auto_lower()
else:
# Left Trigger press
if trigger_pressed(self.xbox.Hand.kLeft):
# when starting, or below our low volt limit, then set speed to initial ramp voltage
if (self.lift_motor_speed is 0.0) or (self.lift_motor_speed <=
low_volt):
self.lift_motor_speed = init_volt
# while the lift is not moving, slowly increase the voltage
# don't pass high voltage limit
elif (int(abs(self.encoder.getRate()))
== 0) and (self.lift_motor_speed <= high_volt):
self.lift_motor_speed += volt_rate_raise
# Right Trigger press
# with B button held, lower to 0 volts
elif trigger_pressed(
self.xbox.Hand.kRight) and self.xbox.getBButton():
self.lift_motor_speed -= volt_rate_low if self.lift_motor.get(
) > 0.0 else 0.0
# while voltage is positive, then lower at lower rate, then bottom out at low volt limit
elif trigger_pressed(
self.xbox.Hand.kRight) and (self.lift_motor_speed > 0):
self.lift_motor_speed -= volt_rate_low if round(
self.lift_motor.get(), 2) > low_volt else low_volt
# no trigger pressed, then run auto set code
# will either keep lowest holding voltage or lower to zero
else:
auto_lower()
# set lift motor speed
self.lift_motor.set(self.lift_motor_speed)
def grab_control(self):
if self.xbox.getBumper(self.xbox.Hand.kLeft) \
and not self.xbox.getBumper(self.xbox.Hand.kRight):
self.front_motor.set(0.33)
elif self.xbox.getBumperPressed(self.xbox.Hand.kRight) \
and not self.xbox.getBumper(self.xbox.Hand.kLeft):
self.front_motor.set(-0.7)
elif self.xbox.getBumperReleased(self.xbox.Hand.kLeft) \
or self.xbox.getBumperReleased(self.xbox.Hand.kRight):
self.front_motor.set(0)
def hatch_control(self):
if self.xbox.getXButtonPressed():
self.hatch_solenoid.set(2)
elif self.xbox.getXButtonReleased():
self.hatch_solenoid.set(1)
def robotPeriodic(self):
pass
def autonomousInit(self):
self.reset()
def autonomousPeriodic(self):
self.drive_control()
self.lift_control()
self.grab_control()
self.hatch_control()
if self.debug:
self.button_status()
def disabledInit(self):
self.reset()
def disabledPeriodic(self):
self.reset()
def teleopInit(self):
self.reset()
self.drive.setSafetyEnabled(True)
self.loops = 0
self.timer.reset()
self.timer.start()
def teleopPeriodic(self):
self.drive_control()
self.lift_control()
self.grab_control()
self.hatch_control()
if self.debug:
self.button_status()
class Robot(wpilib.IterativeRobot):
def __init__(self):
super().__init__()
# Initiate up robot drive
self.left_motor = wpilib.Spark(LEFT_PORT)
self.right_motor = wpilib.Spark(RIGHT_PORT)
self.drive = DifferentialDrive(self.left_motor, self.right_motor)
self.drive.setExpiration(.1)
# Initiate arm and lift
self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT)
self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2)
# Initiate button stuff
self.buttons = set()
self.button_toggles = None
self.pressed_buttons = {}
self.rumble_time = None
self.rumble_length = RUMBLE_LENGTH
self.reset_buttons()
# Initiate gamepad
self.game_pad = wpilib.Joystick(GAMEPAD_NUM)
self.max_enhancer = 0
self.smart_dashboard = wpilib.SmartDashboard
# auto_chooser = wpilib.SendableChooser()
# auto_chooser.addDefault("Enable Auto", "Enable Auto")
# auto_chooser.addObject("Disable Auto", "Disable Auto")
self.smart_dashboard.putBoolean("Enable Auto", True)
self.auto_start_time = None
self.current_speed = [0, 0]
self.last_tank = 0
self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel
self.tank_dir = None
def reset_buttons(self):
"""Resets the values of the button toggles to default likewise
defined here
"""
self.button_toggles = {
"REVERSE": False,
"LOCK": True,
"STOP": False,
"SPEED": False,
"GRAB": False
}
self.pressed_buttons = {}
self.rumble_time = None
def stop(self):
self.reset_buttons()
self.last_tank = get_millis()
self.drive.stopMotor()
self.robot_lift.stopMotor()
self.robot_lift_2.stopMotor()
self.current_speed = [0, 0]
# Events
def robotInit(self):
"""Runs at the same time as __init__. Appears to be redundant in this
case
"""
self.stop()
def robotPeriodic(self):
pass
def disabledInit(self):
self.stop()
def disabledPeriodic(self):
self.stop()
def autonomousInit(self):
self.stop()
self.auto_start_time = None
self.gyro.calibrate() # Takes five seconds
self.stop() # Have to reset tank time to correct acceleration
def tank(self, left, right, accel=None):
if left == right:
if isinstance(self.tank_dir, type(None)):
self.tank_dir = self.gyro.getAngle()
else:
self.tank_dir = None
turn = None
current_angle = None
if not isinstance(self.tank_dir, type(None)):
current_angle = self.gyro.getAngle()
if current_angle < self.tank_dir - ANGLE_MARGIN:
turn = "right"
elif current_angle > self.tank_dir + ANGLE_MARGIN:
turn = "left"
if isinstance(accel, type(None)):
accel = ACCEL
speed1 = self.current_speed[0]
speed2 = self.current_speed[1]
if not isinstance(turn, type(None)) and GYRO_ENABLE:
if turn == "left":
left -= ANGLE_CHANGE
right += ANGLE_CHANGE
elif turn == "right":
right -= ANGLE_CHANGE
left += ANGLE_CHANGE
print((round(self.tank_dir, 3)
if not isinstance(self.tank_dir, type(None)) else None),
(round(current_angle, 3)
if not isinstance(current_angle, type(None)) else None),
round(left, 3), round(right, 3), turn)
if .4 > speed1 > 0:
if left <= 0:
speed1 = 0
else:
speed1 = .4
if -.4 < speed1 < 0:
if left >= 0:
speed1 = 0
else:
speed1 = -.4
if .4 > speed2 > 0:
if right <= 0:
speed2 = 0
else:
speed2 = .4
if -.4 < speed2 < 0:
if right >= 0:
speed2 = 0
else:
speed2 = -.4
if isinstance(accel, type(None)):
self.drive.tankDrive(left, right)
return None
vel_change = ((get_millis() - self.last_tank) / 1000) * accel
if abs(speed1 - left) < WHEEL_LOCK:
speed1 = left
else:
if speed1 < left:
speed1 += vel_change
elif speed1 > left:
speed1 -= vel_change
if abs(speed2 - right) < WHEEL_LOCK:
speed2 = right
else:
if speed2 < right:
speed2 += vel_change
elif speed2 > right:
speed2 -= vel_change
self.current_speed = [speed1, speed2]
# print([round(left, 2), round(right, 2)], [round(speed1, 2), round(speed2, 2)], round(vel_change, 4))
self.drive.tankDrive(*self.current_speed)
# print([round(x, 2) for x in self.current_speed], round(vel_change, 2), accel)
self.last_tank = get_millis()
def autonomousPeriodic(self):
if self.smart_dashboard.getBoolean("Auto Enabled", True):
if not isinstance(self.auto_start_time, type(None)):
# Auto enabled and running
# percent = (get_millis() - self.auto_start_time) / AUTO_DUR
# if percent < .5:
# speed = (percent/.5) * AUTO_SPEED
# elif percent < 1:
# speed = ((1 - percent)/.5) * AUTO_SPEED
# else:
# speed = 0
if get_millis() >= self.auto_start_time + AUTO_DUR / 2:
self.tank(0, 0, AUTO_ACCEL)
else:
self.tank(AUTO_SPEED, AUTO_SPEED, AUTO_ACCEL)
else:
# Auto enabled and not started
self.auto_start_time = get_millis()
def teleopInit(self):
self.stop()
self.drive.setSafetyEnabled(True)
def set_rumble(self, left=True, value=1, length=RUMBLE_LENGTH):
"""Sets given rumble to given value"""
if not left:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kLeftRumble,
value)
else:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kRightRumble,
value)
self.rumble_time = get_millis()
self.rumble_length = length
def check_rumble(self):
"""Checks if rumbling has surpassed RUMBLE_LENGTH"""
if not isinstance(self.rumble_time, type(None)) and get_millis(
) > self.rumble_time + self.rumble_length:
# This rumbling has gone on long enough!
self.set_rumble(True, 0)
self.set_rumble(False, 0)
self.rumble_time = None
def execute_buttons(self):
"""Check whether each button was just pressed and updates relevant
toggle
"""
arms = 0
button_states = self.get_buttons()
for button_name in BUTTON_PORTS:
port = BUTTON_PORTS[button_name]
angle = -1
if isinstance(port, list):
angle = port[1]
port = port[0]
if port in button_states and button_states[port] is True:
# button was just pressed
if button_name in self.button_toggles:
# needs toggled
if button_name == "GRAB":
pass
# print(button_name, port == POV,
# self.get_raw_buttons()[POV], angle)
if not (port == POV
and not self.get_raw_buttons()[POV] == angle):
new_state = not self.button_toggles[button_name]
self.button_toggles[button_name] = new_state
self.set_rumble(new_state)
# print(button_name, new_state)
elif self.get_raw_buttons()[POV] == angle:
# Button angle correct, check button name
if button_name == "INC SPEED":
self.max_enhancer += SPEED_ADJUST
self.set_rumble(True, 1)
elif button_name == "DEC SPEED":
self.max_enhancer -= SPEED_ADJUST
self.set_rumble(False, 1)
elif button_name == "RES SPEED":
self.max_enhancer = 0
self.set_rumble(True, length=200)
elif port in button_states:
# BUTTON BEING PRESSED
if button_name == "ARM IN":
arms = max(-1, min(-(ARM_SPEED_IN), 1))
self.button_toggles["GRAB"] = False
elif button_name == "ARM OUT":
arms = max(-1, min(ARM_SPEED_OUT, 1))
self.button_toggles["GRAB"] = False
# if arms == 0 and self.button_toggles["GRAB"]:
# self.robot_lift_2.set(-ARM_SPEED_IN)
# else:
# self.robot_lift_2.set(arms)
self.check_rumble()
def execute_axes(self):
"""Checks each axis and updates attached motor"""
axes = self.get_axes()
if not self.button_toggles["STOP"]:
self.tank(axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS])
# self.left_motor.set(axes[LEFT_WHEELS_AXIS])
# self.right_motor.set(axes[RIGHT_WHEELS_AXIS])
left_trigger = axes[LIFT_AXIS1]
right_trigger = axes[LIFT_AXIS2]
if left_trigger > 0:
if right_trigger <= 0:
self.robot_lift.set(-left_trigger)
self.robot_lift_2.set(-left_trigger)
elif right_trigger > 0:
self.robot_lift.set(right_trigger)
# print(right_trigger)
else:
self.robot_lift.set(0)
self.robot_lift_2.set(0)
def teleopPeriodic(self):
"""Runs repeatedly while robot is in teleop"""
self.execute_buttons()
self.execute_axes()
def get_raw_axes(self, _round=False):
"""Return a dictionary of controller axes"""
i = 0
axes = {}
while True:
try:
value = self.game_pad.getRawAxis(i)
if i == RIGHT_WHEELS_AXIS:
if value < 0:
value /= .9 # Right stick does not go full forward
value = min(value, 1)
if _round:
axes[i] = round(value, 2)
else:
axes[i] = value
except IndexError:
break
i += 1
return axes
def get_axes(self, _round=False):
"""Returns the current axes and values with deadzones and scaled"""
axes = self.get_raw_axes(_round)
# Correct deadzones and scale
for axis in axes:
if axis != LIFT_AXIS1 and axis != LIFT_AXIS2:
if axis in DEADZONES:
value = axes[axis]
neg = -1 if value < 0 else 1
deadzone = DEADZONES[axis]
if abs(value) > deadzone:
value -= deadzone * neg
value /= 1 - deadzone
if not self.button_toggles[
"SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[axis][value >= 0] +
self.max_enhancer, 1))
else:
value = 0
if REVERSES[axis]:
value *= -1
axes[axis] = value
else: # TODO: Clean up
# TRIGGERS FOR LIFT
value = axes[axis]
deadzone = DEADZONES[LIFT_AXIS1]
if value > deadzone:
value -= deadzone
value /= 1 - deadzone
if not self.button_toggles["SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[LIFT_AXIS1][axis == LIFT_AXIS1] +
self.max_enhancer, 1))
if self.button_toggles["REVERSE"]:
axes[LEFT_WHEELS_AXIS] *= -1
axes[RIGHT_WHEELS_AXIS] *= -1
dif, avg = None, None
if self.button_toggles["LOCK"]:
left = axes[LEFT_WHEELS_AXIS]
right = axes[RIGHT_WHEELS_AXIS]
dif = abs(abs(left) - abs(right))
if dif <= WHEEL_LOCK and left != 0 and right != 0:
lneg = left < 0
rneg = right < 0
smaller = min(abs(left), abs(right))
avg = smaller + dif / 2
axes[LEFT_WHEELS_AXIS] = avg
axes[RIGHT_WHEELS_AXIS] = avg
if lneg:
axes[LEFT_WHEELS_AXIS] *= -1
if rneg:
axes[RIGHT_WHEELS_AXIS] *= -1
# print(self.button_toggles["LOCK"], left, right,
# axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS], dif, avg)
return axes
def get_raw_buttons(self):
"""Return a dictionary of raw button states"""
i = 1 # Controller buttons start at 1
buttons = {}
while i <= 10:
# Gets ten buttons or stops at index error <-- shouldn't happen
try:
buttons[i] = self.game_pad.getRawButton(i)
except IndexError:
break
i += 1
buttons[i] = self.game_pad.getPOV(0)
return buttons
def get_buttons(self):
"""Returns a dictionary of bools representing whether each button was
just pressed
"""
raw_buttons = self.get_raw_buttons()
for button in raw_buttons:
being_pressed = (
not raw_buttons[button] is False) and raw_buttons[button] != -1
if button not in self.pressed_buttons and being_pressed:
# If button not already accounted for and being pressed
self.pressed_buttons[button] = True
elif button in self.pressed_buttons:
if being_pressed:
# Being pressed, already used
self.pressed_buttons[button] = False
else:
# Was pressed, no longer being pressed
del self.pressed_buttons[button]
return self.pressed_buttons
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""Init is called once when the robot is turned on."""
self.efacing = 1
"""efacing is used to invert our controls."""
self.CarEncoder = wpilib.Encoder(0, 1)
#self.HatchEncoder = wpilib.Encoder(3, 4)
self.chooser = wpilib.SendableChooser()
self.chooser.addObject('cargo', '1')
self.chooser.addObject('hatch panel', '2')
self.left = wpilib.VictorSP(0)
self.right = wpilib.VictorSP(1)
"""Motors used for driving"""
self.myRobot = DifferentialDrive(self.left, self.right)
"""DifferentialDrive is the main object that deals with driving"""
self.RotServo = wpilib.Servo(2)
self.TiltServo = wpilib.Servo(3)
"""Our two servos will rotate (RotServo) and tilt (TiltServo) our
vision cameras."""
self.motor1 = wpilib.Talon(5)
self.motor2 = wpilib.Talon(6)
"""I mostly just added these motor controllers anticipating some sort
of intake system that uses motors."""
self.Punch = wpilib.DoubleSolenoid(0, 1)
self.DPunch = wpilib.DoubleSolenoid(3, 2)
"""The punching mechanism for removal of the hatch panels can use a
DoubleSolenoid or regular Solenoid. The Solenoid only needs the channel
it's plugged into (4) whereas the Double Solenoid needs the module
number, forward channel number, and reverse channel order in that
order."""
self.XBox0 = wpilib.XboxController(0)
self.XBox1 = wpilib.XboxController(1)
"""Xbox controllers 1 and 2 on the driver station."""
self.myRobot.setExpiration(0.1)
"""If communication is cut off between the driver station and the robot
for over 0.1 seconds, the robot will emergency stop."""
def teleopInit(self):
"""Executed once at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
"""Motor Safety is another version of setExperiation, except motor
safety is from the WPI Library."""
def teleopPeriodic(self):
"""Called every 20 ms during teleop"""
if self.XBox0.getStartButtonPressed():
self.efacing *= -1
"""This will invert our controls."""
if self.XBox0.getAButton():
self.Punch.set(DoubleSolenoid.Value.kForward)
print ("Punch is Forward")
else:
self.Punch.set(DoubleSolenoid.Value.kReverse)
if self.XBox0.getBButton():
self.DPunch.set(DoubleSolenoid.Value.kForward)
print ("DPunch is Forward")
else:
self.DPunch.set(DoubleSolenoid.Value.kReverse)
self.myRobot.arcadeDrive(self.XBox0.getY(0)* -self.efacing, self.XBox0.getX(0)* self.efacing)
"""The efacing variable is here to invert our controls. It's negative
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""Robot initialization function"""
self.frontLeft = wpilib.Spark(1)
self.rearLeft = wpilib.Spark(2)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.frontRight = wpilib.Spark(3)
self.rearRight = wpilib.Spark(4)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.drive = DifferentialDrive(self.left, self.right)
# object that handles basic drive operations
#self.myRobot = wpilib.Drive.DifferentialDrive(0, 1)
#self.myRobot.setExpiration(0.1)
# joystick #0
self.stick = wpilib.Joystick(0)
def disabledInit(self):
self.drive.setSafetyEnabled(False)
def disabledPeriodic(self):
self.drive.setSafetyEnabled(False)
def robotPeriodic(self):
self.drive.setSafetyEnabled(False)
def testInit(self):
print("Test Init")
def testPeriodic(self):
if (self.stick.getRawButtonPressed(1) == True):
print("Test Button 1 Pressed.")
def autonomousInit(self):
self.drive.setSafetyEnabled(False)
def autonomousPeriodic(self):
"""Called when autonomous mode is enabled"""
while self.isAutonomous() and self.isEnabled():
#wpilib.Timer.delay(0.01)
self.drive.arcadeDrive(0.20, 0.1)
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.drive.setSafetyEnabled(False)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
# timer = wpilib.Timer()
# timer.start()
while self.isOperatorControl() and self.isEnabled():
# Move a motor with a Joystick
#wpilib.Timer.delay(0.02)
self.drive.arcadeDrive(self.stick.getRawButton(1), True)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.leftMotor = wpilib.Victor(0)
self.rightMotor = wpilib.Victor(1)
#self.myRobot = wpilib.RobotDrive(0, 1)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
# joyStick 0
self.joyStick = wpilib.Joystick(0)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
# encoders
self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k4X)
self.leftEncoder = wpilib.Encoder(2,3, False, wpilib.Encoder.EncodingType.k4X)
# set up encoder
self.drivePulsePerRotation = 1024
self.driveWheelRadius = 3.0
self. driveGearRatio = 1.0/1.0
self.driveEncoderPulsePerRot = self.drivePulsePerRotation * self.driveGearRatio
self.driveEncoderDistPerPulse = (math.pi*2*self.driveWheelRadius)/self.driveEncoderPulsePerRot;
self.leftEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.leftEncoder.setReverseDirection(False)
self.rightEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
self.rightEncoder.setReverseDirection(False)
self.timer = wpilib.Timer()
def autonomousInit(self):
'''
:return:
'''
self.timer.reset()
self.timer.start()
print("START")
self.printEncoderValues("Left Encoder",self.leftEncoder)
self.printEncoderValues("Right Encoder", self.rightEncoder)
def autonomousPeriodic(self):
if not self.timer.hasPeriodPassed(5):
self.leftMotor.set(0.5)
self.rightMotor.set(0.5)
else:
self.leftMotor.set(0)
self.rightMotor.set(0)
self.timer.stop()
print("FINISH")
self.printEncoderValues("Left Encoder", self.leftEncoder)
self.printEncoderValues("Right Encoder", self.rightEncoder)
def teleopInit(self):
"""Executed at the start of teleop mode
:return:
"""
pass
def teleopPeriodic(self):
"""Runs the motors with tank steering
:return:
"""
move = self.joyStick.getRawAxis(1)
turn = self.joyStick.getRawAxis(4)
self.myRobot.arcadeDrive(move, turn)
def printEncoderValues(self, name,encoder):
print(name)
print()
print("{0}: {1}".format("DistancePerPulse",encoder.getDistancePerPulse()))
print("{0}: {1}".format("Distance", encoder.getDistance()))
print("{0}: {1}".format("Raw Count", encoder.getRaw()))
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(1)
# self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(2)
self.frontRightMotor = wpilib.Spark(3)
# self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(4)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = ''
# joysticks 1 & 2 on the driver station
self.Stick1 = wpilib.XboxController(0)
self.Stick2 = wpilib.Joystick(1)
self.aSolenoidLow = wpilib.DoubleSolenoid(2, 3)
self.aSolenoidHigh = wpilib.DoubleSolenoid(0, 1)
self.iSolenoid = wpilib.DoubleSolenoid(4, 5)
self.gyro = wpilib.ADXRS450_Gyro()
def autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance(
).getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
global firstTime
firstTime = True
def autonomousPeriodic(self):
# This program tests 90 degree turn with gyro
global firstTime, maxV, minV
while firstTime:
global fD, fluc, sD, v
sD = self.gyro.getAngle()
fD = sD - 90
firstTime = False
v = 0.65
cD = self.gyro.getAngle()
# left smaller right bigger
if cD > fD:
cD = self.gyro.getAngle()
speed_turn = v
self.myRobot.tankDrive(-speed_turn, speed_turn)
print(cD)
else:
self.myRobot.tankDrive(0, 0)
def disabledInit(self):
self.myRobot.tankDrive(0, 0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
self.iSolenoid.set(1)
lastspeed = 0
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sp = forward - backward
if abs(self.Stick1.getX(0)) > 0.1:
steering = (self.Stick1.getX(0) + 0.1 * sp) / 1.5
else:
steering = 0
self.myRobot.tankDrive(sp + steering, sp - steering)
class MyRobot(wpilib.TimedRobot):
'''Main robot class'''
# NetworkTables API for controlling this
#: Speed to set the motors to
autospeed = ntproperty('/robot/autospeed',
defaultValue=0,
writeDefault=True)
#: Test data that the robot sends back
telemetry = ntproperty('/robot/telemetry',
defaultValue=(0, ) * 9,
writeDefault=False)
prior_autospeed = 0
WHEEL_DIAMETER = 0.5
ENCODER_PULSE_PER_REV = 360
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
left_front_motor = wpilib.Spark(1)
left_front_motor.setInverted(False)
right_front_motor = wpilib.Spark(2)
right_front_motor.setInverted(False)
left_rear_motor = wpilib.Spark(3)
left_rear_motor.setInverted(False)
right_rear_motor = wpilib.Spark(4)
right_rear_motor.setInverted(False)
#
# Configure drivetrain movement
#
l = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor)
r = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor)
self.drive = DifferentialDrive(l, r)
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0)
#
# Configure encoder related functions -- getpos and getrate should return
# ft and ft/s
#
encoder_constant = (
1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi
l_encoder = wpilib.Encoder(0, 1)
l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder_getpos = l_encoder.getDistance
self.l_encoder_getrate = l_encoder.getRate
r_encoder = wpilib.Encoder(2, 3)
r_encoder.setDistancePerPulse(encoder_constant)
self.r_encoder_getpos = r_encoder.getDistance
self.r_encoder_getrate = r_encoder.getRate
# Set the update rate instead of using flush because of a NetworkTables bug
# that affects ntcore and pynetworktables
# -> probably don't want to do this on a robot in competition
NetworkTables.setUpdateRate(0.010)
def disabledInit(self):
self.logger.info("Robot disabled")
self.drive.tankDrive(0, 0)
def disabledPeriodic(self):
pass
def robotPeriodic(self):
# feedback for users, but not used by the control program
sd = wpilib.SmartDashboard
sd.putNumber('l_encoder_pos', self.l_encoder_getpos())
sd.putNumber('l_encoder_rate', self.l_encoder_getrate())
sd.putNumber('r_encoder_pos', self.r_encoder_getpos())
sd.putNumber('r_encoder_rate', self.r_encoder_getrate())
def teleopInit(self):
self.logger.info("Robot in operator control mode")
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX())
def autonomousInit(self):
self.logger.info("Robot in autonomous mode")
def autonomousPeriodic(self):
'''
If you wish to just use your own robot program to use with the data
logging program, you only need to copy/paste the logic below into
your code and ensure it gets called periodically in autonomous mode
Additionally, you need to set NetworkTables update rate to 10ms using
the setUpdateRate call.
Note that reading/writing self.autospeed and self.telemetry are
NetworkTables operations (using pynetworktables's ntproperty), so
if you don't read/write NetworkTables in your implementation it won't
actually work.
'''
# Retrieve values to send back before telling the motors to do something
now = wpilib.Timer.getFPGATimestamp()
l_encoder_position = self.l_encoder_getpos()
l_encoder_rate = self.l_encoder_getrate()
r_encoder_position = self.r_encoder_getpos()
r_encoder_rate = self.r_encoder_getrate()
battery = self.ds.getBatteryVoltage()
motor_volts = battery * abs(self.prior_autospeed)
l_motor_volts = motor_volts
r_motor_volts = motor_volts
# Retrieve the commanded speed from NetworkTables
autospeed = self.autospeed
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive(autospeed, autospeed, False)
# send telemetry data array back to NT
self.telemetry = (now, battery, autospeed, l_motor_volts,
r_motor_volts, l_encoder_position,
r_encoder_position, l_encoder_rate, r_encoder_rate)
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.leftVictor = ctre.WPI_VictorSPX(LEFT)
self.rightVictor = ctre.WPI_VictorSPX(RIGHT)
self.centerVictor1 = ctre.WPI_VictorSPX(CENTER1)
self.centerVictor2 = ctre.WPI_VictorSPX(CENTER2)
self.left = wpilib.SpeedControllerGroup(self.leftVictor)
self.right = wpilib.SpeedControllerGroup(self.rightVictor)
self.center1 = wpilib.SpeedControllerGroup(self.centerVictor1)
self.center2 = wpilib.SpeedControllerGroup(self.centerVictor2)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
# self.leftStick = wpilib.Joystick(0)
# self.rightStick = wpilib.Joystick(1)
self.DEADZONE = 0.4
self.LEFT = GenericHID.Hand.kLeft
self.RIGHT = GenericHID.Hand.kRight
self.driver = wpilib.XboxController(0)
self.ballManipulator = BallManipulator(
ctre.WPI_VictorSPX(BALL_MANIP_ID))
def autonomousInit(self):
self.myRobot.tankDrive(0.8, 0.8)
def autonomousPeriodic(self):
self.myRobot.tankDrive(1, 0.5)
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def setCenters(self, speed_value):
self.center1.set(-speed_value)
self.center2.set(speed_value)
def deadzone(self, val, deadzone):
if abs(val) < deadzone:
return 0
return val
def teleopPeriodic(self):
ballMotorSetPoint = 0
if self.driver.getBumper(self.LEFT):
ballMotorSetPoint = 1.0
elif self.driver.getBumper(self.RIGHT):
ballMotorSetPoint = -1.0
else:
ballMotorSetPoint = 0.0
self.ballManipulator.set(ballMotorSetPoint)
"""Runs the motors with tank steering"""
#right = self.driver.getY(self.RIGHT)
#left = self.driver.getY(self.LEFT)
#self.myRobot.tankDrive(right, left)
forward = -self.driver.getRawAxis(5)
rotation_value = rotation_value = self.driver.getX(LEFT_HAND)
forward = deadzone(forward, 0.2)
self.myRobot.arcadeDrive(forward, rotation_value)
center_speed = self.driver.getX(self.RIGHT)
self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class MyRobot(wpilib.TimedRobot):
def robotInit(self):
"""
Initializes all motors in the robot.
"""
self.leftTalon = ctre.WPI_TalonSRX(LEFT)
self.rightTalon = ctre.WPI_TalonSRX(RIGHT)
self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1)
self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2)
self.left = wpilib.SpeedControllerGroup(self.leftTalon)
self.right = wpilib.SpeedControllerGroup(self.rightTalon)
self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1)
self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2)
self.myRobot = DifferentialDrive(self.right, self.left)
self.myRobot.setExpiration(0.1)
# reasonable deadzone size
self.DEADZONE = 0.1
self.driver = wpilib.XboxController(0)
# Toggles whether or not robot can move
self.emergencyStop = False
def autonomousInit(self):
pass #Do nothing for now in auton
def autonomousPeriodic(self):
pass #Do nothing for now in auton
def teleopInit(self):
"""
Huh?
"""
self.myRobot.setSafetyEnabled(True)
def deadzone(self, val, deadzone):
if abs(val) < deadzone:
return 0
return val
def teleopPeriodic(self):
"""
What does this do?
"""
"""
This function inverts the input from the stick for movement,
and takes the input from the stick for rotation and sets them
equal to the forward movement speed and rotation speed respecively.
Then it calls a function to move the robot with the given parameters.
"""
# forward = self.driver.getY(RIGHT_HAND)
# rotation_value = self.driver.getX(LEFT_HAND)
forward = -self.driver.getRawAxis(1)
rotation_value = -self.driver.getRawAxis(4)
forward = deadzone(forward, 0.2) #Safety
# Toggles emergency stop on A button pressed
if self.driver.getAButtonPressed():
self.emergencyStop = not self.emergencyStop
if self.emergencyStop:
forward = 0
rotation_value = 0
#comment
self.myRobot.arcadeDrive(forward, rotation_value) #Actualy move
