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)