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()
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)
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())
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)
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()
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
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
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)
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)
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
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)
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()
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)
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 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)
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)
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)
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
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)
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)
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)
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)
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)
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)
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)