Beispiel #1
0
    def robotInit(self):
        '''Robot initialization function'''

        # object that handles basic drive operations
        self.leftMotor = wpilib.Victor(0)
        self.rightMotor = wpilib.Victor(1)

        #self.myRobot = wpilib.RobotDrive(0, 1)
        self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)


        # joyStick 0
        self.joyStick = wpilib.Joystick(0)
        self.myRobot.setExpiration(0.1)
        self.myRobot.setSafetyEnabled(True)

        # encoders
        self.rightEncoder = wpilib.Encoder(0, 1, False, wpilib.Encoder.EncodingType.k4X)
        self.leftEncoder = wpilib.Encoder(2,3, False, wpilib.Encoder.EncodingType.k4X)

        # set up encoder
        self.drivePulsePerRotation  = 1024
        self.driveWheelRadius = 3.0
        self. driveGearRatio = 1.0/1.0
        self.driveEncoderPulsePerRot = self.drivePulsePerRotation * self.driveGearRatio
        self.driveEncoderDistPerPulse = (math.pi*2*self.driveWheelRadius)/self.driveEncoderPulsePerRot;

        self.leftEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
        self.leftEncoder.setReverseDirection(False)
        self.rightEncoder.setDistancePerPulse(self.driveEncoderDistPerPulse)
        self.rightEncoder.setReverseDirection(False)

        self.timer = wpilib.Timer()
Beispiel #2
0
    def robotInit(self):
        """Robot initialization function"""
        # object that handles basic drive operations
        self.leftVictor = ctre.WPI_VictorSPX(LEFT)
        self.rightVictor = ctre.WPI_VictorSPX(RIGHT)
        self.centerVictor1 = ctre.WPI_VictorSPX(CENTER1)
        self.centerVictor2 = ctre.WPI_VictorSPX(CENTER2)

        self.left = wpilib.SpeedControllerGroup(self.leftVictor)
        self.right = wpilib.SpeedControllerGroup(self.rightVictor)

        self.center1 = wpilib.SpeedControllerGroup(self.centerVictor1)
        self.center2 = wpilib.SpeedControllerGroup(self.centerVictor2)

        self.myRobot = DifferentialDrive(self.left, self.right)
        self.myRobot.setExpiration(0.1)

        # joysticks 1 & 2 on the driver station
        # self.leftStick = wpilib.Joystick(0)
        # self.rightStick = wpilib.Joystick(1)

        self.DEADZONE = 0.4

        self.LEFT = GenericHID.Hand.kLeft
        self.RIGHT = GenericHID.Hand.kRight

        self.driver = wpilib.XboxController(0)

        self.ballManipulator = BallManipulator(
            ctre.WPI_VictorSPX(BALL_MANIP_ID))
class Chassis:
    chassis_left_rear: rev.CANSparkMax
    chassis_left_front: rev.CANSparkMax
    chassis_right_rear: rev.CANSparkMax
    chassis_right_front: rev.CANSparkMax

    def setup(self) -> None:
        self.chassis_left_rear.setInverted(False)
        self.chassis_left_front.setInverted(False)
        self.chassis_right_rear.setInverted(False)
        self.chassis_right_front.setInverted(False)

        self.chassis_left_rear.follow(self.chassis_left_front)
        self.chassis_right_rear.follow(self.chassis_right_front)

        self.diff_drive = DifferentialDrive(self.chassis_left_front,
                                            self.chassis_right_front)

    def execute(self) -> None:
        self.diff_drive.arcadeDrive(self.vx, self.vz, squareInputs=False)

    def drive(self, vx: float, vz: float) -> None:
        """Drive the robot with forwards velocity and rotational velocity
            vx: Forward is positive.
            vz: Clockwise is negative
        """
        self.vx, self.vz = vx, -vz

    def get_heading(self) -> float:
        # TODO
        return 0.0

    def get_position(self) -> Tuple[float, float]:
        # TODO
        return (0.0, 0.0)
Beispiel #4
0
class MyRobot(magicbot.MagicRobot):
    def createObjects(self):

        self.leftStick = wpilib.Joystick(2)
        self.elevatorStick = wpilib.Joystick(1)
        self.rightStick = wpilib.Joystick(0)
        self.elevatorMotorOne = wpilib.Victor(2)
        self.elevatorMotorTwo = wpilib.Victor(3)
        self.left = wpilib.Victor(0)
        self.right = wpilib.Victor(1)
        self.myRobot = DifferentialDrive(self.left, self.right)
        self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne,
                                                    self.elevatorMotorTwo)
        self.elevatorPot = wpilib.AnalogPotentiometer(0)

        self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
        self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
        self.gearshift.set(1)

    def teleopInit(self):
        pass

    def teleopPeriodic(self):

        self.myRobot.tankDrive(-self.leftStick.getY(), -self.rightStick.getY())
        if (self.trigger.get()):
            if (self.gearshift.get() == 1):
                self.gearshift.set(2)
            elif (self.gearshift.get() == 2):
                self.gearshift.set(1)
        self.elevator.set(self.elevatorStick.getY())
