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)