def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
# Init autonomous
self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS,
self.C.gyroS, self.metabox,
self.driverStation)
# reset state
self.autonomousRoutine.state = 0
def EventHandler(events, ser, car, joystick, hadEvent, forward, reverse, left,
right, quit, yAxis, yAxisInverted, xAxis, xAxisInverted,
interval):
# Handle each event individually
for event in events:
if event.type == pygame.QUIT:
# User exit
hadEvent = True
quit = True
elif event.type == pygame.KEYDOWN:
# A key has been pressed, see if it is one we want
hadEvent = True
if event.key == pygame.K_ESCAPE:
quit = True
elif event.type == pygame.KEYUP:
# A key has been released, see if it is one we want
hadEvent = True
if event.key == pygame.K_ESCAPE:
quit = False
elif event.type == pygame.JOYBUTTONDOWN:
car.setSpeed(90)
car.setTurn(90)
if joystick.get_button(16) == 1:
quit = True
elif joystick.get_button(12) == 1:
Autonomous(ser, car, joystick)
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved, read axis positions (-1 to +1)
hadEvent = True
upDown = joystick.get_axis(yAxis)
leftRight = joystick.get_axis(xAxis)
# Invert any axes which are incorrect
if yAxisInverted:
upDown = -upDown
if xAxisInverted:
leftRight = -leftRight
# Determine Up / Down values
if upDown < -0.1:
forward = True
reverse = False
elif upDown > 0.1:
forward = False
reverse = True
else:
forward = False
reverse = False
# Determine Left / Right values
if leftRight < -0.1:
left = True
right = False
elif leftRight > 0.1:
left = False
right = True
else:
left = False
right = False
return hadEvent, forward, reverse, left, right, quit
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick)
self.intake = Intake(self.intake_wheels_motor, self.intake_arm_motor,
self.arm_stick)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick)
self.shooter = Shooter(self.shooter_motor, self.encoder,
self.shooter_servo, self.arm_stick,
self.shooter_service)
self.autonomous = Autonomous(self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor)
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(
self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick
)
self.intake = Intake(self.intake_wheels_motor,
self.intake_arm_motor,
self.arm_stick
)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick
)
self.shooter = Shooter(self.shooter_motor,
self.encoder,
self.shooter_servo,
self.arm_stick,
self.shooter_service
)
self.autonomous = Autonomous(
self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor
)
def main():
#initialize the connection to the arduino
ser = ArduinoConnect()
print 'To control the car manually, please press "m" on the keyboard'
print 'To control the car with the Remote Controller, then power on the device'
print 'To control the car with the PS3 Controller, \nplease power on and press the "PS" button\n'
#Check if there is a PS3 Controller, assume there is not one.
car = Car('James')
joystick = BluetoothConnect(ser)
try:
val = Autonomous(ser, car, joystick)
if val == False:
while ser.isOpen():
ManualInput(ser, car, joystick)
else:
while ser.isOpen():
PS3Controller(ser, car, joystick)
except KeyboardInterrupt:
print('Switching to manual input...')
ManualInput(ser, car, joystick)
def autonomousInit(self):
self.drivetrain.load_config_values()
self.auto = Autonomous(self, self.autoPositionSelect.getSelected())
self.auto.periodic()
class Robot(wpilib.IterativeRobot):
def robotInit(self):
constants.load_control_config()
wpilib.CameraServer.launch('driver_vision.py:main')
self.autoPositionSelect = wpilib.SendableChooser()
self.autoPositionSelect.addDefault('Middle', 'Middle')
self.autoPositionSelect.addObject('Left', 'Left')
self.autoPositionSelect.addObject('Right', 'Right')
wpilib.SmartDashboard.putData('Robot Starting Position',
self.autoPositionSelect)
self.control_stick = wpilib.Joystick(0)
self.drivetrain = swerve.SwerveDrive(constants.chassis_length,
constants.chassis_width,
constants.swerve_config)
self.lift = lift.RD4BLift(constants.lift_ids['left'],
constants.lift_ids['right'])
self.winch = winch.Winch(constants.winch_id)
# self.claw = lift.Claw(
# constants.claw_id
# )
self.imu = IMU(wpilib.SPI.Port.kMXP)
def disabledInit(self):
# We don't really _need_ to reload configuration in
# every init call-- it's just useful for debugging.
# (no need to restart robot code just to load new values)
self.drivetrain.load_config_values()
def disabledPeriodic(self):
self.drivetrain.update_smart_dashboard()
self.imu.update_smart_dashboard()
def autonomousInit(self):
self.drivetrain.load_config_values()
self.auto = Autonomous(self, self.autoPositionSelect.getSelected())
self.auto.periodic()
def autonomousPeriodic(self):
self.auto.update_smart_dashboard()
self.imu.update_smart_dashboard()
self.drivetrain.update_smart_dashboard()
self.auto.periodic()
def teleopInit(self):
self.teleop = Teleop(self, self.control_stick)
self.drivetrain.load_config_values()
constants.load_control_config()
def teleopPeriodic(self):
# For now: basic driving
constants.load_control_config()
self.teleop.drive()
self.teleop.buttons()
self.teleop.lift_control()
self.drivetrain.update_smart_dashboard()
self.teleop.update_smart_dashboard()
self.imu.update_smart_dashboard()
class Aimbot(wpilib.SimpleRobot):
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(
self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick
)
self.intake = Intake(self.intake_wheels_motor,
self.intake_arm_motor,
self.arm_stick
)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick
)
self.shooter = Shooter(self.shooter_motor,
self.encoder,
self.shooter_servo,
self.arm_stick,
self.shooter_service
)
self.autonomous = Autonomous(
self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor
)
def Autonomous(self):
self.GetWatchdog().SetEnabled(False)
self.autonomous.reset()
while self.IsAutonomous() and self.IsEnabled():
self.autonomous.iterate()
wpilib.Wait(0.01)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
self.mecanum_drive.iterate()
self.intake.iterate()
self.shooter.iterate()
self.shooter_service.iterate()
wpilib.Wait(0.04)
class Randy(wpilib.TimedRobot):
def robotInit(self):
self.C = Component(
) # Components inits all connected motors, sensors, and joysticks. See inits.py.
# Setup subsystems
self.driverStation = wpilib.DriverStation.getInstance()
self.drive = Chassis(self.C.driveTrain, self.C.gyroS,
self.C.driveYEncoderS)
self.lights = Lights()
self.metabox = MetaBox(self.C.elevatorEncoderS, self.C.elevatorLimitS,
self.C.jawsLimitS, self.C.metaboxLimitS,
self.C.jawsM, self.C.elevatorM, self.C.intakeM,
self.C.jawsSol)
self.winch = Winch(self.C.winchM)
self.power = Power()
# Joysticks
self.joystick = wpilib.XboxController(0)
self.leftJ = wpilib.Joystick(1)
# default to rainbow effect
self.lights.run({'effect': 'rainbow'})
self.sd = NetworkTables.getTable('SmartDashboard')
self.sd.putNumber('station', 2)
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
'''Components'''
# Rumble
averageDriveCurrent = self.power.getAverageCurrent([0, 1, 14, 15])
if (averageDriveCurrent > 8):
self.joystick.setRumble(0, 1)
else:
self.joystick.setRumble(0, 0)
print(self.metabox.getEncoder())
'''
TODO: calibrate sparks
'''
# Drive
self.drive.run(self.joystick.getRawAxis(0),
self.joystick.getRawAxis(1),
self.joystick.getRawAxis(4))
# MetaBox
self.metabox.run(
self.leftJ.getY(), # elevator rate of change
self.leftJ.getRawButton(1), # run intake wheels in
self.leftJ.getRawButton(3), # open jaws
self.leftJ.getRawButton(2), # run intake wheels out
self.leftJ.getRawButton(4), # go to bottom
self.leftJ.getRawAxis(2), # set angle of jaws
self.leftJ.getRawButton(8)) # calibrate elevator
# Lights
self.lights.setColor(self.driverStation.getAlliance())
if (self.driverStation.getMatchTime() < 30
and self.driverStation.getMatchTime() != -1):
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 200})
elif (helpers.deadband(self.leftJ.getY(), 0.1) != 0):
self.lights.run({'effect': 'stagger'})
elif (self.leftJ.getRawButton(1) or self.leftJ.getRawButton(2)):
self.lights.run({'effect': 'flash', 'fade': False, 'speed': 20})
else:
self.lights.run({'effect': 'rainbow'})
def teleopInit(self):
"""This function is run once each time the robot enters teleop mode."""
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.lights.run({'effect': 'flash', 'fade': True, 'speed': 400})
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
# Init autonomous
self.autonomousRoutine = Autonomous(self.drive, self.C.driveYEncoderS,
self.C.gyroS, self.metabox,
self.driverStation)
# reset state
self.autonomousRoutine.state = 0
def autonomousPeriodic(self):
self.autonomousRoutine.run() # see autonomous.py
def test(self):
# reset gyro
self.C.gyroS.reset()
# reset encoder
self.C.driveYEncoderS.reset()
def run(self, carID):
"""
Executes the Traci client and the environment representation
"""
ego = carID
step = 0
while traci.simulation.getTime() < 60:
traci.simulationStep()
fw = Framework()
if carID in traci.vehicle.getIDList():
ego_trajectory = fw.getTrajectory(carID)
intersections = fw.getIntersectingTrajectories(
carID, ego_trajectory)
if intersections is not None:
#if len(ego_trajectory) > 1:
framework = fw.getTensorFramework(carID, intersections,
ego_trajectory)
auto = Autonomous(carID, framework, ego_trajectory)
distances = auto.getDistanceToIntercept()
tfc = Traffic(distances)
vehicles = tfc.getConflictoryTraffic()
distances_tfc = tfc.getDistanceToIntercept(vehicles)
safe = Safety(carID, framework, distances, distances_tfc)
trc_safety = safe.getTrafficSafetyMeasures()
#print(len(distances), len(trc_safety))
ego_safety = safe.getEgoSafetyMeasures()
#print(ego_safety)
prio = safe.getPriotizedTraffic(ego_safety, trc_safety)
rel = safe.getRelevantTraffic(prio)
#print(rel)
# for i in range(len(rel)):
# if rel[i][1]:
# safe.getEgoSecurityDistance(carID, rel[i][1][0][0])
env = Tensor(carID, distances, rel, ego_safety,
ego_trajectory)
env.createEnvironmentTensor()
#print(env.createEnvironmentTensor())
else:
corridor = np.full((200, 3), fill_value=-1.0)
corridor[0:200, 2] = 100 / np.maximum(
traci.vehicle.getSpeed(self.carID), 0.001)
#print(corridor)
step += 1
traci.close()
sys.stdout.flush()
def getTensor(self):
fw = Framework()
if self.carID in traci.vehicle.getIDList():
ego_trajectory = fw.getTrajectory(self.carID)
intersections = fw.getIntersectingTrajectories(
self.carID, ego_trajectory)
if intersections is not None:
#if len(ego_trajectory) > 1:
framework = fw.getTensorFramework(self.carID, intersections,
ego_trajectory)
auto = Autonomous(self.carID, framework, ego_trajectory)
distances = auto.getDistanceToIntercept()
tfc = Traffic(distances)
vehicles = tfc.getConflictoryTraffic()
distances_tfc = tfc.getDistanceToIntercept(vehicles)
self.distances_tfc = distances_tfc
safe = Safety(self.carID, framework, distances, distances_tfc)
trc_safety = safe.getTrafficSafetyMeasures()
# print(len(distances), len(trc_safety))
ego_safety = safe.getEgoSafetyMeasures()
# print(ego_safety)
prio = safe.getPriotizedTraffic(ego_safety, trc_safety)
rel = safe.getRelevantTraffic(prio)
#print('\n', prio)
#print(rel)
self.rel = rel
# print(rel)
# for i in range(len(rel)):
# if rel[i][1]:
# safe.getEgoSecurityDistance(carID, rel[i][1][0][0])
env = Tensor(self.carID, distances, rel, ego_safety,
ego_trajectory)
#print(env.createEnvironmentTensor())
return env.createEnvironmentTensor()
else:
corridor = np.full((200, 3), fill_value=-1.0)
#corridor[0:199, 2] = 0
corridor[0:200, 2] = 100 / np.maximum(
traci.vehicle.getSpeed(self.carID), 0.001)
#tensor = np.reshape(corridor[0:200], 600)
return corridor
class Aimbot(wpilib.SimpleRobot):
def __init__(self):
super().__init__()
self.drive_stick = wpilib.Joystick(1)
self.arm_stick = wpilib.Joystick(2)
self.front_right_motor = wpilib.Jaguar(2)
self.front_left_motor = wpilib.Jaguar(1)
self.back_left_motor = wpilib.Jaguar(3)
self.back_right_motor = wpilib.Jaguar(4)
self.intake_wheels_motor = wpilib.Jaguar(10)
self.intake_arm_motor = wpilib.Jaguar(6)
self.shooter_servo = wpilib.Servo(7)
self.shooter_motor = wpilib.Jaguar(5)
self.encoder = wpilib.Encoder(1, 2, True)
self.mecanum_drive = MecanumDrive(self.front_right_motor,
self.front_left_motor,
self.back_right_motor,
self.back_left_motor,
self.drive_stick)
self.intake = Intake(self.intake_wheels_motor, self.intake_arm_motor,
self.arm_stick)
self.shooter_service = ShooterService(self.shooter_motor,
self.shooter_servo,
self.arm_stick)
self.shooter = Shooter(self.shooter_motor, self.encoder,
self.shooter_servo, self.arm_stick,
self.shooter_service)
self.autonomous = Autonomous(self.shooter_service,
self.intake_arm_motor,
self.front_left_motor,
self.front_right_motor,
self.back_left_motor,
self.back_right_motor)
def Autonomous(self):
self.GetWatchdog().SetEnabled(False)
self.autonomous.reset()
while self.IsAutonomous() and self.IsEnabled():
self.autonomous.iterate()
wpilib.Wait(0.01)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
self.mecanum_drive.iterate()
self.intake.iterate()
self.shooter.iterate()
self.shooter_service.iterate()
wpilib.Wait(0.04)