class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.C = Component() # Components inits all connected motors, sensors, and joysticks. See components.py.
# Setup subsystems
self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
self.belt = Belt(self.C.beltM)
self.beltAxis = BeltAxis(self.C.beltAxisM)
self.shoot = Shooter(self.C.flipM, self.C.shootM)
self.lift = Lift(self.C.lift1M, self.C.lift2M)
self.sonic = Sonic(self.C.sonic)
self.arduino = Arduino(self.C.arduino)
self.guide = Guiding(self.sonic, self.drive)
self.sd = SmartDashboard
self.sd.putBoolean("autoMove", False)
self.autoLoop = 0
def operatorControl(self):
self.C.driveTrain.setSafetyEnabled(True) # keeps robot from going crazy if connection with DS is lost
while self.isOperatorControl() and self.isEnabled():
distance = self.sonic.getAverage()
print(distance)
self.sd.putDouble('distance', distance)
if (5.25 < distance and distance < 5.75):
self.sd.putBoolean("Ready", True)
else:
self.sd.putBoolean("Ready", False)
if (self.C.middleJ.getRawButton(1) == True):
self.guide.guideSonic()
# Drive
self.drive.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
# Components
self.belt.run(self.C.rightJ.getRawButton(4), self.C.rightJ.getRawButton(5))
self.beltAxis.run(self.C.rightJ.getRawButton(3), self.C.rightJ.getY(), self.C.beltAxisTS.get(), self.C.beltAxisBS.get())
self.lift.run(self.C.leftJ.getRawButton(1), self.C.leftJ.getY(), self.C.middleJ.getY())
self.shoot.run(self.C.rightJ.getRawButton(2), self.C.beltAxisTS.get(), self.C.rightJ.getRawButton(1), self.C.flipS.get()) # weird argument in the middle makes sure shooter doesn't turn on when belt it up
wpilib.Timer.delay(0.005) # wait for a motor update time
def autonomous(self):
"""This function is called periodically during autonomous."""
if (self.sd.getBoolean("autoMove") == True):
distance = self.sonic.getAverage()
while (self.autoLoop < 2000 and distance > 5):
self.drive.set(-1, 0)
wpilib.Timer.delay(0.005)
self.autoLoop = self.autoLoop + 1
def test(self):
"""This function is called periodically during test mode."""
def test_control_methods():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.shooter_motor.set = MagicMock()
shooter.shoot()
shooter.shooter_motor.set.assert_called_with(-Shooter.max_speed*Shooter.shoot_percentage)
shooter.backdrive()
shooter.shooter_motor.set.assert_called_with(Shooter.max_speed*0.01)
shooter.backdrive_recovery()
shooter.shooter_motor.set.assert_called_with(Shooter.max_speed*1.0)
shooter.stop()
shooter.shooter_motor.set.assert_called_with(0.0)
def test_up_to_speed():
shooter = Shooter()
shooter.shoter_motor = MagicMock()
# test error condition
shooter.shooter_motor.get = MagicMock(
return_value=(Shooter.max_speed * Shooter.shoot_percentage / 2.0) +
1.0)
shooter.shooter_motor.getSetpoint = MagicMock(
return_value=Shooter.max_speed * Shooter.shoot_percentage)
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.03 *
Shooter.max_speed)
assert not shooter.up_to_speed()
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.02 *
Shooter.max_speed)
assert shooter.up_to_speed()
# test that we are at least half of setpoint
shooter.shooter_motor.getSetpoint = MagicMock(
return_value=Shooter.max_speed * Shooter.shoot_percentage)
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.02 *
Shooter.max_speed)
shooter.shooter_motor.get = MagicMock(
return_value=(Shooter.max_speed * Shooter.shoot_percentage / 2.0))
assert not shooter.up_to_speed()
shooter.shooter_motor.get = MagicMock(
return_value=(Shooter.max_speed * Shooter.shoot_percentage / 2.0) +
1.0)
assert shooter.up_to_speed()
def test_stop():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter._speed = 12345
shooter.stop()
shooter.execute()
assert shooter._speed == 0.0
assert shooter.shooter_motor.set.called
def test_set_speed():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.start_shoot()
shooter.execute()
assert shooter._speed != 0.0
shooter.shooter_motor.setFeedbackDevice.assert_called_once_with(CANTalon.FeedbackDevice.QuadEncoder)
assert shooter.shooter_motor.set.called
def test_changed_speed():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.start_shoot()
shooter.execute()
shooter.execute()
# Setpoint for Talon SRX should only get called once
assert shooter.shooter_motor.set.call_count == 1
def __init__(self):
wpilib.SimpleRobot.__init__(self)
#create component instances
'''ex self.my_feeder = Feeder(feeder_motor,
frisbee_sensor,
feeder_sensor)'''
self.feeder = Feeder(feeder_servo)
self.angle = Angle(angle_control)
self.shooter = Shooter(shooter_wheel)
self.robot_drive = wpilib.RobotDrive(l_drive, r_drive)
def robotInit(self):
self.C = Component() # Components inits all connected motors, sensors, and joysticks. See components.py.
# Setup subsystems
self.drive = Chassis(self.C.driveTrain, self.C.gyroS)
self.belt = Belt(self.C.beltM)
self.beltAxis = BeltAxis(self.C.beltAxisM)
self.shoot = Shooter(self.C.flipM, self.C.shootM)
self.lift = Lift(self.C.lift1M, self.C.lift2M)
self.sonic = Sonic(self.C.sonic)
self.arduino = Arduino(self.C.arduino)
self.guide = Guiding(self.sonic, self.drive)
self.sd = SmartDashboard
self.sd.putBoolean("autoMove", False)
self.autoLoop = 0
def test_up_to_speed():
shooter = Shooter()
shooter.shoter_motor = MagicMock()
# test error condition
shooter.shooter_motor.get = MagicMock(return_value=(Shooter.max_speed*Shooter.shoot_percentage/2.0)+1.0)
shooter.shooter_motor.getSetpoint = MagicMock(return_value=Shooter.max_speed*Shooter.shoot_percentage)
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.03*Shooter.max_speed)
assert not shooter.up_to_speed()
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.02*Shooter.max_speed)
assert shooter.up_to_speed()
# test that we are at least half of setpoint
shooter.shooter_motor.getSetpoint = MagicMock(return_value=Shooter.max_speed*Shooter.shoot_percentage)
shooter.shooter_motor.getClosedLoopError = MagicMock(return_value=0.02*Shooter.max_speed)
shooter.shooter_motor.get = MagicMock(return_value=(Shooter.max_speed*Shooter.shoot_percentage/2.0))
assert not shooter.up_to_speed()
shooter.shooter_motor.get = MagicMock(return_value=(Shooter.max_speed*Shooter.shoot_percentage/2.0)+1.0)
assert shooter.up_to_speed()
class MyRobot (wpilib.SimpleRobot):
def __init__(self):
wpilib.SimpleRobot.__init__(self)
#create component instances
'''ex self.my_feeder = Feeder(feeder_motor,
frisbee_sensor,
feeder_sensor)'''
self.feeder = Feeder(feeder_servo)
self.angle = Angle(angle_control)
self.shooter = Shooter(shooter_wheel)
self.robot_drive = wpilib.RobotDrive(l_drive, r_drive)
#In the 2013 code we had a dictionary of autonomous mode components. I'm assuming we are keeping that.
#initialize other needed SmartDashboard imputs if we use it. This would be real competition stuff.
def RobotInit(self):
pass
def Disabled(self):
print("MyRobot::Autonomous()")
while self.IsDisabled():
wpilib.Wait(CONTROL_LOOP_WAIT_TIME)
def Autonomous(self):
print("MyRobot::Autonomous()")
while self.IsOperatorControl() and self.IsEnabled():
wpilib.Wait(CONTROL_LOOP_WAIT_TIME)
#self.sd.PutNumber("Robot Mode", self.MODE_AUTONOMOUS)
def OperatorControl(self):
print("MyRobot::OperatorControl()")
self.delay = PreciseDelay(CONTROL_LOOP_WAIT_TIME)
# set the watch dog
dog = self.GetWatchdog()
dog.SetExpiration(0.25)
dog.SetEnabled(True)
while self.IsOperatorControl () and self.IsEnabled():
#
#angle
#
if joystick.GetRawButton(3):
self.angle.set_speed(ANGLE_CONTROLLER_SPEED)
elif joystick.GetRawButton(2):
self.angle.set_speed(-ANGLE_CONTROLLER_SPEED)
#
#shooter
#
# gets the value of the throttle treating -1 as 0
self.d_speed = (joystick.GetThrottle() + 1) / 2
self.shooter.set_speed(self.d_speed)
#
#feeder
#
if joystick.GetTrigger():
self.feeder.feed()
#
#update everything
#
self.angle.update()
self.feeder.update()
self.shooter.update()
#
#Set the drive
#
self.robot_drive.ArcadeDrive(joystick.GetY(), joystick.GetX(), True)
dog.Feed()
self.delay.wait()
def test_control_methods():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.shooter_motor.set = MagicMock()
shooter.shoot()
shooter.shooter_motor.set.assert_called_with(-Shooter.max_speed *
Shooter.shoot_percentage)
shooter.backdrive()
shooter.shooter_motor.set.assert_called_with(Shooter.max_speed * 0.01)
shooter.backdrive_recovery()
shooter.shooter_motor.set.assert_called_with(Shooter.max_speed * 1.0)
shooter.stop()
shooter.shooter_motor.set.assert_called_with(0.0)
def test_backdrive():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.state = States.backdriving
shooter.execute()
assert shooter._speed >= 0.0
def test_toggle():
shooter = Shooter()
shooter.shooter_motor = MagicMock()
shooter.state = States.off
shooter.changed_state = False
shooter.toggle()
shooter.execute()
shooter.execute()
assert shooter._speed != 0.0
shooter.state = States.shooting
shooter.changed_state = False
shooter.execute()
shooter.toggle()
shooter.execute()
shooter.execute()
assert shooter._speed == 0.0
assert shooter.shooter_motor.set.call_count == 2