Beispiel #5
0
    def robotInit(self):

        # Launches the camera server so that we can have video through any cameras on the robot.
        wpilib.CameraServer.launch()

        # Defines the motors that will actually be on the robot for use in the drive function.
        self.FLMotor = wpilib.Spark(self.FLChannel)
        self.FRMotor = wpilib.Spark(self.FRChannel)
        self.RLMotor = wpilib.Spark(self.RLChannel)
        self.RRMotor = wpilib.Spark(self.RRChannel)

        # Puts the motors into groups so that they fit the parameters of the function.
        self.LMG = wpilib.SpeedControllerGroup(self.FLMotor, self.RLMotor)
        self.RMG = wpilib.SpeedControllerGroup(self.FRMotor, self.RRMotor)

        # The drive function that tells the computer what kind of drive to use and where the motors are.
        self.drive = DifferentialDrive(self.LMG, self.RMG)

        # Tells the computer how long to wait without input to turn off the motors
        self.drive.setExpiration(0.1)

        # Defines the Joystick that we will be using for driving.
        self.DriveStick = wpilib.Joystick(self.DriveStickChannel)

        self.components = {"drive": self.drive}

        self.automodes = robotpy_ext.autonomous.AutonomousModeSelector(
            "autonomous", self.components)
Beispiel #6
0
    def robotInit(self):
        '''Robot initialization function'''

        # object that handles basic drive operations
        self.frontLeftMotor = wpilib.Talon(1)
        self.rearLeftMotor = wpilib.Talon(2)
        self.frontRightMotor = wpilib.Talon(3)
        self.rearRightMotor = wpilib.Talon(4)

        self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
                                                self.rearLeftMotor)
        self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
                                                 self.rearRightMotor)

        self.drive = DifferentialDrive(self.left, self.right)
        self.drive.setExpiration(0.1)
        self.timer = wpilib.Timer()

        # joystick 1 on the driver station
        self.stick = wpilib.Joystick(1)

        # Initialization of the pneumatic system
        self.Compressor = wpilib.Compressor(0)
        self.Compressor.setClosedLoopControl(True)
        self.enabled = self.Compressor.enabled()
        self.PSV = self.Compressor.getPressureSwitchValue()
        self.DS = wpilib.DoubleSolenoid(0, 1)
        self.Compressor.start()

        # Initialization of the camera server
        wpilib.CameraServer.launch()
Beispiel #7
0
class MyRobot(wpilib.SampleRobot):
    '''This is a demo program showing how to use Gyro control with the
    RobotDrive class.'''
    def robotInit(self):
        '''Robot initialization function'''
        gyroChannel = 0  # analog input

        self.joystickChannel = 0  # usb number in DriverStation

        # channels for motors
        self.leftMotorChannel = 1
        self.rightMotorChannel = 0
        self.leftRearMotorChannel = 3
        self.rightRearMotorChannel = 2

        self.angleSetpoint = 0.0
        self.pGain = 1  # propotional turning constant

        # gyro calibration constant, may need to be adjusted
        # gyro value of 360 is set to correspond to one full revolution
        self.voltsPerDegreePerSecond = .0128

        self.left = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.leftMotorChannel),
            wpilib.Talon(self.leftRearMotorChannel))
        self.right = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.rightMotorChannel),
            wpilib.Talon(self.rightRearMotorChannel))
        self.myRobot = DifferentialDrive(self.left, self.right)

        self.gyro = wpilib.AnalogGyro(gyroChannel)
        self.joystick = wpilib.Joystick(self.joystickChannel)

    def autonomous(self):
        '''Runs during Autonomous'''
        pass

    def operatorControl(self):
        '''
        Sets the gyro sensitivity and drives the robot when the joystick is pushed. The
        motor speed is set from the joystick while the RobotDrive turning value is assigned
        from the error between the setpoint and the gyro angle.
        '''

        self.gyro.setSensitivity(self.voltsPerDegreePerSecond
                                 )  # calibrates gyro values to equal degrees
        while self.isOperatorControl() and self.isEnabled():
            turningValue = (self.angleSetpoint -
                            self.gyro.getAngle()) * self.pGain
            if self.joystick.getY() <= 0:
                # forwards
                self.myRobot.arcadeDrive(self.joystick.getY(), turningValue)
            elif self.joystick.getY() > 0:
                # backwards
                self.myRobot.arcadeDrive(self.joystick.getY(), -turningValue)
            wpilib.Timer.delay(0.005)

    def test(self):
        '''Runs during test mode'''
        pass
