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.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.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 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):
'''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 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 MyRobot(wpilib.TimedRobot):
def robotInit(self):
#drive
self.frontLeft = wpilib.Talon(0)
self.frontRight = wpilib.Talon(1)
self.backLeft = wpilib.Talon(2)
self.backRight = wpilib.Talon(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.backLeft)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.backRight)
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# Pnematics
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.Ps = self.Compressor.getPressureSwitchValue()
self.tank2 = wpilib.DoubleSolenoid(0, 0, 1)
self.tank1 = wpilib.DoubleSolenoid(0, 2, 3)
self.Compressor.start()
# xbox cont
self.xbox1 = wpilib.XboxController(1)
self.yL = self.xbox1.getRawAxis(1)
self.xL = self.xbox1.getRawAxis(0)
self.yR = self.xbox1.getRawAxis(5)
self.xR = self.xbox1.getRawAxis(4)
self.leftTrig = self.xbox1.getRawAxis(2)
self.rightTrig = self.xbox1.getRawAxis(3)
self.buttonB = self.xbox1.getBButtonPressed()
self.buttonA = self.xbox1.getAButtonPressed()
self.timer = wpilib.Timer()
self.loops = 0
#vision
wpilib.CameraServer.launch()
def autonomousInit(self):
pass
def autonomousPeriodic(self):
pass
def disabledInit(self):
self.logger.info("%d loops / %f seconds", self.loops, self.timer.get())
def disabledPeriodic(self):
pass
def teleopInit(self):
self.loops = 0
self.timer.reset()
self.timer.start()
def teleopPeriodic(self):
self.loops += 1
if self.timer.hasPeriodPassed(1):
self.logger.info("%d loops / second", self.loops)
self.loops = 0
#drive tele
self.speed = self.yL - ((self.yL * self.rightTrig) + .01)
self.zaxisArcade = self.xR - ((self.xR * self.rightTrig) + .01)
self.drive.arcadeDrive(self.speed, self.zaxisArcade)
#pnematics tele
if self.buttonA == True:
self.tank1.set(1)
self.tank2.set(2)
if self.buttonB == True:
self.tank1.set(2)
self.tank2.set(1)
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 init(self):
# Enable safety
self.myRobot.setSafetyEnabled(False)
# Start the compressor running in closed loop control mode
self.compressor.start()
def periodic(self):
if not self.toggleDirectionButton.get():
self.directionActed = False
if not self.toggleHatchButton.get():
self.hatchActed = False
if not self.toggleBasketButton.get():
self.basketActed = False
# If rightButton2 is pressed; change the direction to forward
if self.toggleDirectionButton.get() and not self.directionActed:
self.directionActed = True
self.direction = -self.direction
# Tank drive with left and right sticks' Y axis
if self.speedUpButton.get():
# Use full axis value for full speed
self.myRobot.tankDrive(
self.gamepad.getRawAxis(1) * self.direction,
self.gamepad.getRawAxis(5) * self.direction)
else:
# Use half of the axis value for decreased speed
self.myRobot.tankDrive(
self.gamepad.getRawAxis(1) * self.direction * 0.5,
self.gamepad.getRawAxis(5) * self.direction * 0.5)
# If raiseBasketButton is pressed; lower the basket
if self.raiseBasketButton.get() and not self.lowerBasketButton.get():
if not self.basketLimitSwitch.get():
# If basketLimitSwitch is triggered stop the basket
self.basketMotor.set(-1)
else:
self.basketMotor.set(0)
# If lowerBasketButton is pressed; raise the basket
elif self.lowerBasketButton.get() and not self.raiseBasketButton.get():
self.basketMotor.set(1)
else:
self.basketMotor.set(0)
# If toggleHatchButton is pressed and retractHatchButton is not; push the hatch
if self.toggleHatchButton.get() and not self.hatchActed:
self.hatchActed = True
if self.hatchSolenoid.get(
) == wpilib.DoubleSolenoid.Value.kForward:
self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward)
# If pushBasketButton is pressed and retractBasketButton is not; push the basket
if self.toggleBasketButton.get() and not self.basketActed:
self.basketActed = True
if self.basketSolenoid.get(
) == wpilib.DoubleSolenoid.Value.kForward:
self.basketSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.basketSolenoid.set(wpilib.DoubleSolenoid.Value.kForward)
def robotInit(self):
# Robot initialization function
# VictorSPX = Motor Controllers
self.frontLeftMotor = ctre.WPI_VictorSPX(0)
self.rearLeftMotor = ctre.WPI_VictorSPX(1)
self.frontRightMotor = ctre.WPI_VictorSPX(4)
self.rearRightMotor = ctre.WPI_VictorSPX(5)
self.basketMotor = ctre.WPI_VictorSPX(3)
# Digital Inputs (Limit Switch)
self.basketLimitSwitch = wpilib.DigitalInput(0)
# Motor controller groups for each side of the robot
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# Differential drive with left and right motor controller groups
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.direction = -1
# Declare gamepad
self.gamepad = wpilib.Joystick(0)
# Declare buttons
# Controller mapping (1-10): B (East), A (South), Y (North), X (West), Right Bumper, Left Bumper, ?, ?, ?, ?
self.toggleHatchButton = JoystickButton(self.gamepad, 1)
self.toggleBasketButton = JoystickButton(self.gamepad, 2)
self.toggleDirectionButton = JoystickButton(self.gamepad, 3)
self.speedUpButton = JoystickButton(self.gamepad, 4)
self.raiseBasketButton = JoystickButton(self.gamepad, 5)
self.lowerBasketButton = JoystickButton(self.gamepad, 6)
# Determine if already acted on
self.hatchActed = False
self.basketActed = False
self.directionActed = False
# Solenoids
self.hatchSolenoid = wpilib.DoubleSolenoid(0, 1)
self.basketSolenoid = wpilib.DoubleSolenoid(2, 3)
# Compressor
self.compressor = wpilib.Compressor(0)
# Camera Server
wpilib.CameraServer.launch()
def autonomousInit(self):
# Executed at the start of autonomous mode
self.init()
def autonomousPeriodic(self):
# Autonomous Mode(Sandstorm = Identical to TeleOp)
self.periodic()
def teleopInit(self):
# Executed at the start of teleop mode
self.init()
def teleopPeriodic(self):
# TeleOperated mode
self.periodic()
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.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.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.TimedRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.leftVictor = ctre.WPI_VictorSPX(robotmap.omni['left_motor'])
self.rightVictor = ctre.WPI_VictorSPX(robotmap.omni['right_motor'])
self.left = wpilib.SpeedControllerGroup(self.leftVictor)
self.right = wpilib.SpeedControllerGroup(self.rightVictor)
self.centerVictor1 = ctre.WPI_VictorSPX(robotmap.omni['front_strafe'])
self.centerVictor2 = ctre.WPI_VictorSPX(robotmap.omni['back_strafe'])
self.center1 = wpilib.SpeedControllerGroup(self.centerVictor1)
self.center2 = wpilib.SpeedControllerGroup(self.centerVictor2)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.foot = RoboFoot(
wpilib.DoubleSolenoid(PNCANID, RFForward, RFReverse))
self.driver = wpilib.XboxController(0)
self.operator = wpilib.XboxController(1)
def autonomousInit(self):
#self.myRobot.tankDrive(0.8, 0.8)
self.timer = wpilib.Timer()
self.timer.start()
def autonomousPeriodic(self):
# TODO: Add an auton
if self.timer.get() >= 3.0:
self.myRobot.arcadeDrive(0, 0)
else:
self.myRobot.arcadeDrive(-0.9, 0)
wpilib.Timer.delay(0.01)
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 teleopPeriodic(self):
forward = self.driver.getY(LEFT_HAND)
rotation_value = self.driver.getX(RIGHT_HAND)
forward = deadzone(forward, robotmap.deadzone)
rotation_value = deadzone(rotation_value, robotmap.deadzone)
self.myRobot.arcadeDrive(forward, rotation_value)
#center_speed = deadzone(self.driver.getTriggerAxis(LEFT_HAND), robotmap.deadzone)
left_in = 0
right_in = 0
if self.driver.getRawAxis(2) > 0:
left_in = self.driver.getRawAxis(2)
elif self.driver.getRawAxis(2) < 0:
right_in = self.driver.getRawAxis(2)
if self.operator.getAButtonPressed():
self.foot.kick()
if self.operator.getAButtonReleased():
self.foot.unkick()
if self.driver.getRawAxis(2) > 0:
self.center1.set(left_in)
self.center2.set(-left_in)
elif self.driver.getRawAxis(2) < 0:
self.center1.set(-1)
self.center2.set(1)
elif self.driver.getRawAxis(3) != 0:
self.center1.set(-1)
self.center2.set(1)
else:
self.center1.set(0)
self.center2.set(0)
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, fD, fastD, fastV, slowV, error
if firstTime:
sD = self.gyro.getAngle()
fD = sD - 90 # sD起始角度，fD最终角度，cD当前角度
firstTime = False
fastV = 0.78
slowV = 0.64
fastD = 75
error = 6 #写好一系列参数
cD = self.gyro.getAngle()
#在这里，往左转角度会逐渐变小，往右转会变大
if cD > fD - error:
cD = self.gyro.getAngle()
needD = cD - fD #需要选择的角度，会越来越小
if needD >= 90 - fastD: #两档，快档，慢档
speed_turn = fastV
else:
speed_turn = slowV
self.myRobot.tankDrive(-speed_turn, speed_turn)
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):
''' values for navx'''
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)
''' 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)
''' 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)
''' Controller Initialization '''
# 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)
''' Button Status'''
self.buttonStatus = [False, False, False, False, False, False, False]
''' 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()
''' Sensors '''
# Hall Effect Sensor
self.Hall = wpilib.DigitalInput(7)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' 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")
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()
def autonomousPeriodic(self):
''' 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()
def cargoOne():
if self.liftEncoder.get() <= 133: # Cargo 1
self.lift.set(0.5)
elif self.liftEncoder.get() > 133:
self.lift.set(0.05)
self.buttonStatus[4] = False
def cargoTwo():
if self.liftEncoder.get() <= 270: # Cargo 2
self.lift.set(0.5)
elif self.liftEncoder.get() > 270:
self.lift.set(0.05)
self.buttonStatus[2] = False
def cargoThree():
if self.liftEncoder.get() <= 415: # Cargo 3
self.lift.set(0.5)
elif self.liftEncoder.get() > 415:
self.lift.set(0.05)
self.buttonStatus[0] = False
def hatchOne():
if self.liftEncoder.get() <= 96: # Hatch 1
self.lift.set(0.5)
elif self.liftEncoder.get() > 96:
self.lift.set(0.05)
self.buttonStatus[5] = False
def hatchTwo():
if self.liftEncoder.get() <= 237: # Hatch 2
self.lift.set(0.5)
elif self.liftEncoder.get() > 237:
self.lift.set(0.05)
self.buttonStatus[3] = False
def hatchThree():
if self.liftEncoder.get() <= 378: # Hatch 3
self.lift.set(0.5)
elif self.liftEncoder.get() > 378:
self.lift.set(0.05)
self.buttonStatus[1] = False
def liftEncoderReset():
self.lift.set(0.01)
if self.Hall.get() is True:
self.liftEncoder.reset()
''' Button Status Toggle '''
if self.buttonBox.getRawButtonPressed(1):
self.buttonStatus[0] = not self.buttonStatus[0]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonStatus[1] = not self.buttonStatus[1]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonStatus[2] = not self.buttonStatus[2]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonStatus[3] = not self.buttonStatus[3]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonStatus[4] = not self.buttonStatus[4]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonStatus[5] = not self.buttonStatus[5]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonStatus[6] = not self.buttonStatus[6]
''' Button Box Level Mapping '''
if self.buttonStatus[0] is True:
cargoThree()
elif self.buttonStatus[1] is True:
hatchThree()
elif self.buttonStatus[2] is True:
cargoTwo()
elif self.buttonStatus[3] is True:
hatchTwo()
elif self.buttonStatus[4] is True:
cargoOne()
elif self.buttonStatus[5] is True:
hatchOne()
elif self.buttonStatus[6] is True:
liftEncoderReset()
''' Test Execution '''
if self.DS.getGameSpecificMessage() == "pressure":
Pressure()
elif self.DS.getGameSpecificMessage() == "diagnostics":
Diagnostics()
''' 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")
# 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 Speed")
# 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 stuff '''
# 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!")
#self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# 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 Control '''
# compressor
if self.xbox.getRawButton(9):
self.Compressor.stop()
elif self.xbox.getRawButton(10):
self.Compressor.start()
elif self.rightStick.getRawButton(1): # shift right
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.leftStick.getRawButton(1): # shift left
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse)
elif self.xbox.getRawButton(3): # open claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.xbox.getRawButton(2): # close claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse)
elif self.xbox.getRawButton(4): # eject
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.xbox.getRawButton(1): # retract
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse)
''' Victor SPX (Lift, Lift Arm, Cargo) '''
# lift control
if True in self.buttonStatus:
if self.xbox.getRawButton(5): # hold
self.lift.set(0.05)
elif self.xbox.getRawAxis(3): # up
self.lift.set(self.xbox.getRawAxis(3) / 1.5)
elif self.xbox.getRawAxis(2): # down
self.lift.set(-self.xbox.getRawAxis(2) * 0.25)
else:
self.lift.set(0)
# four-bar control
if self.xbox.getRawButton(6):
self.liftArm.set(0.05)
elif not self.xbox.getRawButton(6):
self.liftArm.set(-self.xbox.getRawAxis(1) / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if self.xbox.getRawButton(7):
self.cargo.set(0.12)
elif self.xbox.getRawAxis(5): # take in
self.cargo.set(self.xbox.getRawAxis(5) * 0.75)
# controller mapping for tank steering
rightAxis = self.rightStick.getRawAxis(1)
leftAxis = self.leftStick.getRawAxis(1)
# drives drive system using tank steering
if self.DoubleSolenoidOne.get() == 1: # if on high gear
self.divisor = 1.2 # 90% of high speed
elif self.DoubleSolenoidOne.get() == 2: # if on low gear
self.divisor = 1.2 # normal slow speed
else:
self.divisor = 1.0
if leftAxis != 0:
self.leftSign = leftAxis / fabs(leftAxis)
else:
self.leftSign = 0
if rightAxis != 0:
self.rightSign = rightAxis / fabs(rightAxis)
else:
self.rightSign = 0
self.drive.tankDrive(
-(self.leftSign) * (1 / self.divisor) * (leftAxis**2),
-(self.rightSign) * (1 / self.divisor) * (rightAxis**2))
#self.drive.tankDrive(-leftAxis / self.divisor, -rightAxis/ self.divisor) # drive divided by appropriate divisor
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.sd.putString(" ", "Match Info")
self.sd.putString("Event Name: ", self.DS.getEventName())
self.sd.putNumber("Match Time: ", self.timer.getMatchTime())
self.sd.putNumber("Match Number: ", self.DS.getMatchTime())
self.sd.putNumber("Location: ", self.DS.getLocation())
if self.DS.getMatchType() == 3:
self.sd.putString("Match Type: ", "Elimination")
elif self.DS.getMatchType() == 1:
self.sd.putString("Match Type: ", "Practice")
elif self.DS.getMatchType() == 2:
self.sd.putString("Match Type: ", "Qualification")
else:
self.sd.putString("Match Type: ", "None")
if self.DS.getAlliance() == 0:
self.sd.putString("Alliance: ", "Red")
elif self.DS.getAlliance() == 1:
self.sd.putString("Alliance: ", "Blue")
else:
self.sd.putString("Alliance: ", "Invalid")
'''
def cargoOne():
if self.liftEncoder.get() <= 133: # Cargo 1
self.lift.set(0.5)
elif self.liftEncoder.get() > 133:
self.lift.set(0.05)
self.buttonStatus[4] = False
def cargoTwo():
if self.liftEncoder.get() <= 270: # Cargo 2
self.lift.set(0.5)
elif self.liftEncoder.get() > 270:
self.lift.set(0.05)
self.buttonStatus[2] = False
def cargoThree():
if self.liftEncoder.get() <= 415: # Cargo 3
self.lift.set(0.5)
elif self.liftEncoder.get() > 415:
self.lift.set(0.05)
self.buttonStatus[0] = False
def hatchOne():
if self.liftEncoder.get() <= 96: # Hatch 1
self.lift.set(0.5)
elif self.liftEncoder.get() > 96:
self.lift.set(0.05)
self.buttonStatus[5] = False
def hatchTwo():
if self.liftEncoder.get() <= 237: # Hatch 2
self.lift.set(0.5)
elif self.liftEncoder.get() > 237:
self.lift.set(0.05)
self.buttonStatus[3] = False
def hatchThree():
if self.liftEncoder.get() <= 378: # Hatch 3
self.lift.set(0.5)
elif self.liftEncoder.get() > 378:
self.lift.set(0.05)
self.buttonStatus[1] = False
def liftEncoderReset():
self.lift.set(0.01)
if self.Hall.get() is True:
self.liftEncoder.reset()
''' Button Status Toggle '''
if self.buttonBox.getRawButtonPressed(1):
self.buttonStatus[0] = not self.buttonStatus[0]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonStatus[1] = not self.buttonStatus[1]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonStatus[2] = not self.buttonStatus[2]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonStatus[3] = not self.buttonStatus[3]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonStatus[4] = not self.buttonStatus[4]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonStatus[5] = not self.buttonStatus[5]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonStatus[6] = not self.buttonStatus[6]
''' Button Box Level Mapping '''
if self.buttonStatus[0] is True:
cargoThree()
elif self.buttonStatus[1] is True:
hatchThree()
elif self.buttonStatus[2] is True:
cargoTwo()
elif self.buttonStatus[3] is True:
hatchTwo()
elif self.buttonStatus[4] is True:
cargoOne()
elif self.buttonStatus[5] is True:
hatchOne()
elif self.buttonStatus[6] is True:
liftEncoderReset()
''' 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")
# 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 Speed")
# 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 '''
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!")
#self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# 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")
# # button states
# self.sd.putBoolean("Button 1 (Cargo 3): ", self.buttonStatusOne)
# self.sd.putBoolean("Button 2 (Hatch 3): ", self.buttonStatusTwo)
# self.sd.putBoolean("Button 3 (Cargo 2): ", self.buttonStatusThree)
# self.sd.putBoolean("Button 4 (Hatch 2): ", self.buttonStatusFour)
# self.sd.putBoolean("Button 5 (Cargo 1): ", self.buttonStatusFive)
# self.sd.putBoolean("Button 6 (Hatch 1): ", self.buttonStatusSix)
# self.sd.putBoolean("Button 7 (Reset): ", self.buttonStatusSeven)
''' Pneumatics Control '''
# compressor
if self.xbox.getRawButton(9):
self.Compressor.stop()
elif self.xbox.getRawButton(10):
self.Compressor.start()
elif self.rightStick.getRawButton(1): # shift right
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.leftStick.getRawButton(1): # shift left
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse)
elif self.xbox.getRawButton(3): # open claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.xbox.getRawButton(2): # close claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse)
elif self.xbox.getRawButton(4): # eject
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward)
elif self.xbox.getRawButton(1): # retract
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse)
''' Victor SPX (Lift, Lift Arm, Cargo) '''
# lift control
if True in self.buttonStatus:
if self.xbox.getRawAxis(3): # up
self.lift.set(self.xbox.getRawAxis(3) / 1.5)
elif self.xbox.getRawAxis(2): # down
self.lift.set(-self.xbox.getRawAxis(2) * 0.25)
elif self.xbox.getRawButton(5): # hold
self.lift.set(0.05)
else:
self.lift.set(0)
# four-bar control
if self.xbox.getRawButton(6):
self.liftArm.set(0.05)
elif not self.xbox.getRawButton(6):
self.liftArm.set(-self.xbox.getRawAxis(1) / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if self.xbox.getRawButton(7):
self.cargo.set(0.12)
elif self.xbox.getRawAxis(5): # take in
self.cargo.set(self.xbox.getRawAxis(5) * 0.75)
# controller mapping for tank steering
rightAxis = self.rightStick.getRawAxis(1)
leftAxis = self.leftStick.getRawAxis(1)
# drives drive system using tank steering
if self.DoubleSolenoidOne.get() == 1: # if on high gear
self.divisor = 1.2 # 90% of high speed
elif self.DoubleSolenoidOne.get() == 2: # if on low gear
self.divisor = 1.2 # normal slow speed
else:
self.divisor = 1.0
if leftAxis != 0:
self.leftSign = leftAxis / fabs(leftAxis)
else:
self.leftSign = 0
if rightAxis != 0:
self.rightSign = rightAxis / fabs(rightAxis)
else:
self.rightSign = 0
self.drive.tankDrive(
-(self.leftSign) * (1 / self.divisor) * (leftAxis**2),
-(self.rightSign) * (1 / self.divisor) * (rightAxis**2))
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""Robot initialization function"""
# variables for managing pneumatics sequence
self.State = -1
self.wait_timer = wpilib.Timer()
# object that handles basic drive operations
self.leftMotor = wpilib.Talon(1)
self.rightMotor = wpilib.Talon(2)
self.frontSolExtend = wpilib.Solenoid(1, 1)
self.frontSolRetract = wpilib.Solenoid(1, 0)
self.rearSolExtend = wpilib.Solenoid(1, 3)
self.rearSolRetract = wpilib.Solenoid(1, 2)
self.frontSolExtend.set(False)
self.frontSolRetract.set(True)
self.rearSolExtend.set(False)
self.rearSolRetract.set(True)
self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
self.myRobot.setExpiration(0.1)
# joysticks 1 & 2 on the driver station
self.stick = wpilib.Joystick(0)
def disabled(self):
"""Called when the robot is disabled"""
self.leftMotor.set(0)
self.rightMotor.set(0)
while self.isDisabled():
wpilib.Timer.delay(0.01)
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.stick.getRawAxis(5),
self.stick.getRawAxis(1))
if self.stick.getRawButtonPressed(1): #A
self.State = 0
if self.State == 0:
print("State0")
self.frontSolExtend.set(True)
self.frontSolRetract.set(False)
self.rearSolExtend.set(False)
self.rearSolRetract.set(True)
self.wait_timer.reset()
self.wait_timer.start()
self.State = 1
elif self.State == 1:
if self.wait_timer.get() > 5.0:
self.State = 2
else:
print("State1, Waiting for 5 seconds to elapse")
elif self.State == 2:
self.frontSolExtend.set(False)
self.frontSolRetract.set(True)
self.rearSolExtend.set(True)
self.rearSolRetract.set(False)
self.wait_timer.reset()
self.wait_timer.start()
self.State = 3
elif self.State == 3:
if self.wait_timer.get() > 5.0:
self.State = 4
else:
print("State3, Waiting for 5 seconds to elapse")
elif self.State == 4:
print("State4")
self.frontSolExtend.set(False)
self.frontSolRetract.set(True)
self.rearSolExtend.set(False)
self.rearSolRetract.set(True)
self.State = -1
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(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)
#turn right, although looks like turn left
# 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) # To make sure it's not accelerating to fast
while timer.get() < 3:
self.myRobot.tankDrive(
0, 0
) # This is problematic. Should have used "pass". Otherwise the motor stops immediately and the robot won't reach the distance.
timer.reset()
timer.start()
while timer.get(
) < 0.85: # Turn 90 degrees. Left = 0.82 was the original value we got during tests.
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.myRobot.tankDrive(
0.7, -0.7
) # Turing left, even though it looks like turning right
while timer.get() < 1.4:
self.myRobot.tankDrive(0, 0) # Should have used "pass".
while timer.get() < 1.9:
self.myRobot.tankDrive(0.6, 0.6)
while timer.get() < 2.2:
self.myRobot.tankDrive(0, 0) # Should have used "pass"
while timer.get() < 4.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)
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 # To make it harder to turn when speed is large in order to assure a smooth drive and prevent drifting
if sp >= 0: # May look weird but it's correct.
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'''
# 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
print(time.time())
global firstTime, fD, fastD, fastV, slowV, error
if firstTime:
sD = self.gyro.getAngle()
fD = sD - 90
firstTime = False
fastV = 0.78
slowV = 0.64
fastD = 75
error = 6
cD = self.gyro.getAngle()
# left smaller right bigger
if cD > fD - error:
cD = self.gyro.getAngle()
needD = cD - fD # getting smaller as turing left
if needD >= 90 - fastD:
speed_turn = fastV
else:
speed_turn = slowV
self.myRobot.tankDrive(-speed_turn, speed_turn)
else:
self.myRobot.tankDrive(0,0)
print(time.time())
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():
threshold = 0.4#机器人能开的速度
slow = 0.7#将这个以下的速度精细控制
current_speed = 0
deadband_forward = 0.1#向前的deadband
deadband_steering = 0.1#转弯的deadband
stering_mutiplier = 1.3#越大，转弯越慢
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sum_speed = forward - backward
if abs(sum_speed) > deadband_forward:#如果开的话转弯变慢
stering_mutiplier = 1.5
else:
stering_mutiplier = 1.3
steering = (self.Stick1.getX(0)) / stering_mutiplier#算转弯
if abs(steering) < deadband_steering:#转弯的deadband
steering = 0
if sum_speed >= 0:#算前进方向
direct = 1
elif sum_speed < 0
direct = -1
if ((abs(sum_speed) < slow) and (abs(sum_speed) > deadband_forward)): #多档控制
calculated_speed = sum_speed*((slow - threshold) / slow) + threshold*direct
elif abs(sum_speed) > slow:
calculated_speed = sum_speed
else:
calculated_speed = 0
print("sum " + str(sum_speed))
print("speed " + str(calculated_speed))
self.myRobot.tankDrive(calculated_speed + steering, calculated_speed - steering)
class MyRobot(wpilib.IterativeRobot):
#Motors pwm
Left_Motor_Front = 8
Right_Motor_Front = 7
Left_Motor_Back = 9
Right_Motor_Back = 6
#Motors pwm
Climber_Motor = 5
#Motors can
Elevator_Motor = 1
Left_Front_Intake = 2
Right_Front_Intake = 3
#Joystick
Joystick_Channel = 0
Sec_Joystick_Channel = 1
def robotInit(self):
#Joysticks
self.Joystick = wpilib.Joystick(self.Joystick_Channel)
self.Sec_Joystick = wpilib.Joystick(self.Sec_Joystick_Channel)
#Magnetic Limit Switches
self.switch1 = wpilib.DigitalInput(0)
#Mechanisms
self.Elevator = ctre.WPI_TalonSRX(self.Elevator_Motor)
self.LF_Intake = ctre.WPI_TalonSRX(self.Left_Front_Intake)
self.RF_Intake = ctre.WPI_TalonSRX(self.Right_Front_Intake)
#PWM Mechanisms
self.Climber = wpilib.Talon(self.Climber_Motor)
#Drive Motors
self.LMF = wpilib.Talon(self.Left_Motor_Front)
self.RMF = wpilib.Talon(self.Right_Motor_Front)
self.LMB = wpilib.Talon(self.Left_Motor_Back)
self.RMB = wpilib.Talon(self.Right_Motor_Back)
self.Left = wpilib.SpeedControllerGroup(self.LMF, self.LMB)
self.Right = wpilib.SpeedControllerGroup(self.RMF, self.RMB)
#This is not the built-in robot drive
self.My_Robot = DifferentialDrive(self.Left, self.Right)
self.My_Robot.setExpiration(0.1)
#SmartDashboard
self.spChooser = wpilib.SendableChooser()
self.spChooser.addDefault("Left", 1)
self.spChooser.addObject("Middle", 2)
self.spChooser.addObject("Right", 3)
wpilib.SmartDashboard.putData('StartingPosition', self.spChooser)
self.amChooser = wpilib.SendableChooser()
self.amChooser.addDefault("Scale", 1)
self.amChooser.addObject("Other Lever", 2)
self.amChooser.addObject("Line", 3)
self.amChooser.addObject("None", 0)
wpilib.SmartDashboard.putData('AutoMode', self.amChooser)
self.writeAutoChooser = wpilib.SendableChooser()
self.writeAutoChooser.addDefault("Read Auto", 0)
self.writeAutoChooser.addObject("Write Auto", 1)
wpilib.SmartDashboard.putData('WriteAuto', self.writeAutoChooser)
self.infoChooser = wpilib.SendableChooser()
self.infoChooser.addDefault("0", 0)
for line in customParsing.read():
self.infoChooser.addObject(line, line)
wpilib.SmartDashboard.putData("InfoChooserChannel", self.infoChooser)
def disabledPeriodic(self):
self.Elevator.disable()
self.LF_Intake.disable()
self.RF_Intake.disable()
self.Climber.disable()
def autonomousPeriodic(self):
self.startPos = self.spChooser.getSelected()
self.autoMode = self.amChooser.getSelected()
auto.Autonomous(self, self.startPos, self.autoMode)
def teleopInit(self):
self.My_Robot.setSafetyEnabled(True)
#The function will get called around 50 times a second
def teleopPeriodic(self):
print(self.switch1.get())
if self.writeAutoChooser.getSelected() == 1:
auto.Autonomous.writeCode(self)
else:
drive.drive(self,
self.Joystick.getRawAxis(4), self.Joystick.getY(),
self.Sec_Joystick.getRawButton(1),
self.Sec_Joystick.getRawButton(2),
self.Sec_Joystick.getRawButton(3),
self.Sec_Joystick.getRawButton(4),
self.Sec_Joystick.getRawButton(5),
self.Sec_Joystick.getRawButton(6),
self.Sec_Joystick.getRawButton(7),
self.Sec_Joystick.getRawButton(8))
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 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 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.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
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(wp.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = ctre.WPI_VictorSPX(1) #motor initialization
self.leftMotor2 = ctre.WPI_VictorSPX(3)
self.leftMotor3 = ctre.WPI_VictorSPX(5)
self.rightMotor1 = ctre.WPI_VictorSPX(0)
self.rightMotor2 = ctre.WPI_VictorSPX(2)
self.rightMotor3 = ctre.WPI_VictorSPX(4)
self.armMotor = ctre.WPI_VictorSPX(6)
self.leftIntakeMotor = ctre.WPI_VictorSPX(7)
self.rightIntakeMotor = ctre.WPI_VictorSPX(8)
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.Joystick(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.compressor = wp.Compressor()
wp.SmartDashboard.putNumber("Straight Position", 4000)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Turn Auto", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 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())
calGyro = wp.SmartDashboard.getBoolean("calGyro:", True)
resetGyro = wp.SmartDashboard.getBoolean("resetGyro:", True)
encodeReset = wp.SmartDashboard.getBoolean("resetEnc:", True)
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
def autonomousInit(self):
self.auto = self.chooser
self.leftMotorVal = 0
self.rightMotorVal = 0
self.gyroFuncGain = 40
self.leftMotVal = 0
self.rightMotVal = 0
def autonomousPeriodic(self):
#autonomous code will go here
if (self.auto == 1):
if (self.rightEncd <= ap.moveStraightPos):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, -0.65, self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.myRobot.tankDrive(self.leftMotVal, self.rightMotVal)
def teleopInit(self):
#telop init here
self.myRobot.setSafetyEnabled(True)
self.pastFrontFlipButton = False
self.flip = True
def teleopPeriodic(self):
#teleop code will go here
#Joystick Variables
leftJoyValY = self.leftStick.getY()
rightJoyValY = self.rightStick.getY()
armJoyValY = self.armStick.getY()
frontFlipButton = self.rightStick.getRawButton(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.myRobot.tankDrive(leftMotVal, rightMotVal)
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 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))
