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)
