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 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 __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