Beispiel #8
0
    def __init__(self):
        super().__init__('DriveTrain')
        map = wpilib.command.Command.getRobot().robotMap

        #create motors here
        motors = {}
        for name in map.CAN.driveMotors:
            motors[name] = wpilib.command.Command.getRobot(
            ).motorHelper.createMotor(map.CAN.driveMotors[name])
        print(motors['leftMotor'].getSensorCollection())
        self.motors = motors
        self.drive = DifferentialDrive(motors['leftMotor'],
                                       motors['rightMotor'])
        self.drive.setSafetyEnabled(False)
        self.minSpeedChange = 0.001
        self.timeRate = 0.2

        self.desired = {}
        self.desired['left'] = 0
        self.desired['right'] = 0
        self.current = {}
        self.current['left'] = 0
        self.current['right'] = 0

        self.lastUpdateTime = time.time()
        self.deadband = 0.1
Beispiel #9
0
    def __init__(self):
        self.pending_differential_drive = None
        self.force_differential_drive = False
        self.pending_gear = LOW_GEAR
        self.pending_position = None
        self.pending_reset = False
        self.og_yaw = None
        self.pending_manual_drive = None
        self.is_manual_mode = False

        # Set encoders
        self.left_motor_master.configSelectedFeedbackSensor(
            ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
        self.right_motor_master.configSelectedFeedbackSensor(
            ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
        self.left_motor_master.setSensorPhase(True)

        # Set slave motors
        self.left_motor_slave.set(ctre.TalonFXControlMode.Follower,
                                  self.left_motor_master.getDeviceID())
        self.left_motor_slave2.set(ctre.TalonFXControlMode.Follower,
                                   self.left_motor_master.getDeviceID())
        self.right_motor_slave.set(ctre.TalonFXControlMode.Follower,
                                   self.right_motor_master.getDeviceID())
        self.right_motor_slave2.set(ctre.TalonFXControlMode.Follower,
                                    self.right_motor_master.getDeviceID())

        # Set up drive control
        self.robot_drive = DifferentialDrive(self.left_motor_master,
                                             self.right_motor_master)
        self.robot_drive.setDeadband(0)
        self.robot_drive.setSafetyEnabled(False)
Beispiel #10
0
    def robotInit(self):
        # SPARK MAX controllers are intialized over CAN by constructing a
        # CANSparkMax object
        #
        # The CAN ID, which can be configured using the SPARK MAX Client, is passed
        # as the first parameter
        #
        # The motor type is passed as the second parameter.
        # Motor type can either be:
        #   rev.MotorType.kBrushless
        #   rev.MotorType.kBrushed
        #
        # The example below initializes two brushless motors with CAN IDs
        # 1 and 2. Change these parameters to match your setup
        self.leftMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
        self.rightMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless)

        # The RestoreFactoryDefaults method can be used to reset the
        # configuration parameters in the SPARK MAX to their factory default
        # state. If no argument is passed, these parameters will not persist
        # between power cycles
        self.leftMotor.restoreFactoryDefaults()
        self.rightMotor.restoreFactoryDefaults()

        self.driveTrain = DifferentialDrive(self.leftMotor, self.rightMotor)
        self.l_stick = wpilib.Joystick(0)
        self.r_stick = wpilib.Joystick(1)
Beispiel #11
0
 def __init__(self, drive: Drive, leftMotors: SpeedControllerGroup, rightMotors: SpeedControllerGroup, gamePad: GenericHID):
   super().__init__("DriveArcade")
   self.requires(drive)
   self.gamePad = gamePad
   self.diffDrive = DifferentialDrive(leftMotors, rightMotors)
   self.nominal = 3.0 / 8.0
   self.deadband = 0.15 # XBox360 controller has a lot of slop
Beispiel #12
0
    def robotInit(self):
        '''Robot initialization function'''
        
        # object that handles basic drive operations
        self.frontLeftMotor = wpilib.Spark(3)
        self.middleLeftMotor = wpilib.Spark(4)
        self.rearLeftMotor = wpilib.Spark(5)

        self.frontRightMotor = wpilib.Spark(0)
        self.middleRightMotor = wpilib.Spark(1)
        self.rearRightMotor = wpilib.Spark(2)

        self.ihigh_motor = wpilib.Spark(6)
        self.ilow_motor = wpilib.Spark(9)

        self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.middleLeftMotor, self.rearLeftMotor)
        self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.middleRightMotor, self.rearRightMotor)

        self.myRobot = DifferentialDrive(self.left, self.right)
        self.myRobot.setExpiration(0.1)

        self.high = 0
        self.low = 0
        self.gameData = 'LRL'

        # joysticks 1 & 2 on the driver station
        self.Stick1 = wpilib.XboxController(0)
        self.Stick2 = wpilib.Joystick(1)
        
        self.aSolenoidLow = wpilib.DoubleSolenoid(2,3)
        self.aSolenoidHigh = wpilib.DoubleSolenoid(0,1)
        self.iSolenoid = wpilib.DoubleSolenoid(4,5)
