def __init__ (self):
'''
Constructor.
'''
super().__init__()
print("Team 1418 robot code for 2014")
#################################################################
# THIS CODE IS SHARED BETWEEN THE MAIN ROBOT AND THE ELECTRICAL #
# TEST CODE. WHEN CHANGING IT, CHANGE BOTH PLACES! #
#################################################################
wpilib.SmartDashboard.init()
# Joysticks
self.joystick1 = wpilib.Joystick(1)
self.joystick2 = wpilib.Joystick(2)
# Motors
self.lf_motor = wpilib.Jaguar(1)
self.lf_motor.label = 'lf_motor'
self.lr_motor = wpilib.Jaguar(2)
self.lr_motor.label = 'lr_motor'
self.rr_motor = wpilib.Jaguar(3)
self.rr_motor.label = 'rr_motor'
self.rf_motor = wpilib.Jaguar(4)
self.rf_motor.label = 'rf_motor'
self.winch_motor = wpilib.CANJaguar(5)
self.winch_motor.label = 'winch'
self.intake_motor = wpilib.Jaguar(6)
self.intake_motor.label = 'intake'
# Catapult gearbox control
self.gearbox_solenoid=wpilib.DoubleSolenoid(2, 1)
self.gearbox_solenoid.label = 'gearbox'
# Arm up/down control
self.vent_bottom_solenoid = wpilib.Solenoid(3)
self.vent_bottom_solenoid.label = 'vent bottom'
self.fill_bottom_solenoid = wpilib.Solenoid(4)
self.fill_bottom_solenoid.label = 'fill bottom'
self.fill_top_solenoid = wpilib.Solenoid(5)
self.fill_top_solenoid.label = 'fill top'
self.vent_top_solenoid = wpilib.Solenoid(6)
self.vent_top_solenoid.label = 'vent top'
self.pass_solenoid = wpilib.Solenoid(7)
self.pass_solenoid.label = 'pass'
self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor, self.lf_motor, self.rf_motor)
self.robot_drive.SetSafetyEnabled(False)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kFrontLeftMotor, True)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kRearLeftMotor, True)
# Sensors
self.gyro = wpilib.Gyro(1)
self.ultrasonic_sensor = wpilib.AnalogChannel(3)
self.ultrasonic_sensor.label = 'Ultrasonic'
self.arm_angle_sensor = wpilib.AnalogChannel(4)
self.arm_angle_sensor.label = 'Arm angle'
self.ball_sensor = wpilib.AnalogChannel(6)
self.ball_sensor.label = 'Ball sensor'
self.accelerometer = wpilib.ADXL345_I2C(1, wpilib.ADXL345_I2C.kRange_2G)
self.compressor = wpilib.Compressor(1,1)
#################################################################
# END SHARED CODE #
#################################################################
#
# Initialize robot components here
#
self.drive = drive.Drive(self.robot_drive, self.ultrasonic_sensor,self.gyro)
self.initSmartDashboard()
self.pushTimer=wpilib.Timer()
self.catapultTimer=wpilib.Timer()
self.catapult=catapult.Catapult(self.winch_motor,self.gearbox_solenoid,self.pass_solenoid,self.arm_angle_sensor,self.ball_sensor,self.catapultTimer)
self.intakeTimer=wpilib.Timer()
self.intake=intake.Intake(self.vent_top_solenoid,self.fill_top_solenoid,self.fill_bottom_solenoid,self.vent_bottom_solenoid,self.intake_motor,self.intakeTimer)
self.pulldowntoggle=False
self.components = {
'drive': self.drive,
'catapult': self.catapult,
'intake': self.intake
}
self.control_loop_wait_time = 0.025
self.autonomous = AutonomousModeManager(self.components)
def __init__(self):
wpilib.SimpleRobot.__init__(self)
self.ds = wpilib.DriverStation.GetInstance()
self.sd = wpilib.SmartDashboard
# create the component instances
climber = Climber(valve1, valve2)
self.my_drive = Driving(drive)
self.my_feeder = Feeder(feeder_motor,
frisbee_sensor,
feeder_sensor)
auto_angle = AnglePositionJaguar(angle_motor, angle_motor_threshold,
ANGLE_MIN_POSITION, ANGLE_MAX_POSITION,
ANGLE_MIN_ANGLE, ANGLE_MAX_ANGLE)
auto_shooter = SpeedJaguar(shooter_motor, shooter_motor_threshold)
# Bang-bang doesn't work on a Jaguar..
#auto_shooter = BangBangJaguar(shooter_motor, shooter_motor_threshold)
self.my_shooter_platform = ShooterPlatform(auto_angle,
auto_shooter,
climber)
self.my_target_detector = TargetDetector()
# create the system instances
self.my_robot_turner = RobotTurner(self.my_drive)
self.my_auto_targeting = AutoTargeting(self.my_robot_turner, self.my_shooter_platform, self.my_target_detector)
self.my_climber = ClimberSystem(climber, self.my_shooter_platform)
self.my_shooter = Shooter(self.my_shooter_platform, self.my_feeder)
# autonomous mode needs a dict of components
components = {
# components
'drive': self.my_drive,
'feeder': self.my_feeder,
'shooter_platform': self.my_shooter_platform,
'target_detector': self.my_target_detector,
# systems
'auto_targeting': self.my_auto_targeting,
'climber': self.my_climber,
'robot_turner': self.my_robot_turner,
'shooter': self.my_shooter,
}
self.components = []
self.components = [v for v in components.values() if hasattr(v, 'update')]
self.components.append(climber)
self.autonomous_mode = AutonomousModeManager(components)
self.operator_control_mode = OperatorControlManager(components, self.ds)
# initialize other needed SmartDashboard inputs
self.sd.PutBoolean("Wheel On", False)
self.sd.PutBoolean("Auto Feeder", True)
self.sd.PutBoolean("Fire", False)
class MyRobot(wpilib.SimpleRobot):
'''
This is where it all starts
'''
def __init__ (self):
'''
Constructor.
'''
super().__init__()
print("Team 1418 robot code for 2014")
#################################################################
# THIS CODE IS SHARED BETWEEN THE MAIN ROBOT AND THE ELECTRICAL #
# TEST CODE. WHEN CHANGING IT, CHANGE BOTH PLACES! #
#################################################################
wpilib.SmartDashboard.init()
# Joysticks
self.joystick1 = wpilib.Joystick(1)
self.joystick2 = wpilib.Joystick(2)
# Motors
self.lf_motor = wpilib.Jaguar(1)
self.lf_motor.label = 'lf_motor'
self.lr_motor = wpilib.Jaguar(2)
self.lr_motor.label = 'lr_motor'
self.rr_motor = wpilib.Jaguar(3)
self.rr_motor.label = 'rr_motor'
self.rf_motor = wpilib.Jaguar(4)
self.rf_motor.label = 'rf_motor'
self.winch_motor = wpilib.CANJaguar(5)
self.winch_motor.label = 'winch'
self.intake_motor = wpilib.Jaguar(6)
self.intake_motor.label = 'intake'
# Catapult gearbox control
self.gearbox_solenoid=wpilib.DoubleSolenoid(2, 1)
self.gearbox_solenoid.label = 'gearbox'
# Arm up/down control
self.vent_bottom_solenoid = wpilib.Solenoid(3)
self.vent_bottom_solenoid.label = 'vent bottom'
self.fill_bottom_solenoid = wpilib.Solenoid(4)
self.fill_bottom_solenoid.label = 'fill bottom'
self.fill_top_solenoid = wpilib.Solenoid(5)
self.fill_top_solenoid.label = 'fill top'
self.vent_top_solenoid = wpilib.Solenoid(6)
self.vent_top_solenoid.label = 'vent top'
self.pass_solenoid = wpilib.Solenoid(7)
self.pass_solenoid.label = 'pass'
self.robot_drive = wpilib.RobotDrive(self.lr_motor, self.rr_motor, self.lf_motor, self.rf_motor)
self.robot_drive.SetSafetyEnabled(False)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kFrontLeftMotor, True)
self.robot_drive.SetInvertedMotor(wpilib.RobotDrive.kRearLeftMotor, True)
# Sensors
self.gyro = wpilib.Gyro(1)
self.ultrasonic_sensor = wpilib.AnalogChannel(3)
self.ultrasonic_sensor.label = 'Ultrasonic'
self.arm_angle_sensor = wpilib.AnalogChannel(4)
self.arm_angle_sensor.label = 'Arm angle'
self.ball_sensor = wpilib.AnalogChannel(6)
self.ball_sensor.label = 'Ball sensor'
self.accelerometer = wpilib.ADXL345_I2C(1, wpilib.ADXL345_I2C.kRange_2G)
self.compressor = wpilib.Compressor(1,1)
#################################################################
# END SHARED CODE #
#################################################################
#
# Initialize robot components here
#
self.drive = drive.Drive(self.robot_drive, self.ultrasonic_sensor,self.gyro)
self.initSmartDashboard()
self.pushTimer=wpilib.Timer()
self.catapultTimer=wpilib.Timer()
self.catapult=catapult.Catapult(self.winch_motor,self.gearbox_solenoid,self.pass_solenoid,self.arm_angle_sensor,self.ball_sensor,self.catapultTimer)
self.intakeTimer=wpilib.Timer()
self.intake=intake.Intake(self.vent_top_solenoid,self.fill_top_solenoid,self.fill_bottom_solenoid,self.vent_bottom_solenoid,self.intake_motor,self.intakeTimer)
self.pulldowntoggle=False
self.components = {
'drive': self.drive,
'catapult': self.catapult,
'intake': self.intake
}
self.control_loop_wait_time = 0.025
self.autonomous = AutonomousModeManager(self.components)
def Autonomous(self):
'''Called when the robot is in autonomous mode'''
wpilib.SmartDashboard.PutNumber('RobotMode', MODE_AUTONOMOUS)
self.autonomous.run(self, self.control_loop_wait_time)
def Disabled(self):
'''Called when the robot is in disabled mode'''
wpilib.SmartDashboard.PutNumber('RobotMode', MODE_DISABLED)
while self.IsDisabled():
self.communicateWithSmartDashboard(True)
wpilib.Wait(0.01)
def OperatorControl(self):
'''Called when the robot is in Teleoperated mode'''
wpilib.SmartDashboard.PutNumber('RobotMode', MODE_TELEOPERATED)
dog = self.GetWatchdog()
dog.SetExpiration(0.25)
dog.SetEnabled(True)
self.compressor.Start()
preciseDelay = delay.PreciseDelay(self.control_loop_wait_time)
while self.IsOperatorControl()and self.IsEnabled():
self.robotMode=1
dog.Feed()
#
# Driving
#
if self.joystick2.GetZ()==1:
self.drive.move((-1)*self.joystick1.GetX(), self.joystick1.GetY(), self.joystick2.GetX())
else:
self.drive.move(self.joystick1.GetX(), (-1)*self.joystick1.GetY(), self.joystick2.GetX())
# Intake
#
if self.joystick1.GetRawButton(2):
self.intake.armDown()
if self.joystick1.GetRawButton(3):
self.intake.armUp()
if self.joystick1.GetRawButton(5):
self.intake.ballIn()
if self.joystick1.GetRawButton(4):
self.intake.ballOut()
if self.joystick1.GetRawButton(6):
self.drive.angle_rotation(-10)
if self.joystick1.GetRawButton(7):
self.drive.angle_rotation(10)
#
# Catapult
#
if wpilib.SmartDashboard.GetBoolean("AutoWinch"):
self.catapult.autoWinch()
if self.joystick2.GetRawButton(1):
self.catapult.launchNoSensor()
if self.joystick1.GetRawButton(1):
self.catapult.pulldownNoSensor()
#
# Other
#
self.communicateWithSmartDashboard(False)
self.update()
preciseDelay.wait()
# Disable the watchdog at the end
dog.SetEnabled(False)
# only run the compressor in teleoperated mode
self.compressor.Stop()
def update(self):
'''This function calls all of the doit functions for each component'''
for component in self.components.values():
component.doit()
def initSmartDashboard(self):
self.sdTimer = wpilib.Timer()
self.sdTimer.Start()
wpilib.SmartDashboard.PutBoolean("AutoWinch", False)
wpilib.SmartDashboard.PutBoolean("EnableTuning", False)
wpilib.SmartDashboard.PutNumber("FirePower", 100)
wpilib.SmartDashboard.PutNumber("ArmSet", 0)
wpilib.SmartDashboard.PutBoolean("Fire", False)
wpilib.SmartDashboard.PutBoolean("GyroEnabled", True)
wpilib.SmartDashboard.PutNumber("GyroAngle",self.gyro.GetAngle())
wpilib.SmartDashboard.PutNumber("Compressor", self.compressor.GetPressureSwitchValue())
wpilib.SmartDashboard.PutNumber("AngleConstant", self.drive.angle_constant)
print (self.compressor.GetPressureSwitchValue())
def communicateWithSmartDashboard(self, in_disabled):
'''Sends and recieves values to/from the SmartDashboard'''
# only send values every once in awhile
if self.sdTimer.HasPeriodPassed(0.1):
# Send the distance to the driver station
wpilib.SmartDashboard.PutNumber("Distance",self.ultrasonic_sensor.GetVoltage())
wpilib.SmartDashboard.PutNumber("GyroAngle",self.gyro.GetAngle())
# Battery can actually be done dashboard side, fix that self (Shayne)
# Put the arm state
wpilib.SmartDashboard.PutNumber("ArmState",self.intake.GetMode())
# Get if a ball is loaded
wpilib.SmartDashboard.PutBoolean("BallLoaded", self.catapult.check_ready())
wpilib.SmartDashboard.PutNumber("ShootAngle",self.catapult.getCatapultLocation())
wpilib.SmartDashboard.PutNumber("Compressor", self.compressor.GetPressureSwitchValue())
# don't remove this, this allows us to disable the gyro
self.drive.set_gyro_enabled(wpilib.SmartDashboard.GetBoolean('GyroEnabled'))
# don't set any of the other variables in disabled mode!
if in_disabled:
return
# Get the number to set the winch power
#self.WinchPowerVar = wpilib.SmartDashboard.PutNumber("FirePower",1)
# TODO: Cleanup catapult.py and finish this
self.drive.set_angle_constant(wpilib.SmartDashboard.GetNumber('AngleConstant'))
# If its 0 then update the arm state
arm_state = wpilib.SmartDashboard.GetNumber("ArmSet")
if arm_state != 0:
self.intake.SetMode(arm_state)
wpilib.SmartDashboard.PutNumber("ArmSet", 0)
# 0 it to avoid locking the driver out of arm controls
if wpilib.SmartDashboard.GetBoolean("Fire"):
self.catapult.launchNoSensor()
wpilib.SmartDashboard.PutBoolean("Fire", False)
self.catapult.setWinchLocation(wpilib.SmartDashboard.GetNumber('FirePower'))
class MyRobot(wpilib.SimpleRobot):
# import into this namespace
ANGLE_MIN_ANGLE = ANGLE_MIN_ANGLE
ANGLE_MAX_ANGLE = ANGLE_MAX_ANGLE
# keep in sync with driver station
MODE_DISABLED = 1
MODE_AUTONOMOUS = 2
MODE_TELEOPERATED = 3
def __init__(self):
wpilib.SimpleRobot.__init__(self)
self.ds = wpilib.DriverStation.GetInstance()
self.sd = wpilib.SmartDashboard
# create the component instances
climber = Climber(valve1, valve2)
self.my_drive = Driving(drive)
self.my_feeder = Feeder(feeder_motor,
frisbee_sensor,
feeder_sensor)
auto_angle = AnglePositionJaguar(angle_motor, angle_motor_threshold,
ANGLE_MIN_POSITION, ANGLE_MAX_POSITION,
ANGLE_MIN_ANGLE, ANGLE_MAX_ANGLE)
auto_shooter = SpeedJaguar(shooter_motor, shooter_motor_threshold)
# Bang-bang doesn't work on a Jaguar..
#auto_shooter = BangBangJaguar(shooter_motor, shooter_motor_threshold)
self.my_shooter_platform = ShooterPlatform(auto_angle,
auto_shooter,
climber)
self.my_target_detector = TargetDetector()
# create the system instances
self.my_robot_turner = RobotTurner(self.my_drive)
self.my_auto_targeting = AutoTargeting(self.my_robot_turner, self.my_shooter_platform, self.my_target_detector)
self.my_climber = ClimberSystem(climber, self.my_shooter_platform)
self.my_shooter = Shooter(self.my_shooter_platform, self.my_feeder)
# autonomous mode needs a dict of components
components = {
# components
'drive': self.my_drive,
'feeder': self.my_feeder,
'shooter_platform': self.my_shooter_platform,
'target_detector': self.my_target_detector,
# systems
'auto_targeting': self.my_auto_targeting,
'climber': self.my_climber,
'robot_turner': self.my_robot_turner,
'shooter': self.my_shooter,
}
self.components = []
self.components = [v for v in components.values() if hasattr(v, 'update')]
self.components.append(climber)
self.autonomous_mode = AutonomousModeManager(components)
self.operator_control_mode = OperatorControlManager(components, self.ds)
# initialize other needed SmartDashboard inputs
self.sd.PutBoolean("Wheel On", False)
self.sd.PutBoolean("Auto Feeder", True)
self.sd.PutBoolean("Fire", False)
def RobotInit(self):
pass
def Disabled(self):
print("MyRobot::Disabled()")
self.sd.PutNumber("Robot Mode", self.MODE_DISABLED)
while self.IsDisabled():
wpilib.Wait(control_loop_wait_time)
def Autonomous(self):
print("MyRobot::Autonomous()")
self.sd.PutNumber("Robot Mode", self.MODE_AUTONOMOUS)
# put this in a consistent state when starting the robot
self.my_climber.lower()
# this does all the autonomous mode work for us
self.autonomous_mode.run(self, control_loop_wait_time)
def OperatorControl(self):
print("MyRobot::OperatorControl()")
self.sd.PutNumber("Robot Mode", self.MODE_TELEOPERATED)
if self.my_climber.position() is None:
self.my_climber.lower()
# set the watch dog
dog = self.GetWatchdog()
dog.SetEnabled(False)
dog.SetExpiration(0.25)
compressor.Start()
# All operator control functions are now in OperatorControlManager
self.operator_control_mode.run(self, control_loop_wait_time)
#
# Done with operator mode, finish up
#
compressor.Stop()
#
# Other
#
def update(self):
'''Runs update on all components'''
for component in self.components:
component.update()