예제 #1
0
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)
예제 #2
0
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
예제 #3
0
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
예제 #4
0
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()