Beispiel #13
0
    def robotInit(self):
        """
        This function is called upon program startup and
        should be used for any initialization code.
        """
        self.lt_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
        self.lf_motor = rev.CANSparkMax(3, rev.MotorType.kBrushless)
        self.lb_motor = rev.CANSparkMax(1, rev.MotorType.kBrushless)
        self.rt_motor = rev.CANSparkMax(5, rev.MotorType.kBrushless)
        self.rf_motor = rev.CANSparkMax(4, rev.MotorType.kBrushless)
        self.rb_motor = rev.CANSparkMax(6, rev.MotorType.kBrushless)

        self.left = wpilib.SpeedControllerGroup(self.lt_motor, self.lf_motor,
                                                self.lb_motor)
        self.right = wpilib.SpeedControllerGroup(self.rt_motor, self.rf_motor,
                                                 self.rb_motor)

        self.drive = DifferentialDrive(self.left, self.right)

        self.lt_motor.setInverted(True)
        self.rt_motor.setInverted(True)

        self.joystick = wpilib.Joystick(0)

        self.previous_button = False

        self.gear_switcher = wpilib.DoubleSolenoid(0, 1)
    def robotInit(self):            #This function is called upon program startup and should be used for mapping everything

        #motors
        self.motorL = wpilib.Spark(0)                               #channel 0
        self.motorR = wpilib.Spark(1)                               #channel 1
        self.drive = DifferentialDrive(self.motorL, self.motorR)    #dive setup, differential is used with tank
        
        #solenoids
        self.arm = wpilib.DoubleSolenoid(0,0,1)                     #modul 0 channels 0 and 1
        self.claw = wpilib.DoubleSolenoid(0,2,3)                    #modul 0 channels 2 and 3

        #controller
        self.controller = wpilib.Joystick(1)
            #in code use the following for each button or joystick with Logitech in "D" mode
            #left joystick horizontal   -   self.controller.getX()
            #left joystick vertical     -   self.controller.getY()
            #right joystick horizontal  -   self.controller.getZ()
            #right joystick vertical    -   self.controller.getT()
            #button X                   -   self.controller.getButton(1)
            #button A                   -   self.controller.getButton(2)
            #button B                   -   self.controller.getButton(3)
            #button Y                   -   self.controller.getButton(4)
            #trigger top left           -   self.controller.getButton(5)
            #trigger top right          -   self.controller.getButton(6)
            #bumper bottom left         -   self.controller.getButton(7)      
            #bumper bottom right        -   self.controller.getButton(8)      
            #button Back                -   self.controller.getButton(9)
            #button Start               -   self.controller.getButton(10)
       
        self.timer = wpilib.Timer()
Beispiel #15
0
    def setup(self):
        # Setting Masters
        self.left_slave.follow(self.left_master)
        self.right_slave.follow(self.right_master)

        # Setting safety
        self.left_master.setSafetyEnabled(False)
        self.left_slave.setSafetyEnabled(False)
        self.right_master.setSafetyEnabled(False)
        self.right_slave.setSafetyEnabled(False)

        # Setting inversion
        self.left_master.setInverted(False)
        self.left_slave.setInverted(False)
        self.right_master.setInverted(False)
        self.right_slave.setInverted(False)

        # Setting the Encoders and configuring the Talons
        self.right_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder)
        self.left_master.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder)

        self.left_master.configVoltageCompSaturation(11)
        self.right_master.configVoltageCompSaturation(11)

        self.left_master.enableVoltageCompensation(True)
        self.right_master.enableVoltageCompensation(True)

        # Creating a drive
        self.drive = DifferentialDrive(self.left_master, self.right_master)
        self.drive.setSafetyEnabled(True)
Beispiel #16
0
    def robotInit(self):
        '''Robot initialization function'''
        gyroChannel = 0  # analog input

        self.joystickChannel = 0  # usb number in DriverStation

        # channels for motors
        self.leftMotorChannel = 1
        self.rightMotorChannel = 0
        self.leftRearMotorChannel = 3
        self.rightRearMotorChannel = 2

        self.angleSetpoint = 0.0
        self.pGain = 1  # propotional turning constant

        # gyro calibration constant, may need to be adjusted
        # gyro value of 360 is set to correspond to one full revolution
        self.voltsPerDegreePerSecond = .0128

        self.left = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.leftMotorChannel),
            wpilib.Talon(self.leftRearMotorChannel))
        self.right = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.rightMotorChannel),
            wpilib.Talon(self.rightRearMotorChannel))
        self.myRobot = DifferentialDrive(self.left, self.right)

        self.gyro = wpilib.AnalogGyro(gyroChannel)
        self.joystick = wpilib.Joystick(self.joystickChannel)
