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