def __init__(self, iterations, leader: Car, headway: float,
max_noise: float, delay: int):
super().__init__(iterations, headway)
self.pid.set_tuning(40.5, 0, 0.1, 1 * MILLISECOND)
self.leader = leader
self.sensor = Sensor(delay, max_noise)
class FollowerControlSensor(FollowerControlData):
def __init__(self, iterations: int, connection: Connection, headway: float, leader, noise, sensor_delay):
super().__init__(iterations, connection, headway)
self.connection = connection
self.sensor = Sensor(sensor_delay, noise)
self.leader = leader
def tick(self, frame: int, time_passed: float):
packets = self.connection.read()
for packet in packets:
if isinstance(packet, PacketTodo):
if packet.timestamp < frame:
self.pid.setpoint = packet.v
else:
self.todo[packet.timestamp] = packet
if self.todo[frame]:
self.pid.setpoint = self.todo[frame].v
self.last_todo = frame
self.last_packet = self.sensor.get_info(self.leader, frame)
if self.last_packet is not None:
p = self.last_packet
self.pid2.setpoint = calc_ref_v(self.headway, self.x[frame - 1], p.a, p.v, p.x,
(frame - p.timestamp - 1) * MILLISECOND)
super(FollowerControlData, self).tick(frame, time_passed)
def drive(self, frame: int, time_passed: float):
self.pid.compute(self.v[frame - 1])
self.pid2.compute(self.v[frame - 1])
sum = self.motor.get_acceleration(self.v[frame - 1], self.pid.output)
sum += self.motor.get_acceleration(self.v[frame - 1], self.pid2.output)
if sum > 4:
sum = 4
elif sum < -8:
sum = -8
self.a[frame] = sum
def get_name(self):
return "Future + Sensor"
class FollowerSensor(CarPID):
def __init__(self, iterations, leader: Car, headway: float,
max_noise: float, delay: int):
super().__init__(iterations, headway)
self.pid.set_tuning(40.5, 0, 0.1, 1 * MILLISECOND)
self.leader = leader
self.sensor = Sensor(delay, max_noise)
def tick(self, frame: int, time_passed: float):
last_info = self.sensor.get_info(self.leader, frame)
if last_info:
self.pid.setpoint = calc_ref_v(
self.headway, self.x[frame - 1], last_info.a, last_info.v,
last_info.x, (frame - last_info.timestamp - 1) * MILLISECOND)
super().tick(frame, time_passed)
def get_name(self):
return "Sensor"
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# hello
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.sweeper_relay = wpilib.Relay(0)
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.can_motor = wpilib.CANTalon(15)
self.sensor = Sensor(self.tote_motor, self.can_motor)
self.tote_forklift = ToteForklift(self.tote_motor, self.sensor, 2)
self.can_forklift = CanForklift(self.can_motor, self.sensor, 3)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.next_pos = 1
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift,
self.drive)
self.components = {
'tote_forklift': self.tote_forklift,
'can_forklift': self.can_forklift,
'drive': self.drive,
'align': self.align,
'sensors': self.sensor
}
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.components)
# #Defining Buttons##
self.canUp = Button(self.joystick1, 3)
self.canDown = Button(self.joystick1, 2)
self.canTop = Button(self.joystick1, 6)
self.canBottom = Button(self.joystick1, 7)
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.ui_joystick_can_up = Button(self.ui_joystick, 5)
self.ui_joystick_can_down = Button(self.ui_joystick, 3)
self.reverseDirection = Button(self.joystick1, 1)
#self.alignTrigger = Button(self.joystick2, 1)
self.toteTo = 0
self.oldTote = 0
self.canTo = 0
self.oldCan = 0
self.reverseRobot = False
self.oldReverseRobot = False
self.autoLift = False
self.sd.putNumber('liftTo', 0)
self.sd.putNumber('binTo', 0)
self.sd.putBoolean('autoLift', False)
self.sd.putBoolean('reverseRobot', False)
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# hello
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(
wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.sweeper_relay = wpilib.Relay(0)
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.can_motor = wpilib.CANTalon(15)
self.sensor = Sensor(self.tote_motor, self.can_motor)
self.tote_forklift = ToteForklift(self.tote_motor, self.sensor, 2)
self.can_forklift = CanForklift(self.can_motor, self.sensor, 3)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.next_pos = 1
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift,
self.drive)
self.components = {
'tote_forklift': self.tote_forklift,
'can_forklift': self.can_forklift,
'drive': self.drive,
'align': self.align,
'sensors': self.sensor
}
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.components)
# #Defining Buttons##
self.canUp = Button(self.joystick1, 3)
self.canDown = Button(self.joystick1, 2)
self.canTop = Button(self.joystick1, 6)
self.canBottom = Button(self.joystick1, 7)
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.ui_joystick_can_up = Button(self.ui_joystick, 5)
self.ui_joystick_can_down = Button(self.ui_joystick, 3)
self.reverseDirection = Button(self.joystick1, 1)
#self.alignTrigger = Button(self.joystick2, 1)
self.toteTo = 0
self.oldTote = 0
self.canTo = 0
self.oldCan = 0
self.reverseRobot = False
self.oldReverseRobot = False
self.autoLift = False
self.sd.putNumber('liftTo', 0)
self.sd.putNumber('binTo', 0)
self.sd.putBoolean('autoLift', False)
self.sd.putBoolean('reverseRobot', False)
def autonomous(self):
self.automodes.run(self.control_loop_wait_time, self.update)
def operatorControl(self):
self.can_forklift.set_manual(0)
self.tote_forklift.set_manual(0)
self.tote_forklift.set_pos_stack5()
delay = PreciseDelay(self.control_loop_wait_time)
self.logger.info("Entering teleop mode")
while self.isOperatorControl() and self.isEnabled():
self.sensor.update()
#self.calibrator.calibrate()
self.drive.move(self.joystick1.getY(), self.joystick1.getX(),
self.joystick2.getX(), True)
#
# Can forklift controls
#
if self.joystick1.getRawButton(5):
self.can_forklift.set_manual(1)
elif self.joystick1.getRawButton(4):
self.can_forklift.set_manual(-1)
elif self.canUp.get():
self.can_forklift.raise_forklift()
elif self.canDown.get():
self.can_forklift.lower_forklift()
if self.canTop.get():
self.can_forklift.set_pos_top()
elif self.canBottom.get():
self.can_forklift.set_pos_bottom()
# #Tote forklift controls##
if self.joystick2.getRawButton(5):
self.tote_forklift.set_manual(1)
elif self.joystick2.getRawButton(4):
self.tote_forklift.set_manual(-1)
elif self.toteUp.get():
self.tote_forklift.raise_forklift()
elif self.toteDown.get():
self.tote_forklift.lower_forklift()
if self.toteTop.get():
self.tote_forklift.set_pos_top()
elif self.toteBottom.get():
self.tote_forklift.set_pos_bottom()
# INFRARED DRIVE#
if self.joystick2.getTrigger():
self.drive.isTheRobotBackwards = False
self.align.align()
elif self.autoLift:
if self.sensor.toteLimitL and self.sensor.toteLimitR:
self.tote_forklift.raise_forklift()
if self.joystick2.getRawButton(11):
self.drive.reset_gyro_angle()
if self.joystick2.getRawButton(8):
self.drive.wall_strafe(-.7)
elif self.joystick2.getRawButton(9):
self.drive.wall_strafe(.7)
# REVERSE DRIVE#
if self.reverseDirection.get():
self.drive.switch_direction()
if self.joystick1.getRawButton(10):
self.sweeper_relay.set(RelayValue.kForward)
elif self.joystick1.getRawButton(11):
self.sweeper_relay.set(RelayValue.kReverse)
else:
self.sweeper_relay.set(RelayValue.kOff)
if self.toteTo != self.oldTote:
if self.toteTo == 0:
self.tote_forklift._set_position(0)
elif self.toteTo == 1:
self.tote_forklift._set_position(1)
elif self.toteTo == 2:
self.tote_forklift._set_position(2)
elif self.toteTo == 3:
self.tote_forklift._set_position(3)
elif self.toteTo == 4:
self.tote_forklift._set_position(4)
elif self.toteTo == 2048:
self.tote_forklift.set_pos_top()
self.oldTote = self.toteTo
if self.canTo != self.oldCan:
if self.canTo == 0:
self.can_forklift._set_position(0)
elif self.canTo == 1:
self.can_forklift._set_position(1)
elif self.canTo == 2:
self.can_forklift._set_position(2)
elif self.canTo == 3:
self.can_forklift._set_position(3)
elif self.canTo == 4:
self.can_forklift._set_position(4)
elif self.canTo == 2048:
self.can_forklift.set_pos_top()
elif self.canTo == 7000:
self.can_forklift.set_pos_7000()
self.oldCan = self.canTo
if self.reverseRobot != self.oldReverseRobot:
if self.reverseRobot == 0:
self.drive.switch_direction()
self.oldReverseRobot = self.reverseRobot
self.ui_joystick_buttons()
self.smartdashbord_update()
self.update()
# Replaces wpilib.Timer.delay()
delay.wait()
def smartdashbord_update(self):
self.sd.putNumber('backSensorValue', self.backSensor.getDistance())
self.sensor.update_sd()
self.can_forklift.update_sd('Can Forklift')
self.tote_forklift.update_sd('Tote Forklift')
self.sd.putNumber('gyroAngle', self.gyro.getAngle())
self.toteTo = self.sd.getInt('liftTo', self.toteTo)
self.canTo = self.sd.getInt('binTo', self.canTo)
self.autoLift = self.sd.getBoolean('autoLift', self.autoLift)
self.reverseRobot = self.sd.getBoolean('reverseRobot',
self.reverseRobot)
def ui_joystick_buttons(self):
if self.ui_joystick_can_down.get():
self.can_forklift.set_pos_bottom()
elif self.ui_joystick_can_up.get():
self.can_forklift.set_pos_top()
if self.ui_joystick_tote_down.get():
self.tote_forklift.set_pos_bottom()
elif self.ui_joystick_tote_up.get():
self.tote_forklift.set_pos_top()
def update(self):
for component in self.components.values():
component.doit()
def disabled(self):
'''Called when the robot is in disabled mode'''
self.logger.info("Entering disabled mode")
while not self.isEnabled():
self.sensor.update()
self.smartdashbord_update()
wpilib.Timer.delay(.01)
def test(self):
'''Called when the robot is in test mode'''
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def test_detect_position(hal_data):
tote_motor = wpilib.CANTalon(1)
sensors = Sensor(tote_motor)
forklift = ToteForklift(tote_motor, sensors, 5)
forklift.set_manual(0)
can = hal_data['CAN'][1]
# Should return None when not calibrated
assert forklift.isCalibrated == False
assert forklift._detect_position_index(170, 0) == None
forklift.isCalibrated = True
keys = [
'Tote Forklift|bottom',
'Tote Forklift|stack1',
'Tote Forklift|stack2',
'Tote Forklift|stack3',
'Tote Forklift|stack4',
'Tote Forklift|stack5',
'Tote Forklift|stack6',
]
# set positions to known values
for i, key in enumerate(keys):
wpilib.SmartDashboard.putNumber(key, i*1000)
can['enc_position'] = 5
sensors.update()
assert forklift.get_position() == 5
down_tests = [
# encoder, expected position, offset
(-2000, 0, 0),
(-200, 0, 0),
(150, 0, 0),
(500, 1, 0),
(800, 1, 0),
(900, 1, 0),
(1000, 1, 0),
(1150, 1, 0),
(1500, 2, 1),
(1700, 2, 1),
(2000, 2, 1),
(2150, 2, 1),
(2500, 3, 2)
]
for enc_value, expected_pos, expected_idx in down_tests:
print(enc_value)
can['enc_position'] = enc_value
sensors.update()
assert forklift._detect_position_index(170, 0) == expected_pos
forklift.set_manual(0)
forklift.lower_forklift()
assert forklift.target_index == expected_idx
up_tests = [
# encoder, expected position + 1, target_inde
(-2000, -1, 1),
(-150, 0, 1),
(200, 0, 1),
(500, 0, 1),
(850, 1, 2),
(900, 1, 2),
(1000, 1, 2),
(1200, 1, 2),
(1500, 1, 2),
(1700, 1, 2),
(1900, 2, 3),
(2000, 2, 3),
(2200, 2, 3),
(2500, 2, 3)
]
for enc_value, expected_pos, expected_idx in up_tests:
print(enc_value)
can['enc_position'] = enc_value
sensors.update()
assert forklift._detect_position_index(-170, -1) == expected_pos
forklift.set_manual(0)
forklift.raise_forklift()
assert forklift.target_index == expected_idx
def __init__(self, iterations: int, connection: Connection, headway: float, leader, noise, sensor_delay):
super().__init__(iterations, connection, headway)
self.connection = connection
self.sensor = Sensor(sensor_delay, noise)
self.leader = leader
def test_detect_position(hal_data):
tote_motor = wpilib.CANTalon(1)
can_motor = wpilib.CANTalon(2)
sensors = Sensor(tote_motor, can_motor)
forklift = ToteForklift(tote_motor, sensors, 5)
forklift.set_manual(0)
can = hal_data['CAN'][1]
# Should return None when not calibrated
assert forklift.isCalibrated == False
assert forklift._detect_position_index(170, 0) == None
forklift.isCalibrated = True
# set positions to known values
for i, pos in enumerate(forklift.positions):
pos.value = i*1000
can['enc_position'] = 5
sensors.update()
assert forklift.get_position() == 5
down_tests = [
# encoder, expected position, offset
(-2000, 0, 0),
(-200, 0, 0),
(150, 0, 0),
(500, 1, 0),
(800, 1, 0),
(900, 1, 0),
(1000, 1, 0),
(1150, 1, 0),
(1500, 2, 1),
(1700, 2, 1),
(2000, 2, 1),
(2150, 2, 1),
(2500, 3, 2)
]
for enc_value, expected_pos, expected_idx in down_tests:
print(enc_value)
can['enc_position'] = enc_value
sensors.update()
assert forklift._detect_position_index(170, 0) == expected_pos
forklift.set_manual(0)
forklift.lower_forklift()
assert forklift.target_index == expected_idx
up_tests = [
# encoder, expected position + 1, target_inde
(-2000, -1, 1),
(-150, 0, 1),
(200, 0, 1),
(500, 0, 1),
(850, 1, 2),
(900, 1, 2),
(1000, 1, 2),
(1200, 1, 2),
(1500, 1, 2),
(1700, 1, 2),
(1900, 2, 3),
(2000, 2, 3),
(2200, 2, 3),
(2500, 2, 3)
]
for enc_value, expected_pos, expected_idx in up_tests:
print(enc_value)
can['enc_position'] = enc_value
sensors.update()
assert forklift._detect_position_index(-170, -1) == expected_pos
forklift.set_manual(0)
forklift.raise_forklift()
assert forklift.target_index == expected_idx
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.sensor = Sensor(self.tote_motor)
self.tote_forklift = ToteForklift(self.tote_motor,self.sensor,2)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.autoLifter = autolift.Autolift(self.sensor, self.tote_forklift)
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift, self.drive, self.autoLifter)
# These must have a doit function
self.components = {
'tote_forklift': self.tote_forklift,
'drive': self.drive,
'autolift': self.autoLifter,
'align': self.align,
'sensors': self.sensor
}
# These do not, and get passed to autonomous mode
self.auton_components = {
'pin_servo': self.pinServo
}
self.auton_components.update(self.components)
# #Defining Buttons##
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.reverseDirection = Button(self.joystick1, 1)
# Secondary driver's joystick
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.oldReverseRobot = False
self.toteTo = self.sd.getAutoUpdateValue('toteTo', -1)
self.reverseRobot = self.sd.getAutoUpdateValue('reverseRobot',False)
self.autoLift = self.sd.getAutoUpdateValue('autoLift', False)
# If set, this means we go forward and try to pickup the three totes
# that we've deposited in autonomous mode
self.autoPickup = self.sd.getAutoUpdateValue('autoPickup', False)
self.autoPickupSpeed = self.sd.getAutoUpdateValue('autoPickupSpeed', -0.2)
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.auton_components)
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.sd = NetworkTable.getTable('SmartDashboard')
# #INITIALIZE JOYSTICKS##
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
self.ui_joystick = wpilib.Joystick(2)
self.pinServo = wpilib.Servo(6)
# #INITIALIZE MOTORS##
self.lf_motor = wpilib.Talon(0)
self.lr_motor = wpilib.Talon(1)
self.rf_motor = wpilib.Talon(2)
self.rr_motor = wpilib.Talon(3)
# #ROBOT DRIVE##
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kFrontRight, True)
self.robot_drive.setInvertedMotor(wpilib.RobotDrive.MotorType.kRearRight, True)
# #INITIALIZE SENSORS#
self.gyro = wpilib.Gyro(0)
self.tote_motor = wpilib.CANTalon(5)
self.sensor = Sensor(self.tote_motor)
self.tote_forklift = ToteForklift(self.tote_motor,self.sensor,2)
self.calibrator = Calibrator(self.tote_forklift, self.sensor)
self.autoLifter = autolift.Autolift(self.sensor, self.tote_forklift)
self.backSensor = SharpIRGP2Y0A41SK0F(6)
self.drive = drive.Drive(self.robot_drive, self.gyro, self.backSensor)
self.align = alignment.Alignment(self.sensor, self.tote_forklift, self.drive, self.autoLifter)
# These must have a doit function
self.components = {
'tote_forklift': self.tote_forklift,
'drive': self.drive,
'autolift': self.autoLifter,
'align': self.align,
'sensors': self.sensor
}
# These do not, and get passed to autonomous mode
self.auton_components = {
'pin_servo': self.pinServo
}
self.auton_components.update(self.components)
# #Defining Buttons##
self.toteUp = Button(self.joystick2, 3)
self.toteDown = Button(self.joystick2, 2)
self.toteTop = Button(self.joystick2, 6)
self.toteBottom = Button(self.joystick2, 7)
self.reverseDirection = Button(self.joystick1, 1)
# Secondary driver's joystick
self.ui_joystick_tote_down = Button(self.ui_joystick, 4)
self.ui_joystick_tote_up = Button(self.ui_joystick, 6)
self.oldReverseRobot = False
self.toteTo = self.sd.getAutoUpdateValue('toteTo', -1)
self.reverseRobot = self.sd.getAutoUpdateValue('reverseRobot',False)
self.autoLift = self.sd.getAutoUpdateValue('autoLift', False)
# If set, this means we go forward and try to pickup the three totes
# that we've deposited in autonomous mode
self.autoPickup = self.sd.getAutoUpdateValue('autoPickup', False)
self.autoPickupSpeed = self.sd.getAutoUpdateValue('autoPickupSpeed', -0.2)
self.control_loop_wait_time = 0.025
self.automodes = AutonomousModeSelector('autonomous', self.auton_components)
def autonomous(self):
self.sd.putBoolean('autoPickup', False)
self.automodes.run(self.control_loop_wait_time, self.update)
def operatorControl(self):
self.tote_forklift.set_manual(0)
delay = PreciseDelay(self.control_loop_wait_time)
self.logger.info("Entering teleop mode")
while self.isOperatorControl() and self.isEnabled():
self.sensor.update()
#self.calibrator.calibrate()
try:
self.drive.move(self.joystick1.getY()*((1+self.joystick1.getZ())/2), self.joystick1.getX(), self.joystick2.getX(),True)
except:
if not self.isFMSAttached():
raise
#
# Can forklift controls
#
## Tote forklift controls##
try:
if self.joystick2.getRawButton(5):
self.tote_forklift.set_manual(1)
elif self.joystick2.getRawButton(4):
self.tote_forklift.set_manual(-1)
elif self.toteUp.get():
self.tote_forklift.raise_forklift()
elif self.toteDown.get():
self.tote_forklift.lower_forklift()
if self.toteTop.get():
self.tote_forklift.set_pos_top()
elif self.toteBottom.get():
self.tote_forklift.set_pos_bottom()
except:
if not self.isFMSAttached():
raise
if self.toteTo.value >= 0:
toteTo = int(self.toteTo.value)
if toteTo == 2048:
self.tote_forklift.set_pos_top()
else:
self.tote_forklift._set_position(toteTo)
self.sd.putDouble('toteTo', -1)
#
# Driver-enabled automatic alignment code
#
try:
if self.joystick2.getTrigger():
# self.drive.isTheRobotBackwards = False
# self.align.align()
#elif self.autoLift.value:
self.autoLifter.autolift()
except:
if not self.isFMSAttached():
raise
#
# Utilities
#
try:
if self.joystick2.getRawButton(11):
self.drive.reset_gyro_angle()
except:
if not self.isFMSAttached():
raise
try:
if self.joystick2.getRawButton(8):
self.drive.wall_strafe(-.7)
elif self.joystick2.getRawButton(9):
self.drive.wall_strafe(.7)
except:
if not self.isFMSAttached():
raise
#
# Reverse drive
#
try:
if self.reverseDirection.get():
self.drive.switch_direction()
except:
if not self.isFMSAttached():
raise
try:
if self.reverseRobot.value != self.oldReverseRobot:
if self.reverseRobot.value == 0:
self.drive.switch_direction()
self.oldReverseRobot = self.reverseRobot.value
except:
if not self.isFMSAttached():
raise
#
# At the end of autonomous mode, pick up the three bins in
# front of us if enabled
#
#if self.autoPickup.value:
# try:
# # End autopickup when the driver gives joystick input
# if abs(self.joystick1.getX())>.1 or abs(self.joystick1.getY())>.1 or abs(self.joystick2.getX())>.1:
# self.sd.putBoolean('autoPickup', False)
# else:
# if not self.sensor.is_against_tote():
# self.drive.move(self.autoPickupSpeed.value, 0, 0)
# else:
# # Move slow once we hit the totes
# self.drive.move(-0.1, 0, 0)
#
# self.autoLifter.autolift()
#
# except:
# self.sd.putBoolean('autoPickup', False)
# if not self.isFMSAttached():
# raise
try:
self.ui_joystick_buttons()
except:
if not self.isFMSAttached():
raise
try:
self.smartdashboard_update()
except:
if not self.isFMSAttached():
raise
try:
self.update()
except:
if not self.isFMSAttached():
raise
# Replaces wpilib.Timer.delay()
delay.wait()
def smartdashboard_update(self):
self.sensor.update_sd()
self.tote_forklift.update_sd('Tote Forklift')
self.sd.putNumber('backSensorValue', self.backSensor.getDistance())
self.sd.putNumber('gyroAngle', self.gyro.getAngle())
def ui_joystick_buttons(self):
if self.ui_joystick_tote_down.get():
self.tote_forklift.set_pos_bottom()
elif self.ui_joystick_tote_up.get():
self.tote_forklift.set_pos_top()
def update (self):
for component in self.components.values():
component.doit()
def disabled(self):
'''Called when the robot is in disabled mode'''
self.logger.info("Entering disabled mode")
while not self.isEnabled():
self.sensor.update()
self.smartdashboard_update()
wpilib.Timer.delay(.01)
def test(self):
'''Called when the robot is in test mode'''
while self.isTest() and self.isEnabled():
wpilib.LiveWindow.run()
wpilib.Timer.delay(.01)
def isFMSAttached(self):
return self.ds.isFMSAttached()