Beispiel #17
0
    def robotInit(self):
        # Note the lack of the camera server initialization here.

        # Initializing drive motors
        RLMotor = Spark(self.RLMotorChannel)
        RRMotor = Spark(self.RRMotorChannel)
        FLMotor = Spark(self.FLMotorChannel)
        FRMotor = Spark(self.FRMotorChannel)

        # Grouping drive motors into left and right.
        self.Left = SpeedControllerGroup(RLMotor, FLMotor)
        self.Right = SpeedControllerGroup(RRMotor, FRMotor)

        # Initializing drive Joystick.
        self.DriveStick = Joystick(self.DriveStickChannel)

        # Initializing drive function.
        self.drive = DifferentialDrive(self.Left, self.Right)

        # Sets the right side motors to be inverted.
        self.drive.setRightSideInverted(rightSideInverted=True)

        # Turns the drive off after .1 seconds of inactivity.
        self.drive.setExpiration(0.1)

        # Components is a dictionary that contains any variables you want to put into it. All of the variables put into
        # components dictionary is given to the autonomous modes.
        self.components = {"drive": self.drive}

        # Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous
        # modes should inherit.
        self.automodes = autonomous.AutonomousModeSelector(
            "Sim_Autonomous", self.components)
Beispiel #18
0
    def __init__(self, robot):
        """Save the robot object, and assign and save hardware ports
        connected to the drive motors."""
        super().__init__(name = "drivetrain")
        self.robot = robot

        # The digital gyro plugged into the SPI port on RoboRIO
        self.gyro = wpilib.ADXRS450_Gyro()

        # Motors used for driving
        self.left = rev.CANSparkMax(1, rev.CANSparkMax.MotorType.kBrushless)
        self.leftB = rev.CANSparkMax(3, rev.CANSparkMax.MotorType.kBrushless)
        self.right = rev.CANSparkMax(2, rev.CANSparkMax.MotorType.kBrushless)
        self.rightB = rev.CANSparkMax(4, rev.CANSparkMax.MotorType.kBrushless)

        # TODO: switch to DifferentialDrive is the main object that deals with driving
        self.drive = DifferentialDrive(self.left, self.right)

        #TODO: These probably will not be the actual ports used
        self.left_encoder = wpilib.Encoder(2, 3)
        self.right_encoder = wpilib.Encoder(4, 5)

        # Encoders may measure differently in the real world and in
        # simulation. In this example the robot moves 0.042 barleycorns
        # per tick in the real world, but the simulated encoders
        # simulate 360 tick encoders. This if statement allows for the
        # real robot to handle this difference in devices.
        # TODO: Measure our encoder's distance per pulse
        if robot.isReal():
            self.left_encoder.setDistancePerPulse(0.042)
            self.right_encoder.setDistancePerPulse(0.042)
        else:
            # Circumference in ft = 4in/12(in/ft)*PI
            self.left_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
            self.right_encoder.setDistancePerPulse((4.0 / 12.0 * math.pi) / 360.0)
Beispiel #19
0
class Drivetrain(Subsystem):
    def __init__(self, robot):
        '''
		Command Dependencies:
			Do_Tank_Drive()

		'''
        super().__init__("drivetrain")
        #print("drivetrain init! no seg fault please")

        self.robot = robot
        self.lm_inst = Left_Motors().left_motor_group
        self.rm_inst = Right_Motors().right_motor_group
        self.drive = DifferentialDrive(self.lm_inst, self.rm_inst)

    #XXX not sure if this is correct
    def initDefaultCommand(self):
        self.setDefaultCommand(Do_Tank_Drive(self.robot))

    def set_tank_speed(self, left_joy, right_joy):
        left_speed = left_joy.getY()
        right_speed = right_joy.getY()
        self.drive.tankDrive(left_speed, right_speed)

    def stop_robot(self):
        self.drive.tankDrive(0, 0)
Beispiel #20
0
    def robotInit(self):
        """
        Initializes all motors in the robot.
        """

        self.leftTalon = ctre.WPI_TalonSRX(LEFT)
        self.rightTalon = ctre.WPI_TalonSRX(RIGHT)
        self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1)
        self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2)

        self.left = wpilib.SpeedControllerGroup(self.leftTalon)
        self.right = wpilib.SpeedControllerGroup(self.rightTalon)

        self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1)
        self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2)

        self.myRobot = DifferentialDrive(self.right, self.left)
        self.myRobot.setExpiration(0.1)

        #  reasonable deadzone size
        self.DEADZONE = 0.1

        self.driver = wpilib.XboxController(0)

        # Toggles whether or not robot can move
        self.emergencyStop = False
