class MyRobot(wpilib.SampleRobot): # Defines the channels on the RoboRio that the motors are plugged into. There can be up to eight. FLChannel = 3 FRChannel = 4 RLChannel = 1 RRChannel = 2 # Defines the order that the sticks that are plugged in are assigned. DriveStickChannel = 0 # ExtraStickChannel = 1 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 autonomous(self): self.automodes.run() def operatorControl(self): # Enables the safety on the drive. Very important. DO NOT FORGET! self.drive.setSafetyEnabled(True) # Checks to see if the robot is activated and that operator control is active, so your robot does not move # when it is not supposed to. while self.isOperatorControl() and self.isEnabled(): # drives the robot with the arcade drive, which uses one joystick and is a bit easier to use. # It is a part of DifferentialDrive self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False) wpilib.Timer.delay(0.005)
class Arcade(Subsystem): """ Subsystem to control the motors for ArcadeDrive """ def __init__(self): super().__init__("Arcade") # motors and the channels they are connected to self.frontLeftMotor = wpilib.PWMVictorSPX(0) self.rearLeftMotor = wpilib.PWMVictorSPX(1) self.frontRightMotor = wpilib.PWMVictorSPX(2) self.rearRightMotor = wpilib.PWMVictorSPX(3) 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.drive.setSafetyEnabled(False) def speed(self, xSpeed, zRotation): self.drive.arcadeDrive(xSpeed, zRotation) def speed2(self, leftSpeed, rightSpeed): self.drive.tankDrive(leftSpeed, rightSpeed) def initDefaultCommand(self): self.setDefaultCommand(ThrottleMixer())
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
class DriveTrain(Subsystem): def __init__(self): # Ensures a single-time initialization Subsystem.__init__(self, "DriveTrain") # Front Motor Controllers self.front_cont_right = ctre.WPI_TalonSRX(3) self.rear_cont_right = ctre.WPI_TalonSRX(2) self.right = wpilib.SpeedControllerGroup(self.front_cont_right, self.rear_cont_right) # Rear Motor Controllers self.front_cont_left = ctre.WPI_TalonSRX(4) self.rear_cont_left = ctre.WPI_TalonSRX(5) self.left = wpilib.SpeedControllerGroup(self.front_cont_left, self.rear_cont_left) # Drive Type self.drive = DifferentialDrive(self.left, self.right) self.drive.setExpiration(0.1) # enable safety self.drive.setSafetyEnabled(False) def engageDrive(self, speed, rotation): self.drive.arcadeDrive(speed, rotation) #print(speed) def initDefaultCommand(self): self.setDefaultCommand(Drive())
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(TimedRobot): # Defines the channels that are used on the inputs. In the simulator, they have to match up with the physics.py file # This is really useful when you have a variable used hundreds of times and you want to have it set so you can # change it all in one go. RLMotorChannel = 2 RRMotorChannel = 0 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 # RobotInit is where all of the things we are using is initialized. 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) # Autonomous and teleop periodic both run the code on a fixed loop time. A part of TimedRobot. def autonomousPeriodic(self): # runs the autonomous modes when Autonomous is activated. self.automodes.run() def teleopPeriodic(self): # Turns on drive safety and checks to se if operator control and the robot is enabled. self.drive.setSafetyEnabled(True) if self.isOperatorControl() and self.isEnabled(): # Drives the robot with controller inputs. You can use buttons with self.DriveStick.getRawButton(ButtonNum). self.drive.arcadeDrive(self.DriveStick.getY(), -self.DriveStick.getX(), squareInputs=False)
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 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)
class MyRobot(wpilib.SampleRobot): def robotInit(self): self.frontLeft = wpilib.Talon(0) self.rearLeft = wpilib.Talon(1) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight = wpilib.Talon(2) self.rearRight = wpilib.Talon(3) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.drive = DifferentialDrive(self.left, self.right) self.stick2 = wpilib.Joystick(0) self.stick = wpilib.Joystick(1) def disabled(self): while self.isDisabled(): wpilib.Timer.delay(0.01) def operatorControl(self): timer = wpilib.Timer() timer.start() i = -1 while self.isOperatorControl() and self.isEnabled(): # if self.stick.getRawButton(2): # # self.drive.arcadeDrive(self.stick.getY(), self.stick.getX()) # self.drive.arcadeDrive(1, 0) # # self.drive.tankDrive(self.stick.getY() * -0.7, self.stick2.getY() * -0.7, True) # else: # # self.drive.arcadeDrive(self.stick.getY(), self.stick.getX()) # self.drive.arcadeDrive(1, 0) # # self.drive.tankDrive(self.stick.getY() * 0.7, self.stick2.getY() * 0.7, True) # # i = i * self.stick.getRawAxis(4) # # Move a motor with a Joystick self.drive.arcadeDrive(0.4, 0) if timer.hasPeriodPassed(1.0): print("Analog 8: %s" % self.ds.getBatteryVoltage()) wpilib.Timer.delay(0.02) def autonomous(self): timer = wpilib.Timer() timer.start() while self.isAutonomous() and self.isEnabled(): if timer.get() < 3.0: self.drive.tankDrive(-0.1, -1) else: self.drive.tankDrive(0., 0) wpilib.Timer.delay(0.01)
class Robot(TimedRobot): def robotInit(self): # Right Motors self.RightMotor1 = WPI_TalonFX(1) self.RightMotor2 = WPI_TalonFX(2) self.RightMotor3 = WPI_TalonFX(3) self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1, self.RightMotor2, self.RightMotor3) # Left Motors self.LeftMotor1 = WPI_TalonFX(4) self.LeftMotor2 = WPI_TalonFX(5) self.LeftMotor3 = WPI_TalonFX(6) self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1, self.LeftMotor2, self.LeftMotor3) self.drive = DifferentialDrive(self.leftDriveMotors, self.rightDriveMotors) self.testEncoder = Encoder(1, 2, 3) self.dashboard = NetworkTables.getTable("SmartDashboard") def disabledInit(self): pass def autonomousInit(self): pass def teleopInit(self): pass def testInit(self): pass def robotPeriodic(self): self.dashboard.putNumber("Encoder", self.testEncoder.getRate()) def disabledPeriodic(self): pass def autonomousPeriodic(self): self.drive.arcadeDrive(.5, 0) def teleopPeriodic(self): pass def testPeriodic(self): pass
class Robot(wpilib.TimedRobot): 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 four brushless motors with CAN IDs # 1, 2, 3, 4. Change these parameters to match your setup self.leftLeadMotor = rev.CANSparkMax(1, rev.MotorType.kBrushless) self.rightLeadMotor = rev.CANSparkMax(3, rev.MotorType.kBrushless) self.leftFollowMotor = rev.CANSparkMax(2, rev.MotorType.kBrushless) self.rightFollowMotor = rev.CANSparkMax(4, rev.MotorType.kBrushless) # Passing in the lead motors into DifferentialDrive allows any # commmands sent to the lead motors to be sent to the follower motors. self.driveTrain = DifferentialDrive(self.leftLeadMotor, self.rightLeadMotor) self.joystick = wpilib.Joystick(0) # 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.leftLeadMotor.restoreFactoryDefaults() self.rightLeadMotor.restoreFactoryDefaults() self.leftFollowMotor.restoreFactoryDefaults() self.rightFollowMotor.restoreFactoryDefaults() # In CAN mode, one SPARK MAX can be configured to follow another. This # is done by calling the follow() method on the SPARK MAX you want to # configure as a follower, and by passing as a parameter the SPARK MAX # you want to configure as a leader. # # This is shown in the example below, where one motor on each side of # our drive train is configured to follow a lead motor. self.leftFollowMotor.follow(self.leftLeadMotor) self.rightFollowMotor.follow(self.rightLeadMotor) def teleopPeriodic(self): # Drive with arcade style self.driveTrain.arcadeDrive(-self.joystick.getY(), self.joystick.getX())
class DriveArcade(wpilib.command.Command): """ Command to control drive using gamepad in arcade mode. """ diffDrive: DifferentialDrive gamePad: GenericHID nominal: float 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 tweak(self, val, scale): tweaked: float = DifferentialDrive.applyDeadband(val, self.deadband) if tweaked == 0.0: # User input value in deadband, just return 0.0 return tweaked tweaked *= scale * (1 - self.nominal) if tweaked > 0: tweaked = (tweaked * tweaked) + self.nominal else: tweaked = (tweaked * -tweaked) - self.nominal return tweaked def initialize(self): self.diffDrive.setSafetyEnabled(True) def execute(self): throttle: float = self.tweak(-self.gamePad.getY(GenericHID.Hand.kLeft), 1.0) rotation: float = self.tweak(self.gamePad.getX(GenericHID.Hand.kRight), 0.5) #print("Throttle", throttle, "Rotation", rotation) self.diffDrive.arcadeDrive(throttle, rotation, False) def isFinished(self): return False def interrupted(self): self.diffDrive.stopMotor() self.diffDrive.setSafetyEnabled(False) def end(self): self.interrupted()
class DriveTrain(Subsystem): 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 setDeadband(self, deadband): self.drive.deadband = deadband def move(self, spd, rot): #print("Spd" ,spd, "rot", rot) self.drive.arcadeDrive(spd, rot, True) #print("Left Position: %f"%(self.motors['leftMotor'].getSelectedSensorPosition(0))) #print("Right Position: %f"%(self.motors['rightMotor'].getSelectedSensorPosition(0))) def initDefaultCommand(self): self.setDefaultCommand(commands.driveControlled.DriveControlled()) def moveTank(self, left, right): self.drive.tankDrive(left, right, True)
class Drivetrain(SubsystemBase): def __init__(self, controller: XboxController): super().__init__() frontLeftMotor, backLeftMotor = PWMVictorSPX(0), PWMVictorSPX(1) frontRightMotor, backRightMotor = PWMVictorSPX(2), PWMVictorSPX(3) self.leftMotors = SpeedControllerGroup(frontLeftMotor, backLeftMotor) self.rightMotors = SpeedControllerGroup(frontRightMotor, backRightMotor) self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors) self.controller = controller def arcadeDrive(self): self.drivetrain.arcadeDrive( self.controller.getY(GenericHID.Hand.kLeftHand), self.controller.getX(GenericHID.Hand.kRightHand))
class DriveTrain(Subsystem): def __init__(self): super().__init__('DriveTrain') map=wpilib.command.Command.getRobot().robotMap #create motors here motors={} for name in map.CAN.driveMotors: motor=map.CAN.driveMotors[name] motors[name]=ctre.wpi_talonsrx.WPI_TalonSRX(motor['channel']) #motors[name]=ctre.wpi_talonsrx.WPI_TalonSRX(motor['channel']) #motors[name] = wpilib.PWMSpeedController(motor['channel']) motors[name].setInverted(motor['inverted']) self.drive = DifferentialDrive(motors['frontLeftMotor'],motors['frontRightMotor']) def move (self,x,y,rot): self.drive.arcadeDrive(x,y) def initDefaultCommand(self): self.setDefaultCommand(commands.driveControlled.DriveControlled())
class MyRobot(wpilib.IterativeRobot): def robotInit(self): '''Robot initialization function''' print("ROBORINIT") self.leftMotor = ctre.WPI_TalonSRX( 8) # initialize the motor as a Talon on channel 0 self.rightMotor = ctre.WPI_TalonSRX(4) self.stick = wpilib.XboxController( 0) # initialize the joystick on port 0 self.right = wpilib.SpeedControllerGroup(self.rightMotor) self.left = wpilib.SpeedControllerGroup(self.leftMotor) self.drive = DifferentialDrive(self.left, self.right) def teleopPeriodic(self): #self.drive.arcadeDrive(self.stick, True) print(self.stick.getX(GenericHID.Hand.kLeft)) self.drive.arcadeDrive(self.stick.getY(GenericHID.Hand.kLeft), self.stick.getX(GenericHID.Hand.kLeft), squaredInputs=True) print(self.stick.getX(GenericHID.Hand.kLeft))
class Driver(): 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) def driveRobotWithJoystick(self): speed = self.controlScheme.joystickDriveForward() * ( 1 - (0.95 * self.controlScheme.triggerSlowSpeed())) #speed = joystick.getY(GenericHID.Hand.kLeft)*(1-(0.75*joystick.getTriggerAxis(GenericHID.Hand.kRight))) rotation = self.alignWithVisionTargets( self.controlScheme.joystickTurn(), self.controlScheme.buttonVisionAlign()) * ( 1 - (0.95 * self.controlScheme.triggerSlowRotation())) #rotation = self.alignWithVisionTargets(joystick)*(1-(0.75*joystick.getTriggerAxis(GenericHID.Hand.kLeft))) self.drive.arcadeDrive(-speed, rotation, squareInputs=True) def alignWithVisionTargets(self, rotation, buttonVisionAlign): visionTable = NetworkTables.getTable('PiData') if (buttonVisionAlign): visionAlignment = visionTable.getNumber('yawToTarget', 0) if (visionAlignment < 0): rotation = -0.4 elif (visionAlignment > 0): rotation = 0.4 else: rotation = rotation return rotation
class MyRobot(wpilib.TimedRobot): RLMotorChannel = 1 RRMotorChannel = 2 FLMotorChannel = 3 FRMotorChannel = 4 DriveStickChannel = 0 def robotInit(self): RLMotor = wpilib.Spark(self.RLMotorChannel) RRMotor = wpilib.Spark(self.RRMotorChannel) FLMotor = wpilib.Spark(self.FLMotorChannel) FRMotor = wpilib.Spark(self.FRMotorChannel) self.Left = wpilib.SpeedControllerGroup(RLMotor, FLMotor) self.Right = wpilib.SpeedControllerGroup(RRMotor, FRMotor) self.DriveStick = wpilib.Joystick(self.DriveStickChannel) self.drive = DifferentialDrive(self.Left, self.Right) self.drive.setExpiration(0.1) def operatorControl(self): self.drive.setSafetyEnabled(True) while self.isEnabled() and self.isOperatorControl(): self.drive.arcadeDrive( self.DriveStick.getX(), self.DriveStick.getY(), squareInputs=True ) f
class MyRobot(wpilib.TimedRobot): """Main robot class""" # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty("/robot/autospeed", defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty("/robot/telemetry", defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 4096 PIDIDX = 0 def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) # Left front left_front_motor = ctre.WPI_TalonSRX(1) left_front_motor.setInverted(False) left_front_motor.setSensorPhase(False) self.left_front_motor = left_front_motor # Right front right_front_motor = ctre.WPI_TalonSRX(2) right_front_motor.setInverted(False) right_front_motor.setSensorPhase(False) self.right_front_motor = right_front_motor # Left rear -- follows front left_rear_motor = ctre.WPI_TalonSRX(3) left_rear_motor.setInverted(False) left_rear_motor.setSensorPhase(False) left_rear_motor.follow(left_front_motor) # Right rear -- follows front right_rear_motor = ctre.WPI_TalonSRX(4) right_rear_motor.setInverted(False) right_rear_motor.setSensorPhase(False) right_rear_motor.follow(right_front_motor) # # Configure drivetrain movement # self.drive = DifferentialDrive(left_front_motor, right_front_motor) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi) left_front_motor.configSelectedFeedbackSensor( left_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.l_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.l_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) right_front_motor.configSelectedFeedbackSensor( right_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.r_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.r_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) # # Configure gyro # # You only need to uncomment the below lines if you want to characterize wheelbase radius # Otherwise you can leave this area as-is self.gyro_getangle = lambda: 0 # Uncomment for the KOP gyro # Note that the angle from all implementors of the Gyro class must be negated because # getAngle returns a clockwise angle, and we require a counter-clockwise one # gyro = ADXRS450_Gyro() # self.gyro_getangle = lambda: -1 * gyro.getAngle() # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber("l_encoder_pos", self.l_encoder_getpos()) sd.putNumber("l_encoder_rate", self.l_encoder_getrate()) sd.putNumber("r_encoder_pos", self.r_encoder_getpos()) sd.putNumber("r_encoder_rate", self.r_encoder_getrate()) sd.putNumber("gyro_angle", self.gyro_getangle()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): """ If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. """ # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() l_motor_volts = self.left_front_motor.getMotorOutputVoltage() r_motor_volts = self.right_front_motor.getMotorOutputVoltage() gyro_angle = self.gyro_getangle() # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = ( now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate, gyro_angle, )
class MyRobot(wpilib.TimedRobot): 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)) def autonomousInit(self): self.myRobot.tankDrive(0.8, 0.8) def autonomousPeriodic(self): self.myRobot.tankDrive(1, 0.5) def teleopInit(self): """Executed at the start of teleop mode""" self.myRobot.setSafetyEnabled(True) def setCenters(self, speed_value): self.center1.set(-speed_value) self.center2.set(speed_value) def deadzone(self, val, deadzone): if abs(val) < deadzone: return 0 return val def teleopPeriodic(self): ballMotorSetPoint = 0 if self.driver.getBumper(self.LEFT): ballMotorSetPoint = 1.0 elif self.driver.getBumper(self.RIGHT): ballMotorSetPoint = -1.0 else: ballMotorSetPoint = 0.0 self.ballManipulator.set(ballMotorSetPoint) """Runs the motors with tank steering""" #right = self.driver.getY(self.RIGHT) #left = self.driver.getY(self.LEFT) #self.myRobot.tankDrive(right, left) forward = -self.driver.getRawAxis(5) rotation_value = rotation_value = self.driver.getX(LEFT_HAND) forward = deadzone(forward, 0.2) self.myRobot.arcadeDrive(forward, rotation_value) center_speed = self.driver.getX(self.RIGHT) self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class MyRobot(wpilib.IterativeRobot): def robotInit(self): """ This function is called upon program startup and should be used for any initialization code. """ # joystick 1 on the driver station self.stick = wpilib.XboxController(0) self.driverStation = wpilib.DriverStation self.frontRight = ctre.wpi_talonsrx.WPI_TalonSRX(3) self.rearRight = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.frontLeft = ctre.wpi_talonsrx.WPI_TalonSRX(4) self.rearLeft = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight.setExpiration(0.2) self.rearRight.setExpiration(0.2) self.frontRight.setExpiration(0.2) self.rearLeft.setExpiration(0.2) self.drive = DifferentialDrive(self.left, self.right) # initialize motors other than drive self.intakeRight = wpilib.VictorSP(0) self.elevator = ctre.wpi_talonsrx.WPI_TalonSRX( 5) # Talon SRX controller with CAN address 5 self.intakeLeft = wpilib.VictorSP(2) self.battleAxe = wpilib.VictorSP(3) self.actuator = wpilib.Spark(4) self.axeExtender = wpilib.Spark(5) ###################################### self.encoderTicksPerInch = 1159 self.elevator.setQuadraturePosition(0, 0) self.elevator.configForwardSoftLimitThreshold( int(round(-0.1 * self.encoderTicksPerInch)), 10) self.elevator.configReverseSoftLimitThreshold( int(round(-39.5 * self.encoderTicksPerInch)), 10) self.elevator.configForwardSoftLimitEnable(True, 10) self.elevator.configReverseSoftLimitEnable(True, 10) self.elevator.configPeakOutputForward(0.8, 10) self.elevator.configPeakOutputReverse(-1, 10) self.elevator.set(ctre.ControlMode.Position, 0) self.elevator.selectProfileSlot(0, 0) self.elevator.config_kF(0, 0, 10) self.elevator.config_kP(0, 0.6, 10) self.elevator.config_kI(0, 0.003, 10) self.elevator.config_kD(0, 0, 10) self.elevator.config_IntegralZone(0, 100, 10) self.elevator.configAllowableClosedloopError( 0, int(round(0.01 * self.encoderTicksPerInch)), 10) # initialize limit switches and hall-effect sensors self.actuatorSwitchMin = wpilib.DigitalInput(0) self.actuatorSwitchMax = wpilib.DigitalInput(1) self.battleAxeSwitchUp = wpilib.DigitalInput(2) self.battleAxeSwitchDown = wpilib.DigitalInput(3) self.battleAxeExtenderSwitch = wpilib.DigitalInput(4) self.elevatorZeroSensor = wpilib.DigitalInput(5) self.powerDistributionPanel = wpilib.PowerDistributionPanel() self.powerDistributionPanel.resetTotalEnergy() # # Communicate w/navX MXP via the MXP SPI Bus. # - Alternatively, use the i2c bus. # See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details # self.navxSensor = navx.AHRS.create_spi() # self.navxSensor = navx.AHRS.create_i2c() # Items in this dictionary are available in your autonomous mode # as attributes on your autonomous object self.components = { 'drive': self.drive, 'navxSensor': self.navxSensor, 'actuator': self.actuator, 'actuatorSwitchMin': self.actuatorSwitchMin, 'actuatorSwitchMax': self.actuatorSwitchMax, 'elevator': self.elevator, 'intakeRight': self.intakeRight, 'intakeLeft': self.intakeLeft, 'gameData': self.driverStation.getInstance() } # * The first argument is the name of the package that your autonomous # modes are located in # * The second argument is passed to each StatefulAutonomous when they # start up self.automodes = AutonomousModeSelector('autonomous', self.components) NetworkTables.initialize() self.sd = NetworkTables.getTable("SmartDashboard") wpilib.CameraServer.launch('vision.py:main') def autonomousInit(self): """This function is run once each time the robot enters autonomous mode.""" gameData = self.driverStation.getInstance().getGameSpecificMessage() if gameData[0] == 'L': self.sd.putString('gameData1', "Left") elif gameData[0] == 'R': self.sd.putString('gameData1', "Right") if gameData[1] == 'L': self.sd.putString('gameData2', "Left") elif gameData[1] == 'R': self.sd.putString('gameData2', "Right") if gameData[2] == 'L': self.sd.putString('gameData3', "Left") elif gameData[2] == 'R': self.sd.putString('gameData3', "Right") def autonomousPeriodic(self): """This function is called periodically during autonomous.""" self.automodes.run() def teleopInit(self): wpilib.IterativeRobot.teleopInit(self) self.setRamp = False self.rampState = False self.battleAxeUp = 0 self.battleAxeDown = 0 self.actuatorSpeedyIn = 0 self.actuatorSpeedyOut = 0.3 self.actuatorCount = 0 self.startClimb = False self.climbMode = False self.climbRobot = False self.enableSequence1 = True self.enableSequence2 = True self.navxSensor.reset() self.minPosition = 0 self.drivePosition = -11 self.climbPosition = -32 self.maxPosition = -39.5 self.positionSelector = 1 self.powerDistributionPanel.resetTotalEnergy() def teleopPeriodic(self): """This function is called periodically during operator control.""" leftXAxis = self.stick.getRawAxis(0) * 0.8 leftYAxis = self.stick.getRawAxis(1) * -0.8 # Get joystick value # leftXAxis = 0 # leftYAxis = 0 # if leftYAxis >= 0.25 or leftYAxis <= -0.25: # if leftYAxis <= -0.65: # leftYAxis = -0.65 # self.setRamp = True # else: # self.setRamp = False # # if self.setRamp != self.rampState: # if self.setRamp is True: # self.frontRight.configOpenLoopRamp(1, 0) # self.rearRight.configOpenLoopRamp(1, 0) # self.frontRight.configOpenLoopRamp(1, 0) # self.rearLeft.configOpenLoopRamp(1, 0) # self.rampState = True # else: # self.frontRight.configOpenLoopRamp(0, 0) # self.rearRight.configOpenLoopRamp(0, 0) # self.frontRight.configOpenLoopRamp(0, 0) # self.rearLeft.configOpenLoopRamp(0, 0) # self.rampState = False self.frontRight.configOpenLoopRamp(0, 0) self.rearRight.configOpenLoopRamp(0, 0) self.frontRight.configOpenLoopRamp(0, 0) self.rearLeft.configOpenLoopRamp(0, 0) self.drive.arcadeDrive(leftYAxis, leftXAxis, squaredInputs=True) self.sdUpdate() rightYAxis = self.stick.getRawAxis(5) rightYAxis = self.normalize(rightYAxis, 0.15) # Set deadzone if -0.15 < self.stick.getRawAxis(5) < 0.15 and self.climbMode is False: if self.elevator.getQuadraturePosition() < ( self.drivePosition + 1) * self.encoderTicksPerInch: self.enableSequence1 = False self.positionSelector = 2 elif -0.15 < self.stick.getRawAxis( 5) < 0.15 and self.climbMode is True: self.positionSelector = 3 self.battleAxeUp = self.stick.getRawAxis(2) * -0.35 self.battleAxeDown = self.stick.getRawAxis(3) * 0.50 if self.battleAxeSwitchUp.get() is True: self.battleAxeUp = 0 if self.battleAxeSwitchDown.get() is True: self.battleAxeDown = 0 self.battleAxe.set(self.battleAxeUp + self.battleAxeDown) else: self.positionSelector = 0 if self.stick.getRawAxis(2) > 0.1 and self.climbMode is False: self.positionSelector = 5 self.elevator.set(ctre.ControlMode.PercentOutput, self.stick.getRawAxis(2)) self.intakeRight.set(-1) self.intakeLeft.set(-1) else: self.intakeRight.set(0) self.intakeLeft.set(0) if self.stick.getRawAxis(3) > 0.1 and self.climbMode is False: self.intakeRight.set(self.stick.getRawAxis(3)) self.intakeLeft.set(self.stick.getRawAxis(3)) if self.climbRobot is True: self.positionSelector = 1 if self.elevator.getQuadraturePosition( ) > -2 * self.encoderTicksPerInch: self.elevator.configPeakOutputForward(0.2, 10) self.elevator.configPeakOutputReverse(-0.5, 10) elif self.elevator.getQuadraturePosition( ) < -37.5 * self.encoderTicksPerInch: self.elevator.configPeakOutputForward(0.8, 10) self.elevator.configPeakOutputReverse(-0.5, 10) else: self.elevator.configPeakOutputForward(0.8, 10) self.elevator.configPeakOutputReverse(-1, 10) if self.positionSelector is 0: self.elevator.set(rightYAxis) elif self.positionSelector is 1: self.elevator.set( ctre.ControlMode.Position, int(round(self.minPosition * self.encoderTicksPerInch))) elif self.positionSelector is 2: self.elevator.set( ctre.ControlMode.Position, int(round(self.drivePosition * self.encoderTicksPerInch))) elif self.positionSelector is 3: self.elevator.set( ctre.ControlMode.Position, int(round(self.climbPosition * self.encoderTicksPerInch))) elif self.positionSelector is 4: self.elevator.set( ctre.ControlMode.Position, int(round(self.maxPosition * self.encoderTicksPerInch))) else: pass if self.elevatorZeroSensor.get() is False: self.elevator.setQuadraturePosition(0, 0) if self.stick.getRawButton(1) is True and self.stick.getRawButton( 7) is True: # A and start button self.startClimb = True if self.stick.getRawButton(3) is True: # X button if self.actuatorCount < 20 and self.elevator.getQuadraturePosition() < \ (self.climbPosition + 1) * self.encoderTicksPerInch: self.actuatorSpeedyIn = -0.5 self.actuatorSpeedyOut = 0 else: self.actuatorSpeedyIn = 0 self.actuatorSpeedyOut = 0 self.actuatorCount += 1 elif self.stick.getRawButton( 3) is False and self.actuatorCount is not 0: self.actuatorSpeedyIn = 0 self.actuatorSpeedyOut = 0.3 self.actuatorCount = 0 if self.battleAxeSwitchUp.get() is True: self.battleAxeUp = 0 if self.startClimb is True: self.actuatorSpeedyIn = -0.45 if self.actuatorSwitchMin.get() is False: self.actuatorSpeedyIn = 0 self.climbMode = True if self.climbMode is False and self.enableSequence1 is False and self.enableSequence2 is False and\ self.startClimb is False: self.battleAxeUp = -0.3 if self.actuatorSwitchMax.get() is False: self.actuatorSpeedyOut = 0 if self.climbMode is False: self.battleAxeDown = 0.2 self.actuator.set(self.actuatorSpeedyIn + self.actuatorSpeedyOut) if self.stick.getRawButton(2) is True and self.stick.getRawButton( 7) is True and self.climbMode is True: self.climbRobot = True if self.startClimb is True: self.battleAxeUp = -0.35 self.battleAxeDown = 0 if self.battleAxeSwitchDown.get() is True: self.battleAxeDown = 0 if self.climbMode is False: self.battleAxe.set(self.battleAxeUp + self.battleAxeDown) if self.stick.getRawButton(6) is True: axeOut = 1 axeIn = 0 elif self.stick.getRawButton(5) is True: axeOut = 0 axeIn = -1 else: axeOut = 0 axeIn = 0 # if self.enableSequence2 is True: # axeIn = -1 # if self.battleAxeExtenderSwitch.get() is False: # axeIn = 0 # self.enableSequence2 = False self.axeExtender.set(axeOut + axeIn) def testInit(self): self.actuatorIn = 0 self.actuatorOut = 0 def testPeriodic(self): """This function is called periodically during test mode.""" self.actuatorIn = (self.stick.getRawAxis(2) * -0.5) self.actuatorOut = (self.stick.getRawAxis(3) * 0.5) # if self.actuatorSwitchMin.get() is False: # self.actuatorIn = 0 # if self.actuatorSwitchMax.get() is False: # self.actuatorOut = 0 self.axeExtender.set(self.actuatorIn + self.actuatorOut) # self.battleAxe.set((self.stick.getRawAxis(2) * -0.5) + (self.stick.getRawAxis(3) * 0.5)) self.sdUpdate() def normalize(self, joystickInput, deadzone): """joystickInput should be between -1 and 1, deadzone should be between 0 and 1.""" if joystickInput > 0: if (joystickInput - deadzone) < 0: return 0 else: return (joystickInput - deadzone) / (1 - deadzone) elif joystickInput < 0: if (joystickInput + deadzone) > 0: return 0 else: return (joystickInput + deadzone) / (1 - deadzone) else: return 0 def sdUpdate(self): self.sd.putNumber('robot/time', wpilib.Timer.getMatchTime()) self.sd.putNumber('drive/navx/yaw', self.navxSensor.getYaw()) self.sd.putNumber('robot/elevator/encoder', self.elevator.getQuadraturePosition()) self.sd.putNumber('robot/elevator/motorPercent', self.elevator.getMotorOutputPercent()) self.sd.putNumber('robot/totalCurrent', self.powerDistributionPanel.getTotalCurrent()) self.sd.putNumber('robot/totalEnergy', self.powerDistributionPanel.getTotalEnergy()) self.sd.putNumber('robot/totalPower', self.powerDistributionPanel.getTotalPower()) if wpilib.DriverStation.getInstance().getAlliance( ) is wpilib.DriverStation.Alliance.Red: theme = "red" self.sd.putString('theme', theme) elif wpilib.DriverStation.getInstance().getAlliance( ) is wpilib.DriverStation.Alliance.Blue: theme = "blue" self.sd.putString('theme', theme) self.sd.putBoolean('example_variable', self.battleAxeSwitchUp.get())
class DriveTrain(Subsystem): """Operate the drivetrain.""" 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) def driveManual(self, xboxcontroller): #self.leftB.follow(self.left, followerType=0) #self.rightB.follow(self.right, followerType=0) #TODO: I'm not sure if these followers should be on or not. Let's find that out. self.drive.arcadeDrive(-xboxcontroller.getY(wpilib.interfaces._interfaces.GenericHID.Hand.kLeftHand), xboxcontroller.getX(wpilib.interfaces._interfaces.GenericHID.Hand.kLeftHand)) self.leftB.follow(self.left) self.rightB.follow(self.right) def driveReverse(self): self.drive.tankDrive(-.5,-.5) def driveForward(self): self.drive.tankDrive(.5,.5) def stopDriving(self): self.drive.tankDrive(0,0) def getHeading(self): """Get the robot's heading in degrees""" return self.gyro.getAngle() def reset(self): """Reset the robots sensors to the zero states.""" self.gyro.reset() self.left_encoder.reset() self.right_encoder.reset() def getDistance(self): """Get the current distance driven. :returns: The distance driven (average of left and right encoders)""" return ( self.left_encoder.getDistance().__init__() ) / 2.0
class DriveTrain(Subsystem): """Operate the drivetrain.""" def __init__(self, robot): """Save the robot object, and assign and save hardware ports connected to the drive motors.""" super().__init__() self.robot = robot # The digital gyro plugged into the SPI port on RoboRIO self.gyro = wpilib.ADXRS450_Gyro() # Motors used for driving self.left = ctre.WPI_TalonSRX(1) self.leftB = ctre.WPI_TalonSRX(2) self.right = ctre.WPI_TalonSRX(3) self.rightB = ctre.WPI_TalonSRX(4) # 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) def driveManual(self, left, right): """Tank style driving for the DriveTrain. :param left: Speed in range [-1, 1] :param right: Speed in range [-1, 1] """ self.leftB.follow(self.left, followerType=0) self.rightB.follow(self.right, followerType=0) self.drive.arcadeDrive(left, right) def getHeading(self): """Get the robot's heading in degrees""" return self.gyro.getAngle() def reset(self): """Reset the robots sensors to the zero states.""" self.gyro.reset() self.left_encoder.reset() self.right_encoder.reset() def getDistance(self): """Get the current distance driven. :returns: The distance driven (average of left and right encoders)""" return ( self.left_encoder.getDistance().__init__() ) / 2.0
class MyRobot(wpilib.TimedRobot): '''Main robot class''' # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty('/robot/autospeed', defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty('/robot/telemetry', defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 360 def robotInit(self): '''Robot-wide initialization code should go here''' self.lstick = wpilib.Joystick(0) left_front_motor = wpilib.Spark(1) left_front_motor.setInverted(False) right_front_motor = wpilib.Spark(2) right_front_motor.setInverted(False) left_rear_motor = wpilib.Spark(3) left_rear_motor.setInverted(False) right_rear_motor = wpilib.Spark(4) right_rear_motor.setInverted(False) # # Configure drivetrain movement # l = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor) r = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor) self.drive = DifferentialDrive(l, r) self.drive.setSafetyEnabled(False) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ( 1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi l_encoder = wpilib.Encoder(0, 1) l_encoder.setDistancePerPulse(encoder_constant) self.l_encoder_getpos = l_encoder.getDistance self.l_encoder_getrate = l_encoder.getRate r_encoder = wpilib.Encoder(2, 3) r_encoder.setDistancePerPulse(encoder_constant) self.r_encoder_getpos = r_encoder.getDistance self.r_encoder_getrate = r_encoder.getRate # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber('l_encoder_pos', self.l_encoder_getpos()) sd.putNumber('l_encoder_rate', self.l_encoder_getrate()) sd.putNumber('r_encoder_pos', self.r_encoder_getpos()) sd.putNumber('r_encoder_rate', self.r_encoder_getrate()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): ''' If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. ''' # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() motor_volts = battery * abs(self.prior_autospeed) l_motor_volts = motor_volts r_motor_volts = motor_volts # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = (now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate)
class Drivetrain(SubsystemBase): def __init__(self): super().__init__() # Create the motor controllers and their respective speed controllers. self.leftMotors = SpeedControllerGroup( PWMSparkMax(constants.kLeftMotor1Port), PWMSparkMax(constants.kLeftMotor2Port), ) self.rightMotors = SpeedControllerGroup( PWMSparkMax(constants.kRightMotor1Port), PWMSparkMax(constants.kRightMotor2Port), ) # Create the differential drivetrain object, allowing for easy motor control. self.drive = DifferentialDrive(self.leftMotors, self.rightMotors) # Create the encoder objects. self.leftEncoder = Encoder( constants.kLeftEncoderPorts[0], constants.kLeftEncoderPorts[1], constants.kLeftEncoderReversed, ) self.rightEncoder = Encoder( constants.kRightEncoderPorts[0], constants.kRightEncoderPorts[1], constants.kRightEncoderReversed, ) # Configure the encoder so it knows how many encoder units are in one rotation. self.leftEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) self.rightEncoder.setDistancePerPulse( constants.kEncoderDistancePerPulse) # Create the gyro, a sensor which can indicate the heading of the robot relative # to a customizable position. self.gyro = ADXRS450_Gyro() # Create the an object for our odometry, which will utilize sensor data to # keep a record of our position on the field. self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d()) # Reset the encoders upon the initilization of the robot. self.resetEncoders() def periodic(self): """ Called periodically when it can be called. Updates the robot's odometry with sensor data. """ self.odometry.update( self.gyro.getRotation2d(), self.leftEncoder.getDistance(), self.rightEncoder.getDistance(), ) def getPose(self): """Returns the current position of the robot using it's odometry.""" return self.odometry.getPose() def getWheelSpeeds(self): """Return an object which represents the wheel speeds of our drivetrain.""" speeds = DifferentialDriveWheelSpeeds(self.leftEncoder.getRate(), self.rightEncoder.getRate()) return speeds def resetOdometry(self, pose): """ Resets the robot's odometry to a given position.""" self.resetEncoders() self.odometry.resetPosition(pose, self.gyro.getRotation2d()) def arcadeDrive(self, fwd, rot): """Drive the robot with standard arcade controls.""" self.drive.arcadeDrive(fwd, rot) def tankDriveVolts(self, leftVolts, rightVolts): """Control the robot's drivetrain with voltage inputs for each side.""" # Set the voltage of the left side. self.leftMotors.setVoltage(leftVolts) # Set the voltage of the right side. It's # inverted with a negative sign because it's motors need to spin in the negative direction # to move forward. self.rightMotors.setVoltage(-rightVolts) # Resets the timer for this motor's MotorSafety self.drive.feed() def resetEncoders(self): """Resets the encoders of the drivetrain.""" self.leftEncoder.reset() self.rightEncoder.reset() def getAverageEncoderDistance(self): """ Take the sum of each encoder's traversed distance and divide it by two, since we have two encoder values, to find the average value of the two. """ return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2 def getLeftEncoder(self): """Returns the left encoder object.""" return self.leftEncoder def getRightEncoder(self): """Returns the right encoder object.""" return self.rightEncoder def setMaxOutput(self, maxOutput): """Set the max percent output of the drivetrain, allowing for slower control.""" self.drive.setMaxOutput(maxOutput) def zeroHeading(self): """Zeroes the gyro's heading.""" self.gyro.reset() def getHeading(self): """Return the current heading of the robot.""" return self.gyro.getRotation2d().getDegrees() def getTurnRate(self): """Returns the turning rate of the robot using the gyro.""" # The minus sign negates the value. return -self.gyro.getRate()
class driveTrain(): def __init__(self, robot): self.operate = operatorFunctions(drive=self,robot=robot) self.gyro = AHRS.create_spi() #self.gyro = wpilib.interfaces.Gyro() """Sets drive motors to a cantalon or victor""" self.instantiateMotors() self.instantiateEncoders() #self.encoderReset() #self.driveBase = arcadeDrive() self.shifter = wpilib.DoubleSolenoid(51, 0, 1)#Initilizes the shifter's solenoid and sets it to read fron digital output 0 self.lfMotor = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.lbMotor = ctre.wpi_victorspx.WPI_VictorSPX(11) self.rfMotor = ctre.wpi_victorspx.WPI_VictorSPX(9) self.rbMotor = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.lfMotor.setNeutralMode(2) self.lbMotor.setNeutralMode(2) self.rfMotor.setNeutralMode(2) self.rbMotor.setNeutralMode(2) self.left = wpilib.SpeedControllerGroup(self.lfMotor, self.lbMotor) self.right = wpilib.SpeedControllerGroup(self.rfMotor, self.rbMotor) self.drive = DifferentialDrive(self.left, self.right) self.firstTime = True#Check for autonDriveStraight self.firstRun = True#Check for autonPivot self.resetFinish = False#Check for encoder reset self.setWheelCircumference() self.oldGyro = 0 self.moveNumber = 1 self.shiftCounter = 0 def setWheelCircumference(self): self.wheelCircumference = 18.84954 def instantiateEncoders(self): self.lfMotor.configSelectedFeedbackSensor(0, 0, 0) self.rbMotor.configSelectedFeedbackSensor(0, 0, 0) self.lfMotor.setSensorPhase(True) def instantiateMotors(self): self.lfMotor = ctre.wpi_talonsrx.WPI_TalonSRX(2) self.lbMotor = ctre.wpi_victorspx.WPI_VictorSPX(11) self.rfMotor = ctre.wpi_victorspx.WPI_VictorSPX(9) self.rbMotor = ctre.wpi_talonsrx.WPI_TalonSRX(1) self.lfMotor.setNeutralMode(2) self.lbMotor.setNeutralMode(2) self.rfMotor.setNeutralMode(2) self.rbMotor.setNeutralMode(2) def drivePass(self, leftY, rightX, leftBumper, rightBumper, aButton): self.arcadeDrive(leftY, rightX) self.shift(leftBumper, rightBumper) self.manualEncoderReset(aButton) pass def scaleInputs(self, leftY, rightX): if abs(leftY) < .05: return .75 * rightX rightX = (-(math.log10((2*(abs(leftY)))+1)-1)*rightX) return rightX def printer(self): print('Why does Hescott look so much like shrek?') def arcadeDrive(self, leftY, rightX): #self.drive.arcadeDrive(leftY * -.82, self.scaleInputs(leftY, rightX) * .82) #print((-1 * self.scaleInputs(leftY, rightX))) #self.drive.arcadeDrive((-1 * self.scaleInputs(leftY, rightX)), (rightX/2)) self.drive.arcadeDrive(leftY * -.82, rightX * .82) def shift(self, leftBumper, rightBumper): #print(self.shifter.get()) if leftBumper:#When left bumper is pressed we shift gears if self.shifter.get() == 1:#Checks to see what gear we are in and shifts accordingly self.shifter.set(2) #print("shifting left") if rightBumper: if self.shifter.get() == 2 or self.shifter.get() == 0: self.shifter.set(1) #print("shifting right") def getGyroAngle(self): return (self.gyro.getAngle()-self.oldGyro) def zeroGyro(self): self.gyro.reset() while(abs(self.getGyroAngle()) > 340): self.gyro.reset() def encoderReset(self): self.lfMotor.setQuadraturePosition(0, 0) self.rbMotor.setQuadraturePosition(0, 0) #print("Encoders Reset") def printEncoderPosition(self): lfEncoder = -(self.lfMotor.getQuadraturePosition()) rbEncoder = self.rbMotor.getQuadraturePosition() averageEncoders = (lfEncoder + rbEncoder) / 2 #print(averageEncoders) #print(rbEncoder) def manualEncoderReset(self, aButton): if aButton: self.lfMotor.setQuadraturePosition(0, 0) self.rbMotor.setQuadraturePosition(0, 0) else: pass def autonShift(self, gear): if gear == 'low': if self.shifter.get() == 1 or self.shifter.get() == 0: self.shifter.set(2) #print('Shift to low') #return False elif gear == 'high': if self.shifter.get() == 2 or self.shifter.get() == 0: self.shifter.set(1) #print('Shift to high') #return False else: pass def autonPivot(self, turnAngle, turnSpeed): slowDownSpeed = .14 correctionDeadzone = .5 if self.firstRun: self.oldGyro = self.gyro.getAngle() self.firstRun = False turnSpeed -= (2*turnSpeed/(1+math.exp(0.045*(-1 if turnAngle>0 else 1)*(-turnAngle + self.getGyroAngle())))) turnSpeed = max(turnSpeed, slowDownSpeed) #turnSpeed = 0.15 ''' if abs(turnAngle - self.getGyroAngle()) < 25: #print('Gyro Angle:'+str(self.getGyroAngle())) #print('Turn Speed:'+str(turnSpeed)) turnSpeed = slowDownSpeed ''' if turnAngle < 0: if abs(turnAngle - self.getGyroAngle()) > correctionDeadzone: if self.getGyroAngle() >= turnAngle: self.drive.tankDrive(-(turnSpeed) * .82, (turnSpeed) * .82,False) #print('Turning Left') return True elif self.getGyroAngle() <= turnAngle: self.drive.tankDrive(.2, -.2) return True else: pass else: #print("Done Turning Left") print(self.getGyroAngle()) self.drive.stopMotor() self.firstRun = True return False elif turnAngle > 0: if abs(turnAngle - self.getGyroAngle()) > correctionDeadzone: if self.getGyroAngle() <= turnAngle: #print('Turning Right') self.drive.tankDrive((turnSpeed) * .82, -(turnSpeed) * .82,False) return True elif self.getGyroAngle() >= turnAngle: self.drive.tankDrive(-.2, .2) return True else: pass else: #print('Done Turning Right') #print(self.getGyroAngle()) self.drive.stopMotor() self.firstRun = True return False def autonDriveStraight(self, speed, distance): #print('entered auton straight') lSpeed = speed rSpeed = speed encoderDistance = (distance / self.wheelCircumference) * 5887#Figueres out how far to spin the wheels in encoder codes, 265 is how many pins on current encoders #print('Encoder Distance' + str(encoderDistance)) if self.firstTime:#Checks for first time through the function to only reset encoders on the first time #print('passed first check')#Debugging #self.encoderReset()#Resets encoders self.oldGyro = self.gyro.getAngle() self.oldPositionLeft = -(self.lfMotor.getQuadraturePosition()) self.oldPositionRight = self.rbMotor.getQuadraturePosition() self.autonCounter = 0 self.firstTime = False self.lfEncoderPosition = -(self.lfMotor.getQuadraturePosition()) - self.oldPositionLeft self.rbEncoderPosition = self.rbMotor.getQuadraturePosition() - self.oldPositionRight #print(self.lfEncoderPosition) #print(self.rbEncoderPosition) averageEncoders = (self.lfEncoderPosition + self.rbEncoderPosition) / 2 #print('Average Encodes' + str(averageEncoders)) ''' if averageEncoders > 250 and not self.resetFinish: #self.encoderReset() self.printEncoderPosition() return True else: if not self.resetFinish: print(self.lfEncoderPosition) print(self.rbEncoderPosition) #print('Encoder Reset Finished') self.resetFinish = True ''' if averageEncoders < encoderDistance and self.autonCounter == 0: speedAdjustment = .05 slowDownSpeed = .25 gyroAngle = self.getGyroAngle(); speedAdjustment /= 1+math.exp(-gyroAngle) speedAdjustment -= 0.025 rSpeed += speedAdjustment lSpeed -= speedAdjustment ''' if self.getGyroAngle() < -1: rSpeed = rSpeed - speedAdjustment elif self.getGyroAngle() > 1: lSpeed = lSpeed - speedAdjustment ''' if averageEncoders > encoderDistance - 500: lSpeed = slowDownSpeed rSpeed = slowDownSpeed #print('Slowing Down') self.drive.tankDrive(lSpeed * .82, rSpeed * .82,False) return True else: if self.autonCounter < 4: #print('Active Breaking') self.drive.tankDrive(-.15 * .82, -.15 * .82,False) self.autonCounter = self.autonCounter + 1 return True else: #print('EndLoop') self.resetFinish = False self.firstTime = True self.drive.stopMotor() #print(self.lfEncoderPosition) print(self.rbEncoderPosition) return False ''' def autonAngledTurn(self, turnAngle):#Angle is in degrees ROBOT_WIDTH = 24.3 def getSpeeds(self, angle, radius, speed=1): return [speed, speed*(lambda x: x[1]/x[0])(getDistances(angle, radius))] def getDistances(self, angle, radius): return [(radius + ROBOT_WIDTH/2)*math.radians(angle), (radius - ROBOT_WIDTH/2)*math.radians(angle) ] ''' def autonMove(self, moveNumberPass, commandNumber, speed, distance, turnAngle, turnSpeed): if moveNumberPass == self.moveNumber: #print(self.moveNumber) if commandNumber == 0: if self.autonDriveStraight(speed, distance): pass else: #print(self.getGyroAngle()) #print('Move ' + str(moveNumberPass) + ' Complete') self.moveNumber = moveNumberPass + 1 elif commandNumber == 1: if self.autonPivot(turnAngle, turnSpeed): pass else: #print(self.getGyroAngle()) #print('Move ' + str(moveNumberPass) + ' Complete') self.moveNumber = moveNumberPass + 1 elif commandNumber == 2: pass else: #print('3rd pass') pass def resetMoveNumber(self): self.moveNumber = 1
class MyRobot(wpilib.TimedRobot): def robotInit(self): """Robot initialization function""" self.frontLeft = wpilib.Spark(1) self.rearLeft = wpilib.Spark(2) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.frontRight = wpilib.Spark(3) self.rearRight = wpilib.Spark(4) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.drive = DifferentialDrive(self.left, self.right) # object that handles basic drive operations #self.myRobot = wpilib.Drive.DifferentialDrive(0, 1) #self.myRobot.setExpiration(0.1) # joystick #0 self.stick = wpilib.Joystick(0) def disabledInit(self): self.drive.setSafetyEnabled(False) def disabledPeriodic(self): self.drive.setSafetyEnabled(False) def robotPeriodic(self): self.drive.setSafetyEnabled(False) def testInit(self): print("Test Init") def testPeriodic(self): if (self.stick.getRawButtonPressed(1) == True): print("Test Button 1 Pressed.") def autonomousInit(self): self.drive.setSafetyEnabled(False) def autonomousPeriodic(self): """Called when autonomous mode is enabled""" while self.isAutonomous() and self.isEnabled(): #wpilib.Timer.delay(0.01) self.drive.arcadeDrive(0.20, 0.1) def teleopInit(self): """Executed at the start of teleop mode""" self.drive.setSafetyEnabled(False) def teleopPeriodic(self): """Runs the motors with tank steering""" # timer = wpilib.Timer() # timer.start() while self.isOperatorControl() and self.isEnabled(): # Move a motor with a Joystick #wpilib.Timer.delay(0.02) self.drive.arcadeDrive(self.stick.getRawButton(1), True)
class Apollo(wpilib.TimedRobot): 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 reset(self): self.drive_rev = False while self.lift_motor_speed > 0: self.lift_motor_speed -= 0.002 self.lift_motor_group.set(self.lift_motor_speed) if self.lift_motor_speed < 0: self.lift_motor_speed = 0 self.lift_motor_group.set(self.lift_motor_speed) self.front_speed = 0 self.front_motor_group.set(self.front_speed) self.lock_controls = False self.hatch_solenoid.set(0) self.encoder.reset() def drive_control(self): lh_y = self.joystick.getY(self.joystick.Hand.kLeft) * (1 / 2) rh_x = self.joystick.getX(self.joystick.Hand.kRight) * (1 / 2) if self.joystick.getStickButtonPressed(self.joystick.Hand.kLeft) or \ self.joystick.getStickButtonPressed(self.joystick.Hand.kRight): self.drive_rev = not self.drive_rev if self.drive_rev: self.drive.arcadeDrive(lh_y, rh_x, True) else: self.drive.arcadeDrive(lh_y * -1, rh_x, True) def lift_control(self): low_volt = 0.2 if self.joystick.getYButtonPressed(): self.lock_controls = not self.lock_controls if not self.lock_controls: if self.joystick.getTriggerAxis(self.joystick.Hand.kLeft) > 0.9: # self.lift_motor_speed = 0.6 if self.lift_motor_speed == 0.0 or self.lift_motor_speed == low_volt: self.lift_motor_speed = 0.6 elif int(abs(self.encoder.getRate())) == 0: self.lift_motor_speed += 0.01 elif (self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9)\ and self.joystick.getBButton(): self.lift_motor_speed -= 0.01 if self.lift_motor_speed > 0.0 else 0.0 elif self.joystick.getTriggerAxis(self.joystick.Hand.kRight) > 0.9: self.lift_motor_speed = low_volt elif (self.lift_motor_speed > low_volt and abs(self.encoder.getRate()) > 0): self.lift_motor_speed -= 0.0025 elif (0.0 < self.lift_motor_speed < low_volt): self.lift_motor_speed -= 0.0025 if self.lift_motor_speed > 0.0 else 0.00 elif self.lift_motor_speed < 0: self.lift_motor_speed = 0 self.lift_motor_group.set(self.lift_motor_speed) def grab_control(self): if self.joystick.getBumper(self.joystick.Hand.kLeft) and not \ self.joystick.getBumper(self.joystick.Hand.kRight): self.front_motor_group.set(0.33) elif self.joystick.getBumperPressed(self.joystick.Hand.kRight) and not \ self.joystick.getBumper(self.joystick.Hand.kLeft): self.front_motor_group.set(-0.7) elif self.joystick.getBumperReleased(self.joystick.Hand.kLeft) or \ self.joystick.getBumperReleased(self.joystick.Hand.kRight): self.front_motor_group.set(0) def hatch_control(self): if self.joystick.getXButtonPressed(): self.hatch_solenoid.set(2) elif self.joystick.getXButtonReleased(): self.hatch_solenoid.set(1) def robotInit(self): self.reset() wpilib.CameraServer.launch('vision.py:main') def robotPeriodic(self): pass def autonomousInit(self): self.reset() def autonomousPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control() def disabledInit(self): self.reset() def disabledPeriodic(self): self.reset() def teleopInit(self): self.reset() self.drive.setSafetyEnabled(True) def teleopPeriodic(self): self.drive_control() self.lift_control() self.grab_control() self.hatch_control()
class Drivetrain: navx = navx.AHRS left_motor_master = WPI_TalonFX left_motor_slave = WPI_TalonFX left_motor_slave2 = WPI_TalonFX right_motor_master = WPI_TalonFX right_motor_slave = WPI_TalonFX right_motor_slave2 = WPI_TalonFX shifter_solenoid = Solenoid arcade_cutoff = tunable(0.1) angle_correction_cutoff = tunable(0.05) angle_correction_factor_low_gear = tunable(0.1) angle_correction_factor_high_gear = tunable(0.1) angle_correction_max = tunable(0.2) little_rotation_cutoff = tunable(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 is_left_encoder_connected(self): return self.left_motor_master.getPulseWidthRiseToRiseUs() != 0 def is_right_encoder_connected(self): return self.right_motor_master.getPulseWidthRiseToRiseUs() != 0 def reset_position(self): self.left_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) self.right_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) def get_position(self): ''' Returns averaged quadrature position in inches. ''' left_position = self.get_left_encoder() right_position = self.get_right_encoder() position = (((left_position + right_position) / 2) * (1 / UNITS_PER_REV) * DISTANCE_PER_REV) return position def drive(self, *args): self.differential_drive(*args) def differential_drive(self, y, rotation=0, squared=True, force=False, quick_turn=False, use_curvature=False): if not self.force_differential_drive: self.pending_differential_drive = DifferentialDriveConfig( y=y, rotation=rotation, squared=squared, quick_turn=quick_turn, use_curvature=use_curvature) self.force_differential_drive = force def turn(self, rotation=0, force=False): self.differential_drive(0, rotation, squared=False, force=force) def reset_angle_correction(self): self.navx.reset() def angle_corrected_differential_drive(self, y, rotation=0): ''' Heading must be reset first. (drivetrain.reset_angle_correction()) ''' # Scale angle to reduce max turn rotation = util.scale(rotation, -1, 1, -0.65, 0.65) # Scale y-speed in high gear if self.pending_gear == HIGH_GEAR: y = util.scale(y, -1, 1, -0.75, 0.75) use_curvature = False quick_turn = False # Small rotation at lower speeds - and also do a quick_turn instead of # the normal curvature-based mode. if abs(y) <= self.little_rotation_cutoff: rotation = util.abs_clamp(rotation, 0, 0.7) quick_turn = True # NEVER USE CURVATURE # Curvature drive for high gear and high speedz # if self.pending_gear == HIGH_GEAR and abs(y) >= 0.5: # use_curvature = True # Angle correction if abs(rotation) <= self.angle_correction_cutoff: heading = self.navx.getYaw() if not self.og_yaw: self.og_yaw = heading factor = self.angle_correction_factor_high_gear if \ self.pending_gear == HIGH_GEAR else \ self.angle_correction_factor_low_gear rotation = util.abs_clamp(-factor * (heading - self.og_yaw), 0, self.angle_correction_max) else: self.og_yaw = None self.differential_drive(y, rotation, quick_turn=quick_turn, use_curvature=use_curvature) def shift_low_gear(self): self.pending_gear = LOW_GEAR def shift_high_gear(self): self.pending_gear = HIGH_GEAR def shift_toggle(self): if self.pending_gear == HIGH_GEAR: self.pending_gear = LOW_GEAR else: self.pending_gear = HIGH_GEAR def manual_drive(self, left, right): self.pending_manual_drive = [left, right] def get_left_encoder(self): return -self.left_motor_master.getQuadraturePosition() def get_right_encoder(self): return self.right_motor_master.getQuadraturePosition() def get_left_encoder_meters(self): return self.get_left_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_meters(self): return self.get_right_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_left_encoder_velocity(self): return -self.left_motor_master.getQuadratureVelocity() def get_right_encoder_velocity(self): return self.right_motor_master.getQuadratureVelocity() def get_left_encoder_velocity_meters(self): return self.get_left_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_velocity_meters(self): return self.get_right_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def set_manual_mode(self, is_manual): self.is_manual_mode = is_manual if not is_manual: self.pending_manual_drive = None def execute(self): # print('exec drivetrain', self.pending_differential_drive) # print('dist_traveled_meters', self.get_left_encoder_meters(), # self.get_right_encoder_meters()) # Shifter self.shifter_solenoid.set(self.pending_gear) # Manual if self.is_manual_mode: if self.pending_manual_drive: left, right = self.pending_manual_drive # left = self.robot_drive.applyDeadband(left, DEADBAND) # right = self.robot_drive.applyDeadband(right, DEADBAND) self.left_motor_master.set(-left) self.right_motor_master.set(right) self.pending_manual_drive = None return # Drive if self.pending_differential_drive: if self.pending_differential_drive.use_curvature and \ abs(self.pending_differential_drive.y) > \ self.arcade_cutoff and \ USE_CURVATURE_DRIVE: self.robot_drive.curvatureDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, isQuickTurn=self.pending_differential_drive.quick_turn) else: self.robot_drive.arcadeDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, squareInputs=self.pending_differential_drive.squared) self.pending_differential_drive = None self.force_differential_drive = False def on_disable(self): self.robot_drive.arcadeDrive(0, 0) def get_state(self): return { 'pending_gear': self.pending_gear, 'pending_differential_drive': self.pending_differential_drive } def put_state(self, state): self.pending_gear = state['pending_gear'] self.pending_differential_drive = DifferentialDriveConfig._make( state['pending_differential_drive']) def limelight_turn(self): self.llt = NetworkTables.getTable('limelight') self.tv = self.llt.getNumber('tv', 0) self.tx = self.llt.getNumber('tx', 0) if self.tv: self.turn(self.get_position() + self.tx)