Beispiel #21
0
    def setup(self):
        self.pending_differential_drive = None
        self.force_differential_drive = False
        self.pending_gear = LOW_GEAR
        self.pending_position = None
        self.pending_reset = False
        self.og_yaw = None
        self.pending_manual_drive = None
        self.is_manual_mode = False

        # Set encoders
        self.left_motor_master.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
        self.right_motor_master.configSelectedFeedbackSensor(
            WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
        self.left_motor_master.setSensorPhase(True)

        # Set slave motors
        self.left_motor_slave.set(WPI_TalonSRX.ControlMode.Follower,
                                  self.left_motor_master.getDeviceID())
        self.left_motor_slave2.set(WPI_VictorSPX.ControlMode.Follower,
                                   self.left_motor_master.getDeviceID())
        self.right_motor_slave.set(WPI_TalonSRX.ControlMode.Follower,
                                   self.right_motor_master.getDeviceID())
        self.right_motor_slave2.set(WPI_VictorSPX.ControlMode.Follower,
                                    self.right_motor_master.getDeviceID())

        # Set up drive control
        self.robot_drive = DifferentialDrive(self.left_motor_master,
                                             self.right_motor_master)
        self.robot_drive.setDeadband(0)
        self.robot_drive.setSafetyEnabled(False)
Beispiel #22
0
    def __init__(self):
        super().__init__()
        self.joystick = wpilib.XboxController(0)

        self.left_drive_motor = wpilib.Talon(0)
        self.left_drive_motor_2 = wpilib.Talon(1)
        self.right_drive_motor = wpilib.Talon(2)
        self.right_drive_motor_2 = wpilib.Talon(3)

        self.left_drive_motor_group = wpilib.SpeedControllerGroup(
            self.left_drive_motor, self.left_drive_motor_2)
        self.right_drive_motor_group = wpilib.SpeedControllerGroup(
            self.right_drive_motor, self.right_drive_motor_2)

        self.drive = DifferentialDrive(self.left_drive_motor_group,
                                       self.right_drive_motor_group)
        self.drive_rev = False

        self.lift_motor = wpilib.Spark(4)
        self.lift_motor_2 = wpilib.Spark(5)
        self.lift_motor_group = wpilib.SpeedControllerGroup(
            self.lift_motor, self.lift_motor_2)
        self.lift_motor_speed = 0
        self.lock_controls = False

        self.front_motor = wpilib.Spark(6)
        self.front_motor_2 = wpilib.Spark(7)
        self.front_motor_2.setInverted(True)
        self.front_motor_group = wpilib.SpeedControllerGroup(
            self.front_motor, self.front_motor_2)

        self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)

        self.encoder = wpilib.Encoder(aChannel=0, bChannel=1)
Beispiel #23
0
    def robotInit(self):
        """Robot initialization function"""
        # variables for managing pneumatics sequence
        self.State = -1
        self.wait_timer = wpilib.Timer()

        # object that handles basic drive operations
        self.leftMotor = wpilib.Talon(1)
        self.rightMotor = wpilib.Talon(2)

        self.frontSolExtend = wpilib.Solenoid(1, 1)
        self.frontSolRetract = wpilib.Solenoid(1, 0)
        self.rearSolExtend = wpilib.Solenoid(1, 3)
        self.rearSolRetract = wpilib.Solenoid(1, 2)

        self.frontSolExtend.set(False)
        self.frontSolRetract.set(True)
        self.rearSolExtend.set(False)
        self.rearSolRetract.set(True)

        self.myRobot = DifferentialDrive(self.leftMotor, self.rightMotor)
        self.myRobot.setExpiration(0.1)

        # joysticks 1 & 2 on the driver station
        self.stick = wpilib.Joystick(0)
Beispiel #24
0
    def __init__(self):
        super().__init__("Chassis")

        self.l_controller = CANSparkMax(0, MotorType.kBrushless)
        self.r_controller = CANSparkMax(1, MotorType.kBrushless)

        self.drive = DifferentialDrive(self.l_controller, self.r_controller)
Beispiel #25
0
    def robotInit(self):
        """Robot initialization function"""
        LEFT = 1
        RIGHT = 2
        CENTER1 = 3
        CENTER2 = 4
        # object that handles basic drive operations
        self.leftTalon = ctre.WPI_TalonSRX(LEFT)
        self.rightTalon = ctre.WPI_TalonSRX(RIGHT)
        self.centerTalon1 = ctre.WPI_TalonSRX(CENTER1)
        self.centerTalon2 = ctre.WPI_TalonSRX(CENTER2)

        self.left = wpilib.SpeedControllerGroup(self.leftTalon)
        self.right = wpilib.SpeedControllerGroup(self.rightTalon)

        self.center1 = wpilib.SpeedControllerGroup(self.centerTalon1)
        self.center2 = wpilib.SpeedControllerGroup(self.centerTalon2)

        self.myRobot = DifferentialDrive(self.left, self.right)
        self.myRobot.setExpiration(0.1)

        # joysticks 1 & 2 on the driver station
        # self.leftStick = wpilib.Joystick(0)
        # self.rightStick = wpilib.Joystick(1)

        self.DEADZONE = 0.4

        self.LEFT = GenericHID.Hand.kLeft
        self.RIGHT = GenericHID.Hand.kRight

        self.driver = wpilib.XboxController(0)
Beispiel #26
0
class Chassis:
    left_motor_slave: WPI_TalonSRX
    right_motor_slave: WPI_TalonSRX
    right_motor_master: WPI_TalonSRX
    left_motor_master: WPI_TalonSRX
    navx: AHRS

    def setup(self):
        self.left_motor_slave.follow(self.left_motor_master)
        self.left_motor_master.configSelectedFeedbackSensor(
            FeedbackDevice.QuadEncoder)
        self.left_motor_master.configVoltageCompSaturation(11)
        self.left_motor_master.enableVoltageCompensation(True)
        self.left_motor_slave.setInverted(False)
        self.left_motor_master.setInverted(False)

        self.right_motor_slave.follow(self.right_motor_master)
        self.right_motor_master.configSelectedFeedbackSensor(
            FeedbackDevice.QuadEncoder)
        self.right_motor_master.configVoltageCompSaturation(11)
        self.right_motor_master.enableVoltageCompensation(True)
        self.right_motor_slave.setInverted(True)
        self.right_motor_master.setInverted(False)

        self.diffrential_drive = DifferentialDrive(self.left_motor_master,
                                                   self.right_motor_master)

        self.y_speed = 0
        self.z_speed = 0

    def get_angle(self):
        return self.navx.getAngle()

    def execute(self):
        self.diffrential_drive.arcadeDrive(self.y_speed, self.z_speed)

    def set_speed(self, y_speed, z_speed):
        self.y_speed = y_speed
        self.z_speed = z_speed

    def get_speed(self):
        return self.y_speed, self.z_speed

    def get_left_encoder(self):
        return self.left_motor_master.getSelectedSensorPosition()

    def get_right_encoder(self):
        return self.right_motor_master.getSelectedSensorPosition()

    def reset_encoders(self):
        self.left_motor_master.setSelectedSensorPosition(0)
        self.right_motor_master.setSelectedSensorPosition(0)

    def reset_gyro(self):
        self.navx.zeroYaw()

    def set_motors_values(self, left_speed, right_speed):
        self.right_motor_master.set(right_speed)
        self.left_motor_master.set(left_speed)
class Drivetrain(Subsystem):
    """Functions for the drivetrain"""
    def __init__(self):
        super().__init__("Drivetrain")

        self.leftleader = WPI_TalonSRX(CAN_LEFT_LEADER)
        set_motor(self.leftleader, WPI_TalonSRX.NeutralMode.Brake, False)

        self.leftfollower = WPI_VictorSPX(CAN_LEFT_FOLLOWER)
        set_motor(self.leftfollower, WPI_VictorSPX.NeutralMode.Brake, False)
        self.leftfollower.follow(self.leftleader)

        self.rightleader = WPI_TalonSRX(CAN_RIGHT_LEADER)
        set_motor(self.rightleader, WPI_TalonSRX.NeutralMode.Brake, True)

        self.rightfollower = WPI_VictorSPX(CAN_RIGHT_FOLLOWER)
        set_motor(self.rightfollower, WPI_VictorSPX.NeutralMode.Brake, True)
        self.rightfollower.follow(self.rightleader)

        self.DS = wpilib.DoubleSolenoid(0, 1)

        self.drive = DifferentialDrive(self.leftleader, self.rightleader)
        self.drive.maxOutput = 1.0

    def stickdrive(self):
        """set the motors based on the user inputs"""
        stick = subsystems.JOYSTICK
        x = stick.getRawAxis(4)
        #if x > 0.3 or x < -0.3:
        #    self.drive.maxOutput = 1.0
        #else:
        #    self.drive.maxOutput = 1.0

        self.set_gear(stick.getZ() > 0.5)

        # print(x)
        y = stick.getY()
        self.drive.arcadeDrive(-x, y)

        #P = self.leftleader.getQuadraturePosition()
        #P2 = self.rightleader.getQuadraturePosition()
        #print (" Left " +str (P) + " Right " +str (P2))
        #P3 = self.leftleader.getMotorOutputVoltage()
        #P4 = self.rightleader.getMotorOutputVoltage()
        #print(" Left " + str(P3) + " Right " + str(P4))
        #P5 = self.leftleader.getOutputCurrent()
        #P6 = self.rightleader.getOutputCurrent()
        #print(" Left " + str(P5) + " Right " + str(P6))

    def initDefaultCommand(self):
        self.setDefaultCommand(Drive())

    def set_gear(self, direction):
        if direction:
            self.DS.set(DoubleSolenoid.Value.kForward)
            subsystems.SERIAL.fire_event('High gear')
        else:
            self.DS.set(DoubleSolenoid.Value.kReverse)
            subsystems.SERIAL.fire_event('Low gear')
class Drivetrain(Subsystem):
    #def __init__(self, robot, ui):
    def __init__(self, robot):
        '''
		Command Dependencies:
			Do_Tank_Drive()
		'''
        super().__init__("drivetrain")

        self.robot = robot
        self.lm_inst = Left_Motors().left_motor_group
        self.rm_inst = Right_Motors().right_motor_group
        self.drive = DifferentialDrive(self.lm_inst, self.rm_inst)
        #XXX encoder DIO inputs and pulses_per_rev are currently incorrect
        pulses_per_rev = 12
        #self.right_encoder = wpilib.Encoder(8, 9)
        #self.right_encoder.setDistancePerPulse(pulses_per_rev)
        #self.left_encoder = wpilib.Encoder(6, 7)
        #self.left_encoder.setDistancePerPulse(pulses_per_rev)
        #self.gear_ratio = 12.75

#		#XXX gyro not plugged in
#		self.gyro = wpilib.ADXRS450_Gyro()
#		# This MUST occur while this doesn't move
#		# (It will take some initial measurements and assumes the robot is still
#		self.gyro.calibrate()
#		# init with gyroAngle and initialPose
#		gyro_angle = self.gyro.getAngle()
#		# initial_pose should be i form of (x position, y position, rotation)
#		initial_pose = (0, 0, 0)
#		#XXX missing the params for DifferentialDriveOdometry()
#		#self.drive_odometry = wpilib.kinematics.DifferentialDriveOdometry(
#			#gyro_angle, initial_pose)
#
#		# GUI CODE
#		#ui.send_button.clicked.connect(self.update_network_tables())

    def initDefaultCommand(self):
        self.setDefaultCommand(Do_Tank_Drive(self.robot))

    def set_tank_speed(self, left_joy, right_joy):
        # Get raw joystick inputs
        left_speed = left_joy.getY()
        right_speed = right_joy.getY()

        # Converts to proper speed values
        # NOTE: documentation claims different numbers than we have found to
        # work for the victor spx
        left_speed = (-1 * left_speed)
        right_speed = (-1 * right_speed)

        self.drive.tankDrive(left_speed, right_speed)
        #XXX remove prints eventually
        #print('DRIVETRAIN ENCODER: ' + str(self.left_encoder.get()))
        #print('DRIVETRAIN ENCODER: ' + str(self.right_encoder.get()))

    def stop_robot(self):
        self.drive.tankDrive(0, 0)
    def driveTrainInit(self, robot, controlScheme):
        self.controlScheme = controlScheme

        self.leftMotorGroup = wpilib.SpeedControllerGroup(
            ctre.WPI_TalonSRX(ports.talonPorts.get("leftChassisMotor")))
        self.rightMotorGroup = wpilib.SpeedControllerGroup(
            ctre.WPI_TalonSRX(ports.talonPorts.get("rightChassisMotor")))
        self.drive = DifferentialDrive(self.leftMotorGroup,
                                       self.rightMotorGroup)
        self.drive.setRightSideInverted(True)
Beispiel #30
0
class MyRobot(wpilib.TimedRobot):
    """This is a demo program showing how to use Gyro control with the
    DifferentialDrive class."""
    def robotInit(self):
        """Robot initialization function"""
        gyroChannel = 0  # analog input

        self.joystickChannel = 0  # usb number in DriverStation

        # channels for motors
        self.leftMotorChannel = 1
        self.rightMotorChannel = 0
        self.leftRearMotorChannel = 3
        self.rightRearMotorChannel = 2

        self.angleSetpoint = 0.0
        self.pGain = 1  # propotional turning constant

        # gyro calibration constant, may need to be adjusted
        # gyro value of 360 is set to correspond to one full revolution
        self.voltsPerDegreePerSecond = 0.0128

        self.left = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.leftMotorChannel),
            wpilib.Talon(self.leftRearMotorChannel))
        self.right = wpilib.SpeedControllerGroup(
            wpilib.Talon(self.rightMotorChannel),
            wpilib.Talon(self.rightRearMotorChannel),
        )
        self.myRobot = DifferentialDrive(self.left, self.right)

        self.gyro = wpilib.AnalogGyro(gyroChannel)
        self.joystick = wpilib.Joystick(self.joystickChannel)

    def teleopInit(self):
        """
        Runs at the beginning of the teleop period
        """
        self.gyro.setSensitivity(self.voltsPerDegreePerSecond
                                 )  # calibrates gyro values to equal degrees

    def teleopPeriodic(self):
        """
        Sets the gyro sensitivity and drives the robot when the joystick is pushed. The
        motor speed is set from the joystick while the RobotDrive turning value is assigned
        from the error between the setpoint and the gyro angle.
        """

        turningValue = (self.angleSetpoint - self.gyro.getAngle()) * self.pGain
        if self.joystick.getY() <= 0:
            # forwards
            self.myRobot.arcadeDrive(self.joystick.getY(), turningValue)
        elif self.joystick.getY() > 0:
            # backwards
            self.myRobot.arcadeDrive(self.joystick.getY(), -turningValue)