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)
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.TimedRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Talon(0)
self.rearLeftMotor = wpilib.Talon(1)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
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)
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
self.myRobot.tankDrive(self.leftStick.getY() * -1,
self.rightStick.getY() * -1)
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 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 teleopPeriodic(self):
# Create tank drive
self.driveTrain.tankDrive(self.l_stick.getY(), self.r_stick.getY())
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
self.frontLeftMotor = wpilib.Talon(1)
self.rearLeftMotor = wpilib.Talon(12)
self.frontRightMotor = wpilib.Talon(2)
self.rearRightMotor = wpilib.Talon(5)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
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)
def teleopInit(self):
'''Executed at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
'''Runs the motors with tank steering'''
self.myRobot.tankDrive(self.leftStick.getY() * -1,
self.rightStick.getY() * -1)
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())
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)
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 MyRobot(wpilib.IterativeRobot):
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)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.myRobot.setExpiration(0.1)
self.myRobot.setSafetyEnabled(True)
def autonomousInit(self):
'''
:return:
'''
pass
def autonomousPeriodic(self):
pass
def teleopInit(self):
"""Executed at the start of teleop mode
:return:
"""
pass
def teleopPeriodic(self):
"""Runs the motors with tank steering
:return:
"""
self.myRobot.tankDrive(self.leftStick.getY(), self.rightStick.getY())
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 MyRobot(wp.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = ctre.WPI_VictorSPX(1) #motor initialization
self.leftMotor2 = ctre.WPI_VictorSPX(3)
self.leftMotor3 = ctre.WPI_VictorSPX(5)
self.rightMotor1 = ctre.WPI_VictorSPX(0)
self.rightMotor2 = ctre.WPI_VictorSPX(2)
self.rightMotor3 = ctre.WPI_VictorSPX(4)
self.armMotor = ctre.WPI_VictorSPX(6)
self.leftIntakeMotor = ctre.WPI_VictorSPX(7)
self.rightIntakeMotor = ctre.WPI_VictorSPX(8)
self.leftSide = wp.SpeedControllerGroup(
self.leftMotor1, self.leftMotor2,
self.leftMotor3) #speed controller groups
self.rightSide = wp.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2,
self.rightMotor3)
self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor,
self.rightIntakeMotor)
self.myRobot = DifferentialDrive(self.leftSide, self.rightSide)
self.myRobot.setExpiration(0.1)
self.leftStick = wp.Joystick(0)
self.rightStick = wp.Joystick(1)
self.armStick = wp.Joystick(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(0, 1)
self.leftEncd = wp.Encoder(2, 3)
self.compressor = wp.Compressor()
wp.SmartDashboard.putNumber("Straight Position", 4000)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Turn Auto", 2)
self.chooser.addObject("Right Turn Auto", 3)
self.chooser.addObject("Straight/Timer", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotPeriodic(self):
wp.SmartDashboard.putNumber("Gyro:", round(self.gyro.getAngle(), 2))
wp.SmartDashboard.putNumber("Right Encoder:", self.rightEncd.get())
wp.SmartDashboard.putNumber("Left Encoder:", self.leftEncd.get())
calGyro = wp.SmartDashboard.getBoolean("calGyro:", True)
resetGyro = wp.SmartDashboard.getBoolean("resetGyro:", True)
encodeReset = wp.SmartDashboard.getBoolean("resetEnc:", True)
if (resetGyro):
self.gyro.reset()
wp.SmartDashboard.putBoolean("resetGyro:", False)
if (calGyro):
self.gyro.calibrate()
wp.SmartDashboard.putBoolean("calGyro:", False)
if (encodeReset):
self.rightEncd.reset()
self.leftEncd.reset()
wp.SmartDashboard.putBoolean("resetEnc:", False)
self.auto = self.chooser
def autonomousInit(self):
self.auto = self.chooser
self.leftMotorVal = 0
self.rightMotorVal = 0
self.gyroFuncGain = 40
self.leftMotVal = 0
self.rightMotVal = 0
def autonomousPeriodic(self):
#autonomous code will go here
if (self.auto == 1):
if (self.rightEncd <= ap.moveStraightPos):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, -0.65, self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.myRobot.tankDrive(self.leftMotVal, self.rightMotVal)
def teleopInit(self):
#telop init here
self.myRobot.setSafetyEnabled(True)
self.pastFrontFlipButton = False
self.flip = True
def teleopPeriodic(self):
#teleop code will go here
#Joystick Variables
leftJoyValY = self.leftStick.getY()
rightJoyValY = self.rightStick.getY()
armJoyValY = self.armStick.getY()
frontFlipButton = self.rightStick.getRawButton(2)
#FrontFlip
if (self.pastFrontFlipButton == False and frontFlipButton):
self.flip = not self.flip
self.pastFrontFlipButton = frontFlipButton
leftMotVal, rightMotVal = rf.flip(self.flip, leftJoyValY, rightJoyValY)
self.myRobot.tankDrive(leftMotVal, rightMotVal)
class MyRobot(wpilib.IterativeRobot):
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 autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance().getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
def autonomousPeriodic(self):
if self.gameData[0:1] == "L":
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5,0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(0.7,0.7)
while timer.get() < 3:
self.myRobot.tankDrive(0,0)
timer.reset()
timer.start()
while timer.get() < 0.7: # Right = 0.68 //original
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.myRobot.tankDrive(-0.7,0.7) # Turning right, even though it looks like turning left
while timer.get() < 1.2:
self.myRobot.tankDrive(0,0)
while timer.get() < 2:
self.myRobot.tankDrive(0.7,0.7)
while timer.get() < 2.5:
self.myRobot.tankDrive(0,0)
while timer.get() < 5: # Outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
self.iSolenoid.set(1)
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
''' Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
elif self.gameData[0:1] == 'R':
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5,0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(0.7,0.7)
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
'''Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
def disabledInit(self):
self.myRobot.tankDrive(0,0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
minv = 0.4
maxv = 0.6
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sp = forward - backward
steering = self.Stick1.getX(0)
mod = minv + maxv*((1-abs(sp))**2)
r = (steering**3)*mod
if sp >= 0:
self.myRobot.tankDrive(sp*0.85 - r, sp*0.85 + r)
else:
self.myRobot.tankDrive(sp*0.85 + r, (sp*0.85 - r))
# intake and outtake
if self.Stick2.getRawButton(11)==True: # intake
self.ihigh_motor.set(0.6)
self.ilow_motor.set(-0.95)
if self.Stick2.getRawButton(11)==False and self.Stick2.getRawButton(12)==False:
self.ihigh_motor.set(0)
self.ilow_motor.set(0)
if self.Stick2.getRawButton(12)==True: # outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
if self.Stick2.getRawButton(9) == True:
self.aSolenoidLow.set(1)
self.iSolenoid.set(1)
if self.Stick2.getRawButton(10) == True:
self.aSolenoidLow.set(2)
if self.Stick2.getRawButton(7) == True:
self.aSolenoidHigh.set(1)
if self.Stick2.getRawButton(8) == True:
self.aSolenoidHigh.set(2)
if self.Stick2.getRawButton(3) == True:
self.iSolenoid.set(1) # push intake
if self.Stick2.getRawButton(4) == True:
self.iSolenoid.set(2) # pull intake
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):
'''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 MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
#wpilib.DriverStation.reportWarning(str(self.myRobot.isRightSideInverted()),False)
def PID(self):
error = self.setpoint - self.gyro.getAngle()
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
self.rcw = self.P * error + self.I * self.integral + self.D * derivative
def autonomousInit(self):
self.myRobot.setSafetyEnabled(False)
def autonomousPeriodic(self):
pass
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
self.myRobot.arcadeDrive(self.rightStick.getY(),
-self.rightStick.getZ() * .4)
self.smartdash.putNumber('gyrosensor', self.gyro.getAngle())
if (self.rightStick.getRawButton(4)):
self.ll.putNumber('ledMode', 1)
if (self.rightStick.getRawButton(3)):
self.right.set(.5)
if (self.trigger.get()):
if (self.gearshift.get() == 1):
self.gearshift.set(2)
elif (self.gearshift.get() == 2):
self.gearshift.set(1)
if (self.rightStick.getRawButton(2)):
self.tx = self.ll.getNumber("tx", None)
if (not type(self.tx) == type(0.0)):
self.tx = 0.0
if (self.tx > 1.0):
self.myRobot.arcadeDrive(0, self.tx * .03)
elif (self.tx < -1.0):
self.myRobot.tankDrive(0, self.tx * .03)
else:
self.myRobot.tankDrive(0, 0)
self.pdp.getVoltage()
class MyRobot(wpilib.TimedRobot):
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 autonomousInit(self): #This function is called once during autonomous
self.timer.reset()
self.timer.start()
def autonomousPeriod(self): #This function is called ~50/s during autonomous (don't use loops)
if self.timer.get() < 3:
self.drive.tankDrive(0.5, 0.5, squaredInputs = True) #drive forward at half speed
elif self.timer.get() < 5:
self.drive.tankDrive(0.0, 0.0, squaredInputs = True) #stop driving
else:
self.drive.tankDrive(-1, 1, squaredInputs = True) #spin left until time runs out
def teleopPeriod(self): #This function is called ~50/s during teleop (don't use loops)
self.drive.tankDrive(self.controller.getY(), self.controller.getAxis(4)) #drive setup is all done
in_out = 'Off' #check for simple button push
if self.controller.getButton(5):
in_out = 'Forward'
elif self.controller.getButton(6):
in_out = 'Reverse'
self.claw.set(in_out) #adjust claw based on button push
up_down = 'Off' #check for simple button push
if self.controller.getButton(7):
up_down = 'Forward'
elif self.controller.getButton(8):
up_down = 'Reverse'
self.arm.set(up_down) #adjust arm based on button push
class MyRobot(wpilib.IterativeRobot):
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)
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):
"""Runs the motors with tank steering"""
right = self.driver.getY(self.RIGHT)
left = self.driver.getY(self.LEFT)
self.myRobot.tankDrive(right, left)
center_speed = self.driver.getX(self.RIGHT)
self.setCenters(self.deadzone(center_speed, self.DEADZONE))
class Robot(wpilib.IterativeRobot):
def __init__(self):
super().__init__()
# Initiate up robot drive
self.left_motor = wpilib.Spark(LEFT_PORT)
self.right_motor = wpilib.Spark(RIGHT_PORT)
self.drive = DifferentialDrive(self.left_motor, self.right_motor)
self.drive.setExpiration(.1)
# Initiate arm and lift
self.robot_lift = wpilib.PWMTalonSRX(LIFT_PORT)
self.robot_lift_2 = wpilib.PWMTalonSRX(LIFT_PORT_2)
# Initiate button stuff
self.buttons = set()
self.button_toggles = None
self.pressed_buttons = {}
self.rumble_time = None
self.rumble_length = RUMBLE_LENGTH
self.reset_buttons()
# Initiate gamepad
self.game_pad = wpilib.Joystick(GAMEPAD_NUM)
self.max_enhancer = 0
self.smart_dashboard = wpilib.SmartDashboard
# auto_chooser = wpilib.SendableChooser()
# auto_chooser.addDefault("Enable Auto", "Enable Auto")
# auto_chooser.addObject("Disable Auto", "Disable Auto")
self.smart_dashboard.putBoolean("Enable Auto", True)
self.auto_start_time = None
self.current_speed = [0, 0]
self.last_tank = 0
self.gyro = wpilib.ADXRS450_Gyro(0) # TODO: Figure out channel
self.tank_dir = None
def reset_buttons(self):
"""Resets the values of the button toggles to default likewise
defined here
"""
self.button_toggles = {
"REVERSE": False,
"LOCK": True,
"STOP": False,
"SPEED": False,
"GRAB": False
}
self.pressed_buttons = {}
self.rumble_time = None
def stop(self):
self.reset_buttons()
self.last_tank = get_millis()
self.drive.stopMotor()
self.robot_lift.stopMotor()
self.robot_lift_2.stopMotor()
self.current_speed = [0, 0]
# Events
def robotInit(self):
"""Runs at the same time as __init__. Appears to be redundant in this
case
"""
self.stop()
def robotPeriodic(self):
pass
def disabledInit(self):
self.stop()
def disabledPeriodic(self):
self.stop()
def autonomousInit(self):
self.stop()
self.auto_start_time = None
self.gyro.calibrate() # Takes five seconds
self.stop() # Have to reset tank time to correct acceleration
def tank(self, left, right, accel=None):
if left == right:
if isinstance(self.tank_dir, type(None)):
self.tank_dir = self.gyro.getAngle()
else:
self.tank_dir = None
turn = None
current_angle = None
if not isinstance(self.tank_dir, type(None)):
current_angle = self.gyro.getAngle()
if current_angle < self.tank_dir - ANGLE_MARGIN:
turn = "right"
elif current_angle > self.tank_dir + ANGLE_MARGIN:
turn = "left"
if isinstance(accel, type(None)):
accel = ACCEL
speed1 = self.current_speed[0]
speed2 = self.current_speed[1]
if not isinstance(turn, type(None)) and GYRO_ENABLE:
if turn == "left":
left -= ANGLE_CHANGE
right += ANGLE_CHANGE
elif turn == "right":
right -= ANGLE_CHANGE
left += ANGLE_CHANGE
print((round(self.tank_dir, 3)
if not isinstance(self.tank_dir, type(None)) else None),
(round(current_angle, 3)
if not isinstance(current_angle, type(None)) else None),
round(left, 3), round(right, 3), turn)
if .4 > speed1 > 0:
if left <= 0:
speed1 = 0
else:
speed1 = .4
if -.4 < speed1 < 0:
if left >= 0:
speed1 = 0
else:
speed1 = -.4
if .4 > speed2 > 0:
if right <= 0:
speed2 = 0
else:
speed2 = .4
if -.4 < speed2 < 0:
if right >= 0:
speed2 = 0
else:
speed2 = -.4
if isinstance(accel, type(None)):
self.drive.tankDrive(left, right)
return None
vel_change = ((get_millis() - self.last_tank) / 1000) * accel
if abs(speed1 - left) < WHEEL_LOCK:
speed1 = left
else:
if speed1 < left:
speed1 += vel_change
elif speed1 > left:
speed1 -= vel_change
if abs(speed2 - right) < WHEEL_LOCK:
speed2 = right
else:
if speed2 < right:
speed2 += vel_change
elif speed2 > right:
speed2 -= vel_change
self.current_speed = [speed1, speed2]
# print([round(left, 2), round(right, 2)], [round(speed1, 2), round(speed2, 2)], round(vel_change, 4))
self.drive.tankDrive(*self.current_speed)
# print([round(x, 2) for x in self.current_speed], round(vel_change, 2), accel)
self.last_tank = get_millis()
def autonomousPeriodic(self):
if self.smart_dashboard.getBoolean("Auto Enabled", True):
if not isinstance(self.auto_start_time, type(None)):
# Auto enabled and running
# percent = (get_millis() - self.auto_start_time) / AUTO_DUR
# if percent < .5:
# speed = (percent/.5) * AUTO_SPEED
# elif percent < 1:
# speed = ((1 - percent)/.5) * AUTO_SPEED
# else:
# speed = 0
if get_millis() >= self.auto_start_time + AUTO_DUR / 2:
self.tank(0, 0, AUTO_ACCEL)
else:
self.tank(AUTO_SPEED, AUTO_SPEED, AUTO_ACCEL)
else:
# Auto enabled and not started
self.auto_start_time = get_millis()
def teleopInit(self):
self.stop()
self.drive.setSafetyEnabled(True)
def set_rumble(self, left=True, value=1, length=RUMBLE_LENGTH):
"""Sets given rumble to given value"""
if not left:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kLeftRumble,
value)
else:
self.game_pad.setRumble(wpilib.Joystick.RumbleType.kRightRumble,
value)
self.rumble_time = get_millis()
self.rumble_length = length
def check_rumble(self):
"""Checks if rumbling has surpassed RUMBLE_LENGTH"""
if not isinstance(self.rumble_time, type(None)) and get_millis(
) > self.rumble_time + self.rumble_length:
# This rumbling has gone on long enough!
self.set_rumble(True, 0)
self.set_rumble(False, 0)
self.rumble_time = None
def execute_buttons(self):
"""Check whether each button was just pressed and updates relevant
toggle
"""
arms = 0
button_states = self.get_buttons()
for button_name in BUTTON_PORTS:
port = BUTTON_PORTS[button_name]
angle = -1
if isinstance(port, list):
angle = port[1]
port = port[0]
if port in button_states and button_states[port] is True:
# button was just pressed
if button_name in self.button_toggles:
# needs toggled
if button_name == "GRAB":
pass
# print(button_name, port == POV,
# self.get_raw_buttons()[POV], angle)
if not (port == POV
and not self.get_raw_buttons()[POV] == angle):
new_state = not self.button_toggles[button_name]
self.button_toggles[button_name] = new_state
self.set_rumble(new_state)
# print(button_name, new_state)
elif self.get_raw_buttons()[POV] == angle:
# Button angle correct, check button name
if button_name == "INC SPEED":
self.max_enhancer += SPEED_ADJUST
self.set_rumble(True, 1)
elif button_name == "DEC SPEED":
self.max_enhancer -= SPEED_ADJUST
self.set_rumble(False, 1)
elif button_name == "RES SPEED":
self.max_enhancer = 0
self.set_rumble(True, length=200)
elif port in button_states:
# BUTTON BEING PRESSED
if button_name == "ARM IN":
arms = max(-1, min(-(ARM_SPEED_IN), 1))
self.button_toggles["GRAB"] = False
elif button_name == "ARM OUT":
arms = max(-1, min(ARM_SPEED_OUT, 1))
self.button_toggles["GRAB"] = False
# if arms == 0 and self.button_toggles["GRAB"]:
# self.robot_lift_2.set(-ARM_SPEED_IN)
# else:
# self.robot_lift_2.set(arms)
self.check_rumble()
def execute_axes(self):
"""Checks each axis and updates attached motor"""
axes = self.get_axes()
if not self.button_toggles["STOP"]:
self.tank(axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS])
# self.left_motor.set(axes[LEFT_WHEELS_AXIS])
# self.right_motor.set(axes[RIGHT_WHEELS_AXIS])
left_trigger = axes[LIFT_AXIS1]
right_trigger = axes[LIFT_AXIS2]
if left_trigger > 0:
if right_trigger <= 0:
self.robot_lift.set(-left_trigger)
self.robot_lift_2.set(-left_trigger)
elif right_trigger > 0:
self.robot_lift.set(right_trigger)
# print(right_trigger)
else:
self.robot_lift.set(0)
self.robot_lift_2.set(0)
def teleopPeriodic(self):
"""Runs repeatedly while robot is in teleop"""
self.execute_buttons()
self.execute_axes()
def get_raw_axes(self, _round=False):
"""Return a dictionary of controller axes"""
i = 0
axes = {}
while True:
try:
value = self.game_pad.getRawAxis(i)
if i == RIGHT_WHEELS_AXIS:
if value < 0:
value /= .9 # Right stick does not go full forward
value = min(value, 1)
if _round:
axes[i] = round(value, 2)
else:
axes[i] = value
except IndexError:
break
i += 1
return axes
def get_axes(self, _round=False):
"""Returns the current axes and values with deadzones and scaled"""
axes = self.get_raw_axes(_round)
# Correct deadzones and scale
for axis in axes:
if axis != LIFT_AXIS1 and axis != LIFT_AXIS2:
if axis in DEADZONES:
value = axes[axis]
neg = -1 if value < 0 else 1
deadzone = DEADZONES[axis]
if abs(value) > deadzone:
value -= deadzone * neg
value /= 1 - deadzone
if not self.button_toggles[
"SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[axis][value >= 0] +
self.max_enhancer, 1))
else:
value = 0
if REVERSES[axis]:
value *= -1
axes[axis] = value
else: # TODO: Clean up
# TRIGGERS FOR LIFT
value = axes[axis]
deadzone = DEADZONES[LIFT_AXIS1]
if value > deadzone:
value -= deadzone
value /= 1 - deadzone
if not self.button_toggles["SPEED"]: # Do not impose caps
value *= max(
-1,
min(
SPEED_CAPS[LIFT_AXIS1][axis == LIFT_AXIS1] +
self.max_enhancer, 1))
if self.button_toggles["REVERSE"]:
axes[LEFT_WHEELS_AXIS] *= -1
axes[RIGHT_WHEELS_AXIS] *= -1
dif, avg = None, None
if self.button_toggles["LOCK"]:
left = axes[LEFT_WHEELS_AXIS]
right = axes[RIGHT_WHEELS_AXIS]
dif = abs(abs(left) - abs(right))
if dif <= WHEEL_LOCK and left != 0 and right != 0:
lneg = left < 0
rneg = right < 0
smaller = min(abs(left), abs(right))
avg = smaller + dif / 2
axes[LEFT_WHEELS_AXIS] = avg
axes[RIGHT_WHEELS_AXIS] = avg
if lneg:
axes[LEFT_WHEELS_AXIS] *= -1
if rneg:
axes[RIGHT_WHEELS_AXIS] *= -1
# print(self.button_toggles["LOCK"], left, right,
# axes[LEFT_WHEELS_AXIS], axes[RIGHT_WHEELS_AXIS], dif, avg)
return axes
def get_raw_buttons(self):
"""Return a dictionary of raw button states"""
i = 1 # Controller buttons start at 1
buttons = {}
while i <= 10:
# Gets ten buttons or stops at index error <-- shouldn't happen
try:
buttons[i] = self.game_pad.getRawButton(i)
except IndexError:
break
i += 1
buttons[i] = self.game_pad.getPOV(0)
return buttons
def get_buttons(self):
"""Returns a dictionary of bools representing whether each button was
just pressed
"""
raw_buttons = self.get_raw_buttons()
for button in raw_buttons:
being_pressed = (
not raw_buttons[button] is False) and raw_buttons[button] != -1
if button not in self.pressed_buttons and being_pressed:
# If button not already accounted for and being pressed
self.pressed_buttons[button] = True
elif button in self.pressed_buttons:
if being_pressed:
# Being pressed, already used
self.pressed_buttons[button] = False
else:
# Was pressed, no longer being pressed
del self.pressed_buttons[button]
return self.pressed_buttons
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(1)
# self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(2)
self.frontRightMotor = wpilib.Spark(3)
# self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(4)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = ''
# 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)
self.gyro = wpilib.ADXRS450_Gyro()
def autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance().getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
global firstTime
firstTime = True
def autonomousPeriodic(self):
# This program tests 90 degree turn with gyro
print(time.time())
global firstTime, fD, fastD, fastV, slowV, error
if firstTime:
sD = self.gyro.getAngle()
fD = sD - 90
firstTime = False
fastV = 0.78
slowV = 0.64
fastD = 75
error = 6
cD = self.gyro.getAngle()
# left smaller right bigger
if cD > fD - error:
cD = self.gyro.getAngle()
needD = cD - fD # getting smaller as turing left
if needD >= 90 - fastD:
speed_turn = fastV
else:
speed_turn = slowV
self.myRobot.tankDrive(-speed_turn, speed_turn)
else:
self.myRobot.tankDrive(0,0)
print(time.time())
def disabledInit(self):
self.myRobot.tankDrive(0,0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
self.iSolenoid.set(1)
lastspeed = 0
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
threshold = 0.4#机器人能开的速度
slow = 0.7#将这个以下的速度精细控制
current_speed = 0
deadband_forward = 0.1#向前的deadband
deadband_steering = 0.1#转弯的deadband
stering_mutiplier = 1.3#越大，转弯越慢
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sum_speed = forward - backward
if abs(sum_speed) > deadband_forward:#如果开的话转弯变慢
stering_mutiplier = 1.5
else:
stering_mutiplier = 1.3
steering = (self.Stick1.getX(0)) / stering_mutiplier#算转弯
if abs(steering) < deadband_steering:#转弯的deadband
steering = 0
if sum_speed >= 0:#算前进方向
direct = 1
elif sum_speed < 0
direct = -1
if ((abs(sum_speed) < slow) and (abs(sum_speed) > deadband_forward)): #多档控制
calculated_speed = sum_speed*((slow - threshold) / slow) + threshold*direct
elif abs(sum_speed) > slow:
calculated_speed = sum_speed
else:
calculated_speed = 0
print("sum " + str(sum_speed))
print("speed " + str(calculated_speed))
self.myRobot.tankDrive(calculated_speed + steering, calculated_speed - steering)
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):
''' Initialization of robot objects. '''
''' Talon SRX Initialization '''
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
''' Motor Groups '''
# drive train motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
''' Victor SPX Initialization '''
# lift motors
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motors
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor
self.cargo = WPI_VictorSPX(5)
''' Encoders '''
# drive train encoders
self.rightEncoder = self.frontRightMotor
self.leftEncoder = self.frontLeftMotor
# lift encoder
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder
self.liftArmEncoder = wpilib.Encoder(5, 6, True)
''' Sensors '''
# Hall Effect Sensor
self.minHall = wpilib.DigitalInput(7)
self.maxHall = wpilib.DigitalInput(4)
self.limitSwitch = wpilib.DigitalInput(3)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
''' Controller Initialization and Mapping '''
# joystick - 0, 1 | controller - 2
self.joystick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
''' Pneumatic Button Status '''
self.clawButtonStatus = Toggle(self.xbox, 2)
self.gearButtonStatus = Toggle(self.joystick, 1)
self.ejectorPinButtonStatus = Toggle(self.xbox, 1)
self.compressorButtonStatus = Toggle(self.xbox, 9)
self.liftHeightButtonStatus = Toggle(self.xbox, 3)
''' Pneumatic Initialization '''
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
''' Smart Dashboard '''
# connection for logging & Smart Dashboard
logging.basicConfig(level=logging.DEBUG)
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
# Smart Dashboard classes
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
''' Timer '''
self.timer = wpilib.Timer()
''' Camera '''
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
''' PID settings for lift '''
self.kP = 0.03
self.kI = 0.0
self.kD = 0.0
self.kF = 0.1
self.PIDLiftcontroller = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.liftEncoder,
output=self)
self.PIDLiftcontroller.setInputRange(0, 400)
self.PIDLiftcontroller.setOutputRange(-0.5, 0.5)
self.PIDLiftcontroller.setAbsoluteTolerance(1.0)
self.PIDLiftcontroller.setContinuous(True)
self.encoderRate = 0
def pidWrite(self, output):
self.encoderRate = output
def robotCode(self):
if self.liftHeightButtonStatus.on:
self.PIDLiftcontroller.setSetpoint(200)
self.liftToHeight = True
elif self.liftHeightButtonStatus.off:
self.PIDLiftcontroller.setSetpoint(0)
self.liftToHeight = False
def hatchOne():
if self.liftEncoder.getDistance() < 80: # Hatch 2
self.lift.set(0.3)
elif self.liftEncoder.getDistance() >= 80:
self.lift.set(0.07)
def hatchTwo():
if self.liftEncoder.getDistance() < 275: # Hatch 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 275:
self.lift.set(0.07)
def cargoOne():
if self.liftEncoder.getDistance() < 150: # Cargo 1
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 150:
self.lift.set(0.05)
def cargoTwo():
if self.liftEncoder.getDistance() < 320: # Cargo 2
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 320:
self.lift.set(0.05)
def cargoShip():
if self.liftEncoder.getDistance() < 280: # Cargo ship
self.lift.set(0.5)
elif self.liftEncoder.getDistance() >= 280:
self.lift.set(0.07)
# ''' Button Box Level Mapping '''
# if self.buttonStatusOne.on:
# # hatchOne()
# cargoOne()
# elif self.buttonStatusTwo.on: # comment out for hatch
# cargoTwo()
# elif self.buttonStatusThree.on:
# # hatchTwo()
# cargoShip()
if self.minHall.get() is False:
self.liftEncoder.reset()
if self.limitSwitch.get() is False:
self.liftArmEncoder.reset()
''' Smart Dashboard '''
# compressor state
if self.Compressor.enabled() is True:
self.sd.putString("Compressor Status: ", "Enabled")
elif self.Compressor.enabled() is False:
self.sd.putString("Compressor Status: ", "Disabled")
''' Pneumatics Dashboard States '''
# gear state
if self.DoubleSolenoidOne.get() == 1:
self.sd.putString("Gear Shift: ", "HIGH SPEED!!!")
elif self.DoubleSolenoidOne.get() == 2:
self.sd.putString("Gear Shift: ", "Low")
# ejector state
if self.DoubleSolenoidThree.get() == 2:
self.sd.putString("Ejector Pins: ", "Ejected")
elif self.DoubleSolenoidThree.get() == 1:
self.sd.putString("Ejector Pins: ", "Retracted")
# claw state
if self.DoubleSolenoidTwo.get() == 2:
self.sd.putString("Claw: ", "Open")
elif self.DoubleSolenoidTwo.get() == 1:
self.sd.putString("Claw: ", "Closed")
''' Ultrasonic Range Detection '''
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
if 0.142 <= self.ultraValue <= 0.146:
self.sd.putString("PLAYER STATION RANGE: ", "YES!!!!")
else:
self.sd.putString("PLAYER STATION RANGE: ", "NO!")
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
if 0.70 <= self.cargoUltraValue <= 1.56:
self.sd.putString("HATCH RANGE: ", "HATCH IN RANGE")
else:
self.sd.putString("HATCH RANGE: ", "NOT IN RANGE")
''' Pneumatics Toggles '''
# Compressor
if self.compressorButtonStatus.on:
self.Compressor.start()
elif self.compressorButtonStatus.off:
self.Compressor.stop()
# Claw Toggle
if self.clawButtonStatus.on:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward) # open claw
elif self.clawButtonStatus.off:
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse) # close claw
# Ejector Pins Toggle
if self.ejectorPinButtonStatus.on:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward) # eject
elif self.ejectorPinButtonStatus.off:
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse) # retract
# Gear Shift Toggle
if self.gearButtonStatus.on:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward) # shift right
elif self.gearButtonStatus.off:
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse) # shift left
''' Victor SPX (Lift, Lift Arm, Cargo) '''
# lift control
if self.liftHeightButtonStatus.get() is False:
if self.xbox.getRawButton(5): # hold
self.lift.set(0.05)
elif self.xbox.getRawAxis(3): # up
self.lift.set(self.xbox.getRawAxis(3) * 0.85)
elif self.xbox.getRawAxis(2): # down
self.lift.set(-self.xbox.getRawAxis(2) * 0.45)
else:
self.lift.set(0)
# else:
# if self.liftToHeight is True:
# self.PIDLiftcontroller.enable()
# self.liftHeight = self.encoderRate
# self.lift.set(self.liftHeight)
# else:
# self.PIDLiftcontroller.disable()
# self.lift.set(0)
# # four-bar control
# if self.xbox.getRawButton(6): # hold
# self.liftArm.set(0.12)
# elif not self.xbox.getRawButton(6):
# self.liftArm.set(-self.xbox.getRawAxis(1) * 0.35)
# else:
# self.liftArm.set(0)
# cargo intake control
if self.xbox.getRawButton(7): # hold
self.cargo.set(0.12)
elif self.xbox.getRawAxis(5): # take in
self.cargo.set(self.xbox.getRawAxis(5) * 0.75)
# controller mapping for arcade steering
self.driveAxis = self.joystick.getRawAxis(1)
self.rotateAxis = self.joystick.getRawAxis(2)
# drives drive system using tank steering
if self.DoubleSolenoidOne.get() == 1: # if on high gear
self.divisor = 1.0 # 90% of high speed
self.turnDivisor = 0.8
elif self.DoubleSolenoidOne.get() == 2: # if on low gear
self.divisor = 0.85 # normal slow speed
self.turnDivisor = 0.75
else:
self.divisor = 1.0
if self.driveAxis != 0:
self.leftSign = self.driveAxis / fabs(self.driveAxis)
else:
self.leftSign = 0
if self.rotateAxis != 0:
self.rightSign = self.rotateAxis / fabs(self.rotateAxis)
else:
self.rightSign = 0
self.drive.arcadeDrive(-self.driveAxis * self.divisor,
self.rotateAxis * 0.75)
def autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# timer config
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
self.liftArmEncoder.reset()
def autonomousPeriodic(self):
self.sd.putBoolean("LIFT RESET ", self.minHall.get())
''' Called periodically during autonomous. '''
'''Test Methods'''
def encoder_test():
''' Drives robot set encoder distance away '''
self.rightPos = fabs(self.rightEncoder.getQuadraturePosition())
self.leftPos = fabs(self.leftEncoder.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
def Diagnostics():
''' Smart Dashboard Tests'''
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber("Right Encoder Speed: ",
abs(self.rightEncoder.getQuadratureVelocity()))
self.sd.putNumber("Left Encoder Speed: ",
abs(self.leftEncoder.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ", self.liftEncoder.getDistance())
def Pressure():
self.Compressor.start()
''' Test Execution '''
if self.DS.getGameSpecificMessage() == "pressure":
Pressure()
elif self.DS.getGameSpecificMessage() == "diagnostics":
Diagnostics()
self.robotCode()
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.rightEncoder.setQuadraturePosition(0, 0)
self.leftEncoder.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
self.robotCode()
class MyRobot(wpilib.IterativeRobot):
def disabledInit(self):
#Update Allience
self.statUpdater.getAlliance()
#Send Data to Networktable
self.statUpdater.UpdateStatus(0)
self.statUpdater.UpdateMatchTime()
def robotInit(self):
#Networktables
self.netTable = NetworkTables.getTable('SmartDashboard')
#Hud Data Handlers
self.statUpdater = SU.StatusUpdater(self, self.netTable)
#Camera Server
wpilib.CameraServer.launch()
#Drive Motors
self.motor1 = ctre.WPI_TalonSRX(1)
self.motor2 = ctre.WPI_TalonSRX(2)
self.motor3 = ctre.WPI_TalonSRX(9)
self.motor4 = ctre.WPI_TalonSRX(10)
#Intake Motors
self.stage1Left = ctre.WPI_TalonSRX(5)
self.stage1Right = ctre.WPI_TalonSRX(6)
self.stage2Left = ctre.WPI_TalonSRX(4)
self.stage2Right = ctre.WPI_TalonSRX(7)
self.stage3Left = ctre.WPI_TalonSRX(3)
self.stage3Right = ctre.WPI_TalonSRX(8)
#Pan Arm Controls
self.leftPanArm = wpilib.PWMVictorSPX(0)
self.rightPanArm = wpilib.PWMVictorSPX(1)
#Shifters
self.shifter = wpilib.DoubleSolenoid(1, 2)
#Climb
self.pto = wpilib.DoubleSolenoid(3, 4)
self.climbLift = wpilib.Solenoid(5)
#User Inputs
self.playerOne = wpilib.XboxController(0)
self.playerTwo = wpilib.XboxController(1)
#Navx
self.navx = navx.AHRS.create_spi()
self.power_board = wpilib.PowerDistributionPanel()
#User Inputs
self.playerOne = wpilib.XboxController(0)
self.playerTwo = wpilib.XboxController(1)
#Navx
self.navx = navx.AHRS.create_spi()
#Points
#self.points = []
#Setup Logic
self.rightDriveMotors = wpilib.SpeedControllerGroup(
self.motor3, self.motor4)
self.leftDriveMotors = wpilib.SpeedControllerGroup(
self.motor1, self.motor2)
self.leftDriveMotors.setInverted(True)
self.robotDrive = DifferentialDrive(self.leftDriveMotors,
self.rightDriveMotors)
self.lowerIntakeMotors = wpilib.SpeedControllerGroup(
self.stage1Left, self.stage1Right, self.stage2Left,
self.stage2Right)
self.stage3 = wpilib.SpeedControllerGroup(self.stage3Left,
self.stage3Right)
if wpilib.SolenoidBase.getPCMSolenoidVoltageStickyFault(0) == True:
wpilib.SolenoidBase.clearAllPCMStickyFaults(0)
self.pto.set(wpilib.DoubleSolenoid.Value.kReverse)
#Drive.py init
self.drive = drive.Drive(self.robotDrive, self.navx, self.motor1,
self.motor2, self.motor3, self.motor4,
self.shifter)
#Intake.py
self.intake = intake.Intake(self.lowerIntakeMotors, self.stage3,
self.leftPanArm, self.rightPanArm)
#Driver Station Instance
self.driverStation = wpilib.DriverStation.getInstance()
#Auto mode variables
self.components = {'drive': self.drive, 'intake': self.intake}
self.automodes = AutonomousModeSelector('autonomous', self.components)
def autonomousInit(self):
self.motor1.setNeutralMode(2)
self.motor2.setNeutralMode(2)
self.motor3.setNeutralMode(2)
self.motor4.setNeutralMode(2)
self.statUpdater.UpdateStatus(1)
self.statUpdater.UpdateMatchTime()
self.drive.resetEncoders()
self.drive.gearbox = True
def autonomousPeriodic(self):
#Hud Data Update
self.statUpdater.UpdateMatchTime()
self.statUpdater.UpdateBatteryStatus()
self.automodes.run()
def teleopInit(self):
self.motor1.setNeutralMode(1)
self.motor2.setNeutralMode(1)
self.motor3.setNeutralMode(1)
self.motor4.setNeutralMode(1)
self.statUpdater.UpdateStatus(2)
self.statUpdater.UpdateMatchTime()
self.start = None
self.drive.resetEncoders()
self.drive.autoForward.disable()
self.drive.autoTurn.disable()
self.drive.turnController.disable()
self.drive.resetGyro()
def teleopPeriodic(self):
mult = 0.5 + (self.playerOne.getTriggerAxis(1) * 0.5)
#Intake
self.intake.suck(
self.playerTwo.getTriggerAxis(1) +
self.playerTwo.getTriggerAxis(0) * -1)
self.intake.ohShootDere(self.playerTwo.getYButton(),
self.playerTwo.getAButton())
#Data Updaters
self.statUpdater.UpdateBatteryStatus()
self.statUpdater.UpdateMatchTime()
#Pan Arms
self.intake.panArms(self.playerTwo.getX(0), self.playerTwo.getX(1),
not self.playerTwo.getStickButton(0))
#Drive
if self.pto.get() == wpilib.DoubleSolenoid.Value.kForward:
self.robotDrive.tankDrive(self.playerOne.getY(1),
self.playerOne.getY(0))
else:
self.drive.driveMeBoi(
self.playerOne.getX(1) * 0.7, (self.playerOne.getY(0) * mult))
#Shifting
if self.playerOne.getAButton():
self.drive.gearbox = True
elif self.playerOne.getBButton():
self.drive.gearbox = False
#FlipFlip
if self.playerOne.getBumperPressed(0) == True:
self.drive.flipflip()
#Climb Mechanism
if self.playerOne.getStartButton() == True:
self.climbLift.set(True)
elif self.playerOne.getYButton() == True:
self.pto.set(wpilib.DoubleSolenoid.Value.kForward)
self.climbLift.set(False)
elif self.playerOne.getXButton() == True:
self.pto.set(wpilib.DoubleSolenoid.Value.kReverse)
for channel in range(15):
print("Channel " + str(channel) + ": " +
str(self.power_board.getCurrent(channel)) + " Amps")
class Gemini(wpilib.TimedRobot):
def __init__(self):
""" Initialization of internal class variables and software-bases only """
super().__init__()
# global button status list construction
self.buttonToggleStatus = [
False, False, False, False, False, False, False
]
from networktables import NetworkTables
# connection for logging & Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
NetworkTables.initialize(server='10.55.49.2')
self.sd.putString(" ", "Connection")
def robotInit(self):
''' Initialization of robot systems. '''
logging.info(
'-( ͡° ͜ʖ ͡°)╯╲_-^o=o\ \"Don\'t mind me, just walking the robot.\"'
)
from wpilib.drive import DifferentialDrive
from ctre import WPI_TalonSRX, WPI_VictorSPX
# drive train motors
self.frontRightMotor = WPI_TalonSRX(4)
self.rearRightMotor = WPI_TalonSRX(3)
self.frontLeftMotor = WPI_TalonSRX(1)
self.rearLeftMotor = WPI_TalonSRX(2)
# lift encoder construction
self.liftEncoder = wpilib.Encoder(8, 9)
# liftArm encoder construction
self.liftArmEncoder = wpilib.Encoder(5, 6)
# drive train motor groups assignment
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# drive train drive group assignment
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# lift motor system initialization
self.liftOne = WPI_VictorSPX(1)
self.liftTwo = WPI_VictorSPX(2)
self.lift = wpilib.SpeedControllerGroup(self.liftOne, self.liftTwo)
# lift arm motor system initialization
self.liftArmOne = WPI_VictorSPX(3)
self.liftArmTwo = WPI_VictorSPX(4)
self.liftArm = wpilib.SpeedControllerGroup(self.liftArmOne,
self.liftArmTwo)
# cargo intake motor initialization
self.cargo = WPI_VictorSPX(5)
# game and joystick controller construction
# joystick - 0, 1 | controller - 2
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.xbox = wpilib.Joystick(2)
self.buttonBox = wpilib.Joystick(3)
# pneumatic and compressor system initialization
self.Compressor = wpilib.Compressor(0)
self.Compressor.setClosedLoopControl(True)
self.enable = self.Compressor.getPressureSwitchValue()
self.DoubleSolenoidOne = wpilib.DoubleSolenoid(0, 1) # gear shifting
self.DoubleSolenoidTwo = wpilib.DoubleSolenoid(2,
3) # hatch panel claw
self.DoubleSolenoidThree = wpilib.DoubleSolenoid(
4, 5) # hatch panel ejection
self.Compressor.start()
# Smart Dashboard and NetworkTables initialization and construction
self.PDP = wpilib.PowerDistributionPanel()
self.roboController = wpilib.RobotController()
self.DS = wpilib.DriverStation.getInstance()
# proximity detection sensors
self.Hall = wpilib.DigitalInput(7)
self.ultrasonic = wpilib.AnalogInput(2)
self.cargoUltrasonic = wpilib.AnalogInput(3)
# timer construction
self.timer = wpilib.Timer()
# initialization of the HTTP camera
wpilib.CameraServer.launch('vision.py:main')
self.sd.putString("", "Top Camera")
self.sd.putString(" ", "Bottom Camera")
from sensors import REV_Color_Sensor_V2
# Initialization and configuration of I2C interface with color sensor.
self.colorSensor = REV_Color_Sensor_V2(wpilib.I2C.Port.kOnboard)
def autonomousInit(self):
''' Executed each time the robot enters autonomous. '''
# pre-auto timer configuration
self.timer.reset()
self.timer.start()
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
self.liftEncoder.reset()
def autonomousPeriodic(self):
'''' Called periodically during autonomous. '''
if self.DS.getGameSpecificMessage():
# Test Methods
if self.DS.getGameSpecificMessage() is 'encoder_test':
# Drives robot set encoder distance away
self.rightPos = fabs(
self.frontRightMotor.getQuadraturePosition())
self.leftPos = fabs(
self.frontLeftMotor.getQuadraturePosition())
self.distIn = ((
(self.leftPos + self.rightPos) / 2) / 4096) * 18.84955
if 0 <= self.distIn <= 72:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
if self.DS.getGameSpecificMessage() is 'diagnostics':
# Smart Dashboard Tests
self.sd.putNumber("Temperature: ", self.PDP.getTemperature())
self.sd.putNumber("Battery Voltage: ",
self.roboController.getBatteryVoltage())
self.sd.putBoolean(" Browned Out?",
self.roboController.isBrownedOut)
# Smart Dashboard diagnostics
self.sd.putNumber(
"Right Encoder Speed: ",
abs(self.frontRightMotor.getQuadratureVelocity()))
self.sd.putNumber(
"Left Encoder Speed: ",
abs(self.frontLeftMotor.getQuadratureVelocity()))
self.sd.putNumber("Lift Encoder: ",
self.liftEncoder.getDistance())
if self.DS.getGameSpecificMessage() is 'pressure':
self.Compressor.start()
elif self.Compressor.enabled() is True:
self.Compressor.stop()
if not self.DS.getGameSpecificMessage(
) is 'pressure' and not self.DS.getGameSpecificMessage(
) is 'encoder_test':
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString(
"Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(
wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(
wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))
def teleopInit(self):
''' Executed at the start of teleop mode. '''
self.drive.setSafetyEnabled(True)
# drive train encoder reset
self.frontRightMotor.setQuadraturePosition(0, 0)
self.frontLeftMotor.setQuadraturePosition(0, 0)
# lift encoder rest
self.liftEncoder.reset()
# compressor
self.Compressor.start()
def teleopPeriodic(self):
''' Periodically executes methods during the teleop mode. '''
# begin normal periodic
# get all required data once per frame
# toggle button management per frame
if self.buttonBox.getRawButtonPressed(1):
self.buttonToggleStatus = [
not self.buttonToggleStatus[0], False, False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(2):
self.buttonToggleStatus = [
False, not self.buttonToggleStatus[1], False, False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(3):
self.buttonToggleStatus = [
False, False, not self.buttonToggleStatus[2], False, False,
False, False
]
elif self.buttonBox.getRawButtonPressed(4):
self.buttonToggleStatus = [
False, False, False, not self.buttonToggleStatus[3], False,
False, False
]
elif self.buttonBox.getRawButtonPressed(5):
self.buttonToggleStatus = [
False, False, False, False, not self.buttonToggleStatus[4],
False, False
]
elif self.buttonBox.getRawButtonPressed(6):
self.buttonToggleStatus = [
False, False, False, False, False,
not self.buttonToggleStatus[5], False
]
elif self.buttonBox.getRawButtonPressed(7):
self.buttonToggleStatus = [
False, False, False, False, False, False,
not self.buttonToggleStatus[6]
]
liftTicks = self.liftEncoder.get()
hallState = self.Hall.get()
compressorState = self.Compressor.enabled()
solenoidStateOne = self.DoubleSolenoidOne.get()
solenoidStateTwo = self.DoubleSolenoidTwo.get()
solenoidStateThree = self.DoubleSolenoidThree.get()
# robot ultrasonic
self.ultraValue = self.ultrasonic.getVoltage()
# cargo ultrasonic
self.cargoUltraValue = self.cargoUltrasonic.getVoltage()
# xbox value states
xboxButtonStates = [
self.xbox.getRawButton(1),
self.xbox.getRawButton(2),
self.xbox.getRawButton(3),
self.xbox.getRawButton(4),
self.xbox.getRawButton(5),
self.xbox.getRawButton(6),
self.xbox.getRawButton(7),
self.xbox.getRawButton(8),
self.xbox.getRawButton(9),
self.xbox.getRawButton(10)
]
xboxAxisStates = [
self.xbox.getRawAxis(1),
self.xbox.getRawAxis(2),
self.xbox.getRawAxis(3),
self.xbox.getRawAxis(4),
self.xbox.getRawAxis(5)
]
# joystick value states
rJoystickButtonStates = [self.rightStick.getRawButton(1)]
rJoystickAxisStates = [
self.rightStick.getRawAxis(1),
self.rightStick.getRawAxis(2),
self.rightStick.getRawAxis(3)
]
lJoystickButtonStates = [self.leftStick.getRawButton(1)]
lJoystickAxisStates = [
self.leftStick.getRawAxis(1),
self.leftStick.getRawAxis(2),
self.leftStick.getRawAxis(3)
]
# define lift stages
def cargo_one():
if liftTicks <= 133: # Cargo 1
self.lift.set(0.5)
elif liftTicks > 133:
self.lift.set(0.05)
def cargo_two():
if liftTicks <= 270: # Cargo 2
self.lift.set(0.5)
elif liftTicks > 270:
self.lift.set(0.05)
def cargo_three():
if liftTicks <= 415: # Cargo 3
self.lift.set(0.5)
elif liftTicks > 415:
self.lift.set(0.05)
def hatch_one():
if liftTicks <= 96: # Hatch 1
self.lift.set(0.5)
elif liftTicks > 96:
self.lift.set(0.05)
def hatch_two():
if liftTicks <= 237: # Hatch 2
self.lift.set(0.5)
elif liftTicks > 237:
self.lift.set(0.05)
def hatch_three():
if liftTicks <= 378: # Hatch 3
self.lift.set(0.5)
elif liftTicks > 378:
self.lift.set(0.05)
def lift_encoder_reset():
self.lift.set(0.01)
if hallState is True:
self.liftEncoder.reset()
if self.buttonToggleStatus[0] is True:
cargo_three()
elif self.buttonToggleStatus[1] is True:
hatch_three()
elif self.buttonToggleStatus[2] is True:
cargo_two()
elif self.buttonToggleStatus[3] is True:
hatch_two()
elif self.buttonToggleStatus[4] is True:
cargo_one()
elif self.buttonToggleStatus[5] is True:
hatch_one()
elif self.buttonToggleStatus[6] is True:
lift_encoder_reset()
# compressor state
self.sd.putString("Compressor Status: ",
"Enabled" if compressorState is True else "Disabled")
# gear state
self.sd.putString(
"Gear Shift: ",
"High Speed" if solenoidStateOne is 1 else "Low Speed")
# ejector state
self.sd.putString(
"Ejector Pins: ",
"Ejected" if solenoidStateThree is 2 else "Retracted")
# claw state
self.sd.putString("Claw: ",
"Open" if solenoidStateTwo is 2 else "Closed")
# hatch station range state
self.sd.putString(
"PLAYER STATION RANGE: ",
"YES!!!!" if 0.142 <= self.ultraValue <= 0.146 else "NO!")
# self.sd.putNumber("Ultrasonic Voltage: ", self.ultraValue)
# hatch spaceship range
self.sd.putString(
"HATCH RANGE: ", "HATCH IN RANGE"
if 0.7 <= self.cargoUltraValue <= 1.56 else "NOT IN RANGE")
# compressor
if xboxButtonStates[8]:
self.Compressor.stop()
elif xboxButtonStates[9]:
self.Compressor.start()
elif rJoystickButtonStates[0]: # shift right
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kForward)
elif lJoystickButtonStates[0]: # shift left
self.DoubleSolenoidOne.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[2]: # open claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[1]: # close claw
self.DoubleSolenoidTwo.set(wpilib.DoubleSolenoid.Value.kReverse)
elif xboxButtonStates[3]: # eject
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kForward)
elif xboxButtonStates[0]: # retract
self.DoubleSolenoidThree.set(wpilib.DoubleSolenoid.Value.kReverse)
# lift control
if True in self.buttonToggleStatus is False:
if xboxButtonStates[4]: # hold
self.lift.set(0.05)
elif xboxAxisStates[2] > 0.1: # up
self.lift.set(xboxAxisStates[2] / 1.5)
elif xboxAxisStates[1] > 0.1: # down
self.lift.set(-xboxAxisStates[1] * 0.25)
else:
self.lift.set(0)
# four-bar control
if xboxButtonStates[5]:
self.liftArm.set(0.05)
elif not xboxButtonStates[5]:
self.liftArm.set(-xboxAxisStates[0] / 4.0)
else:
self.liftArm.set(0)
# cargo intake control
if xboxButtonStates[6]:
self.cargo.set(0.12)
elif xboxAxisStates[4]: # take in
self.cargo.set(xboxAxisStates[4] * 0.75)
# controller mapping for tank steering
rightAxis = rJoystickAxisStates[0]
leftAxis = lJoystickAxisStates[0]
# drives drive system using tank steering
if solenoidStateOne is 1: # if on high gear
divisor = 1.2 # then 90% of high speed
elif solenoidStateOne is 2: # if on low gear
divisor = 1.2 # then normal slow speed
else:
divisor = 1.0
leftSign = leftAxis / fabs(leftAxis) if leftAxis != 0 else 0
rightSign = rightAxis / fabs(rightAxis) if rightAxis != 0 else 0
self.drive.tankDrive(-leftSign * (1 / divisor) * (leftAxis**2),
-rightSign * (1 / divisor) * (rightAxis**2))
class Chassis:
left_master: WPI_TalonSRX
left_slave: WPI_TalonSRX
right_master: WPI_TalonSRX
right_slave: WPI_TalonSRX
navx: AHRS
def __init__(self):
self.y_speed = 0
self.z_speed = 0
self.left_speed = 0
self.right_speed = 0
self.arcade_mode = True
def setup(self):
self.left_slave.follow(self.left_master)
self.right_slave.follow(self.right_master)
self.left_master.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder)
self.right_master.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder)
self.left_master.configVoltageCompSaturation(11)
self.right_master.configVoltageCompSaturation(11)
self.left_master.enableVoltageCompensation(True)
self.right_master.enableVoltageCompensation(True)
self.diff_drive = DifferentialDrive(self.left_master,
self.right_master)
def arcade_drive(self, y_speed: float, z_speed: float):
self.diff_drive.arcadeDrive(y_speed, z_speed)
def tank_drive(self, left_speed: float, right_speed: float):
self.diff_drive.tankDrive(left_speed, right_speed)
def get_encoder_ticks(self):
left_pos = self.left_master.getSelectedSensorPosition()
right_pos = self.right_master.getSelectedSensorPosition()
return left_pos, right_pos
def set_motors_values(self, left: float, right: float):
self.left_master.set(ControlMode.PercentOutput, left)
self.right_master.set(ControlMode.PercentOutput, right)
def reset_encoders(self):
self.left_master.setSelectedSensorPosition(0)
self.right_master.setSelectedSensorPosition(0)
def get_angle(self):
return self.navx.getAngle()
def reset_angle(self):
self.navx.zeroYaw()
print("resetc")
PhysicsEngine.reset = True
def execute(self):
if self.arcade_mode:
self.arcade_drive(self.y_speed, self.z_speed)
else:
self.tank_drive(self.left_speed, self.right_speed)
def upadte_operator(self,
left_stick: Joystick,
right_stick: Joystick = None):
if right_stick is None:
self.arcade_mode = True
self.y_speed = -left_stick.getY()
self.z_speed = -left_stick.getZ()
else:
self.arcade_mode = True
self.left_speed = -left_stick.getY()
self.right_speed = -right_stick.getY()
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontLeftMotor = wpilib.Spark(1)
# self.middleLeftMotor = wpilib.Spark(4)
self.rearLeftMotor = wpilib.Spark(2)
self.frontRightMotor = wpilib.Spark(3)
# self.middleRightMotor = wpilib.Spark(1)
self.rearRightMotor = wpilib.Spark(4)
self.ihigh_motor = wpilib.Spark(6)
self.ilow_motor = wpilib.Spark(9)
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.high = 0
self.low = 0
self.gameData = ''
# 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)
self.gyro = wpilib.ADXRS450_Gyro()
def autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance(
).getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
global firstTime
firstTime = True
def autonomousPeriodic(self):
# This program tests 90 degree turn with gyro
global firstTime, maxV, minV
while firstTime:
global fD, fluc, sD, v
sD = self.gyro.getAngle()
fD = sD - 90
firstTime = False
v = 0.65
cD = self.gyro.getAngle()
# left smaller right bigger
if cD > fD:
cD = self.gyro.getAngle()
speed_turn = v
self.myRobot.tankDrive(-speed_turn, speed_turn)
print(cD)
else:
self.myRobot.tankDrive(0, 0)
def disabledInit(self):
self.myRobot.tankDrive(0, 0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
self.iSolenoid.set(1)
lastspeed = 0
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sp = forward - backward
if abs(self.Stick1.getX(0)) > 0.1:
steering = (self.Stick1.getX(0) + 0.1 * sp) / 1.5
else:
steering = 0
self.myRobot.tankDrive(sp + steering, sp - steering)
class Robot(wpilib.IterativeRobot):
WHEEL_DIAMETER = 0.1524 # Units: Meters
# Currently unused
# ENCODER_PULSE_PER_REV = 42
GEARING = 7.56 # 7.56:1 gear ratio
auto_speed_entry = ntproperty('/robot/autospeed', 0.0)
telemetry_entry = ntproperty('/robot/telemetry', [0.0], writeDefault=False)
rotate_entry = ntproperty('/robot/rotate', False)
l_encoder_pos = ntproperty('/l_encoder_pos', 0)
l_encoder_rate = ntproperty('/l_encoder_rate', 0)
r_encoder_pos = ntproperty('/r_encoder_pos', 0)
r_encoder_rate = ntproperty('/r_encoder_rate', 0)
def robotInit(self):
self.prior_autospeed = 0
self.joystick = wpilib.Joystick(0)
self.left_motor_master = CANSparkMax(1, MotorType.kBrushless)
self.right_motor_master = CANSparkMax(4, MotorType.kBrushless)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.left_motor_master,
CANSparkMax(3, MotorType.kBrushless)
)
self.right_motors = wpilib.SpeedControllerGroup(
self.right_motor_master,
CANSparkMax(6, MotorType.kBrushless)
)
# Configure Gyro
# Note that the angle from the NavX and all implementors of wpilib Gyro
# must be negated because getAngle returns a clockwise positive angle
self.gyro = navx.AHRS.create_spi()
# Configure drivetrain movement
self.drive = DifferentialDrive(self.left_motors, self.right_motors)
self.drive.setDeadband(0)
# Configure encoder related functions -- getDistance and getrate should return
# units and units/s
self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.left_motor_master.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.right_encoder = self.right_motor_master.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set the update rate instead of using flush because of a ntcore bug
# -> probably don't want to do this on a robot in competition
NetworkTables.getDefault().setUpdateRate(0.010)
def disabledInit(self):
print('Robot disabled')
self.drive.tankDrive(0, 0)
def robotPeriodic(self):
# feedback for users, but not used by the control program
self.l_encoder_pos = self.left_encoder.getPosition()
self.l_encoder_rate = self.left_encoder.getVelocity()
self.r_encoder_pos = self.right_encoder.getPosition()
self.r_encoder_rate = self.right_encoder.getVelocity()
def teleopInit(self):
print('Robot in operator control mode')
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.joystick.getY(), self.joystick.getX())
print(f'Left Distance: {self.left_encoder.getPosition()}')
def autonomousInit(self):
print('Robot in autonomous mode')
# 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.
def autonomousPeriodic(self):
# Retrieve values to send back before telling the motors to do somethin
now = wpilib.Timer.getFPGATimestamp()
leftPosition = self.left_encoder.getPosition()
leftRate = self.left_encoder.getVelocity()
rightPosition = self.right_encoder.getPosition()
rightRate = self.right_encoder.getVelocity()
battery = wpilib.RobotController.getInputVoltage()
motorVolts = battery * abs(self.prior_autospeed)
leftMotorVolts = motorVolts
rightMotorVolts = motorVolts
# Retrieve the commanded speed from NetworkTables
autospeed = self.auto_speed_entry
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive((-1 if self.rotate_entry else 1) * autospeed, autospeed, False)
# send telemetry data array back to NT
number_array = [
now, battery, autospeed, leftMotorVolts, rightMotorVolts,
leftPosition, rightPosition, leftRate, rightRate,
math.radians(-self.gyro.getAngle())
]
self.telemetry_entry = number_array
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 MyRobot(wpilib.IterativeRobot):
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 autonomousInit(self):
self.iSolenoid.set(2)
self.aSolenoidLow.set(2)
self.aSolenoidHigh.set(2)
self.gameData = wpilib.DriverStation.getInstance(
).getGameSpecificMessage()
global timer
timer = wpilib.Timer()
timer.start()
def autonomousPeriodic(self):
if self.gameData[0:1] == "L":
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5, 0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(0.7, 0.7)
#turn right, although looks like turn left
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
''' Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
elif self.gameData[0:1] == 'R':
while timer.get() < 0.3:
self.myRobot.tankDrive(0.5, 0.5)
while timer.get() < 1.2:
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.aSolenoidLow.set(1)
self.myRobot.tankDrive(
0.7, 0.7) # To make sure it's not accelerating to fast
while timer.get() < 3:
self.myRobot.tankDrive(
0, 0
) # This is problematic. Should have used "pass". Otherwise the motor stops immediately and the robot won't reach the distance.
timer.reset()
timer.start()
while timer.get(
) < 0.85: # Turn 90 degrees. Left = 0.82 was the original value we got during tests.
self.ihigh_motor.set(0.6) # intake
self.ilow_motor.set(-0.95) # intake
self.myRobot.tankDrive(
0.7, -0.7
) # Turing left, even though it looks like turning right
while timer.get() < 1.4:
self.myRobot.tankDrive(0, 0) # Should have used "pass".
while timer.get() < 1.9:
self.myRobot.tankDrive(0.6, 0.6)
while timer.get() < 2.2:
self.myRobot.tankDrive(0, 0) # Should have used "pass"
while timer.get() < 4.5: # Outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
self.iSolenoid.set(1)
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 4:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 6.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 7.5:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 8:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 9:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 9.5:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 9.2:
# self.aSolenoidLow.set(1)
# while timer.get() > 11 and timer.get() <= 14:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >14 and timer.get() <15:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
'''Waiting to be tested'''
# while timer.get() <= 1:
# self.myRobot.tankDrive(0.6,-0.6)
# while timer.get() <= 1.5:
# self.myRobot.tankDrive(0.6,0.6)
# while timer.get() <= 2.5:
# self.myRobot.tankDrive(-0.6,0.6)
# while timer.get() <= 3:
# self.myRobot.tankDrive(0.6,0.6)
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# if timer.get() <= 2.7:
# self.aSolenoidLow.set(1)
# while timer.get() > 4.5 and timer.get() <= 7.5:
# self.ihigh_motor.set(0.7)
# self.ilow_motor.set(-0.8)
# self.aSolenoidHigh.set(1)
# while timer.get() >7.5 and timer.get() <8.5:
# self.iSolenoid.set(1)
# self.ihigh_motor.set(-1)
# self.ilow_motor.set(1)
def disabledInit(self):
self.myRobot.tankDrive(0, 0)
self.iSolenoid.set(0)
self.aSolenoidLow.set(0)
self.aSolenoidHigh.set(0)
def disabledPeriodic(self):
pass
def teleopInit(self):
'''Execute at the start of teleop mode'''
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
if self.isOperatorControl() and self.isEnabled():
minv = 0.4
maxv = 0.6
forward = self.Stick1.getTriggerAxis(1)
backward = self.Stick1.getTriggerAxis(0)
sp = forward - backward
steering = self.Stick1.getX(0)
mod = minv + maxv * ((1 - abs(sp))**2)
r = (
steering**3
) * mod # To make it harder to turn when speed is large in order to assure a smooth drive and prevent drifting
if sp >= 0: # May look weird but it's correct.
self.myRobot.tankDrive(sp * 0.85 - r, sp * 0.85 + r)
else:
self.myRobot.tankDrive(sp * 0.85 + r, (sp * 0.85 - r))
# intake and outtake
if self.Stick2.getRawButton(11) == True: # intake
self.ihigh_motor.set(0.6)
self.ilow_motor.set(-0.95)
if self.Stick2.getRawButton(
11) == False and self.Stick2.getRawButton(12) == False:
self.ihigh_motor.set(0)
self.ilow_motor.set(0)
if self.Stick2.getRawButton(12) == True: # outtake
self.ihigh_motor.set(-0.8)
self.ilow_motor.set(0.8)
if self.Stick2.getRawButton(9) == True:
self.aSolenoidLow.set(1)
self.iSolenoid.set(1)
if self.Stick2.getRawButton(10) == True:
self.aSolenoidLow.set(2)
if self.Stick2.getRawButton(7) == True:
self.aSolenoidHigh.set(1)
if self.Stick2.getRawButton(8) == True:
self.aSolenoidHigh.set(2)
if self.Stick2.getRawButton(3) == True:
self.iSolenoid.set(1) # push intake
if self.Stick2.getRawButton(4) == True:
self.iSolenoid.set(2) # pull intake
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(wp.IterativeRobot):
def robotInit(self):
'''Robot initialization function'''
self.leftMotor1 = wp.VictorSP(1) #motor initialization
self.leftMotor2 = wp.VictorSP(3)
self.leftMotor3 = wp.VictorSP(5)
self.rightMotor1 = wp.VictorSP(2)
self.rightMotor2 = wp.VictorSP(4)
self.rightMotor3 = wp.VictorSP(6)
self.armMotor = wp.VictorSP(7)
self.leftIntakeMotor = wp.VictorSP(8)
self.rightIntakeMotor = wp.VictorSP(9)
self.leftSide = wp.SpeedControllerGroup(
self.leftMotor1, self.leftMotor2,
self.leftMotor3) #speed controller groups
self.rightSide = wp.SpeedControllerGroup(self.rightMotor1,
self.rightMotor2,
self.rightMotor3)
self.intakeMotors = wp.SpeedControllerGroup(self.leftIntakeMotor,
self.rightIntakeMotor)
self.myRobot = DifferentialDrive(self.leftSide, self.rightSide)
self.myRobot.setExpiration(0.1)
self.leftStick = wp.Joystick(0)
self.rightStick = wp.Joystick(1)
self.armStick = wp.XboxController(2)
self.gyro = wp.ADXRS450_Gyro(0)
self.rightEncd = wp.Encoder(2, 3)
self.leftEncd = wp.Encoder(0, 1)
self.armPot = wp.AnalogPotentiometer(0, 270, -135)
self.compressor = wp.Compressor()
self.shifter = wp.DoubleSolenoid(0, 1)
wp.SmartDashboard.putNumber("Straight Position", 15000)
wp.SmartDashboard.putNumber("leftMiddleAutoStraight", 13000)
wp.SmartDashboard.putNumber("leftMiddleAutoLateral", 14000)
wp.SmartDashboard.putNumber("Left Switch Pos 1", 18000)
wp.SmartDashboard.putNumber("Left Switch Angle", 90)
wp.SmartDashboard.putNumber("Left Switch Pos 2", 5000)
wp.SmartDashboard.putNumber("Switch Score Arm Position", 70)
wp.SmartDashboard.putNumber("Front Position 1", 74)
wp.SmartDashboard.putNumber("Front Position 2", 16)
wp.SmartDashboard.putNumber("Front Position 3", -63)
wp.SmartDashboard.putNumber("lvl2 Position 1", 60)
wp.SmartDashboard.putNumber("lvl2 Position 3", -50)
wp.SmartDashboard.putNumber("lvl3 Position 3", -38)
wp.SmartDashboard.putNumber("lvl3 Position 1", 45)
wp.SmartDashboard.putBoolean("switchScore?", False)
self.chooser = wp.SendableChooser()
self.chooser.addDefault("None", 4)
self.chooser.addObject("Straight/Enc", 1)
self.chooser.addObject("Left Side Switch Auto", 2)
self.chooser.addObject("Right Side Switch Auto (switch)", 3)
self.chooser.addObject("Center Auto", 5)
wp.SmartDashboard.putData("Choice", self.chooser)
wp.CameraServer.launch("vision.py:main")
def robotPeriodic(self):
wp.SmartDashboard.putNumber("Gyro:", round(self.gyro.getAngle(), 2))
wp.SmartDashboard.putNumber("Right Encoder:", self.rightEncd.get())
wp.SmartDashboard.putNumber("Left Encoder:", self.leftEncd.get())
wp.SmartDashboard.putNumber("Arm Postition", self.armPot.get())
calGyro = wp.SmartDashboard.getBoolean("calGyro:", True)
resetGyro = wp.SmartDashboard.getBoolean("resetGyro:", True)
encodeReset = wp.SmartDashboard.getBoolean("resetEnc:", True)
self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True)
self.straightPos = wp.SmartDashboard.getNumber("Straight Position",
4000)
self.leftAutoPos1 = wp.SmartDashboard.getNumber(
"Left Switch Pos 1", 4000)
self.leftAutoPos2 = wp.SmartDashboard.getNumber(
"Left Switch Angle", 90)
self.leftAutoPos3 = wp.SmartDashboard.getNumber(
"Left Switch Pos 2", 4000)
self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78)
self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.frontArmPos3 = wp.SmartDashboard.getNumber(
"Front Position 3", -63)
self.frontArmLvl2Pos1 = wp.SmartDashboard.getNumber(
"lvl2 Position 1", 68)
self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber(
"lvl2 Position 3", -50)
self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber(
"lvl3 Position 3", -38)
self.frontArmLvl3Pos1 = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
if (resetGyro):
self.gyro.reset()
wp.SmartDashboard.putBoolean("resetGyro:", False)
if (calGyro):
self.gyro.calibrate()
wp.SmartDashboard.putBoolean("calGyro:", False)
if (encodeReset):
self.rightEncd.reset()
self.leftEncd.reset()
wp.SmartDashboard.putBoolean("resetEnc:", False)
self.auto = self.chooser.getSelected()
def autonomousInit(self):
self.auto = self.chooser.getSelected()
self.leftMotorVal = 0
self.rightMotorVal = 0
self.gyroFuncGain = 40
self.angleFuncGain = 40
self.leftMotVal = 0
self.rightMotVal = 0
self.armSet = 0
self.intakeSet = 0
self.posGain = 18
self.armSpeed = 1
self.armPos = self.armPot.get()
self.startTime = tm.time()
self.gamePlacement = wp.DriverStation.getInstance(
).getGameSpecificMessage()
self.straightPos = wp.SmartDashboard.getNumber("Straight Position",
16000)
self.leftAutoPos1 = wp.SmartDashboard.getNumber(
"Left Switch Pos 1", 4000)
self.leftAutoPos2 = wp.SmartDashboard.getNumber(
"Left Switch Angle", 90)
self.leftAutoPos3 = wp.SmartDashboard.getNumber(
"Left Switch Pos 2", 4000)
self.leftMiddleAutoStraight = wp.SmartDashboard.getNumber(
"leftMiddleAutoStraight", 21)
self.leftMiddleAutoLateral = wp.SmartDashboard.getNumber(
"leftMiddleAutoLateral", 21)
self.switchScoreArmPos = wp.SmartDashboard.getNumber(
"Switch Score Arm Position", 70)
self.frontArmPos1 = wp.SmartDashboard.getNumber("Front Position 1", 78)
self.frontArmPos2 = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.frontArmPos3 = wp.SmartDashboard.getNumber(
"Front Position 3", -63)
self.switchScore = wp.SmartDashboard.getBoolean("switchScore?", True)
self.StartButtonPos = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
self.stage1 = True
self.stage2 = False
self.stage3 = False
self.stage4 = False
self.stage5 = False
self.stage6 = False
def autonomousPeriodic(self):
#autonomous code will go here
armPos = self.armPot.get()
self.gamePlacement = wp.DriverStation.getInstance(
).getGameSpecificMessage()
if (self.auto == 1):
if (abs(self.rightEncd.get()) < self.straightPos and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.switchScore and self.gamePlacement != ""
and self.gamePlacement == "RRR"
or self.gamePlacement == "RRL"
or self.gamePlacement == "RLR"
or self.gamePlacement == "RLL"):
self.intakeSet = -1
else:
self.instakeSet = 0
if (self.auto == 2):
#Guidlines for switch placement on left side:
#check switch placement
#Move Forward
#Turn right 90 degrees
#Move Forward to wall
#place Cube
if (self.gamePlacement != "" and self.gamePlacement == "LLL"
or self.gamePlacement == "LRL"
or self.gamePlacement == "LLR"
or self.gamePlacement == "LRR"):
if (abs(self.rightEncd.get()) < self.leftAutoPos1
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() < self.leftAutoPos2 + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), self.leftAutoPos2,
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftAutoPos3
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), self.leftAutoPos2, 0.65,
self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) <= self.leftAutoPos1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.auto == 3):
if (self.gamePlacement != "" and self.gamePlacement == "RRR"
or self.gamePlacement == "RRL"
or self.gamePlacement == "RLR"
or self.gamePlacement == "RLL"):
if (abs(self.rightEncd.get()) < self.leftAutoPos1
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1),
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftAutoPos3
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65,
self.gyroFuncGain)
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) <= self.leftAutoPos1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
if (self.auto == 5):
if (self.gamePlacement != "" and self.gamePlacement == "LLL"
or self.gamePlacement == "LRL"
or self.gamePlacement == "LLR"
or self.gamePlacement == "LRR"):
if (abs(self.rightEncd.get()) <
(self.leftMiddleAutoStraight / 2) and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
self.stage2 = True
elif (self.gyro.getAngle() > (self.leftAutoPos2 * -1) + 0.1
and self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1),
self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage3 = True
elif (abs(self.rightEncd.get()) < self.leftMiddleAutoLateral
and self.stage3):
self.stage2 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), (self.leftAutoPos2 * -1), 0.65,
self.gyroFuncGain)
self.stage4 = True
elif (self.gyro.getAngle() < (0) + 0.1 and self.stage4):
self.stage3 = False
self.leftMotVal, self.rightMotVal = rf.angleFunc(
self.gyro.getAngle(), 0, self.angleFuncGain)
self.leftEncd.reset()
self.rightEncd.reset()
self.stage5 = True
elif (abs(self.rightEncd.get()) <
(self.leftMiddleAutoStraight / 1.50) and self.stage5):
self.stage4 = False
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.armSet = 0
else:
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
else:
if (abs(self.rightEncd.get()) < self.straightPos
and self.stage1):
self.leftMotVal, self.rightMotVal = rf.gyroFunc(
self.gyro.getAngle(), 0, 0.65, self.gyroFuncGain)
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.armSet = 0
if (abs(self.rightEncd.get()) > (self.straightPos - 1000)):
self.intakeSet = -1
self.stage2 = True
elif (self.stage2):
self.stage1 = False
self.leftMotVal, self.rightMotVal = 0, 0
self.intakeSet = -1
self.leftIntakeMotor.set(self.intakeSet)
self.rightIntakeMotor.set(self.intakeSet * -1)
self.armMotor.set(self.armSet)
self.myRobot.tankDrive(self.rightMotVal, self.leftMotVal)
def teleopInit(self):
self.myRobot.setSafetyEnabled(True)
self.pastFrontFlipButton = False
self.flip = True
self.armSet = 0
self.shiftSet = 1
self.posGain = 27
self.posGain2 = 27
self.armSpeed = 1
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastLBumper = False
self.lastRBumper = False
self.lastStartButton = False
self.XButtonPos = wp.SmartDashboard.getNumber("Front Position 1", 77.6)
self.YButtonPos = wp.SmartDashboard.getNumber("Front Position 2", 22)
self.BButtonPos = wp.SmartDashboard.getNumber("Front Position 3",
-62.8)
self.AButtonPos = wp.SmartDashboard.getNumber("lvl2 Position 1", 30)
self.frontArmLvl2Pos3 = wp.SmartDashboard.getNumber(
"lvl2 Position 3", -50)
self.frontArmLvl3Pos3 = wp.SmartDashboard.getNumber(
"lvl3 Position 3", -38)
self.StartButtonPos = wp.SmartDashboard.getNumber(
"lvl3 Position 1", 57)
#wp.SmartDashboard.getNumber("Back Position 1", -90)
#wp.SmartDashboard.getNumber("Back Position 2", -45)
#wp.SmartDashboard.getNumber("Back Position 3", -30)
def teleopPeriodic(self):
#Joystick Variables
leftJoyValY = self.leftStick.getY()
rightJoyValY = self.rightStick.getY()
armJoyValY = self.armStick.getRawAxis(3)
frontFlipButton = self.rightStick.getRawButton(2)
armPos = self.armPot.get()
highShiftButton = self.rightStick.getRawButton(4)
lowShiftButton = self.rightStick.getRawButton(5)
self.compressorButtonOn = self.rightStick.getRawButton(9)
self.compressorButtonOff = self.rightStick.getRawButton(8)
self.intakeInButton = self.armStick.getRawButton(7)
self.intakeOutButton = self.armStick.getRawButton(8)
self.buttonX = self.armStick.getRawButton(1)
self.buttonY = self.armStick.getRawButton(4)
self.buttonB = self.armStick.getRawButton(3)
self.buttonA = self.armStick.getRawButton(2)
self.buttonStart = self.armStick.getRawButton(10)
self.buttonLBumper = self.armStick.getRawButton(5)
self.buttonRBumper = self.armStick.getRawButton(6)
#Automatic arm positioning
if (self.buttonX == True and self.lastXButton == False):
self.lastXButton = True
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = False
if (self.buttonY == True and self.lastYButton == False):
self.lastXButton = False
self.lastYButton = True
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = False
if (self.buttonB == True and self.lastBButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = True
self.lastAButton = False
self.lastStartButton = False
if (self.buttonA and self.lastAButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = True
self.lastStartButton = False
if (self.buttonStart and self.lastStartButton == False):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
self.lastAButton = False
self.lastStartButton = True
if (armJoyValY > 0.075 or armJoyValY < -0.075):
self.lastXButton = False
self.lastYButton = False
self.lastBButton = False
if (self.lastXButton):
if (armPos < (self.XButtonPos - 1) or armPos >
(self.XButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.XButtonPos,
self.armSpeed, self.posGain)
else:
self.lastXButton = False
elif (self.lastYButton):
if (armPos < (self.YButtonPos - 1) or armPos >
(self.YButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.YButtonPos,
self.armSpeed, self.posGain)
else:
self.lastYButton = False
elif (self.lastBButton):
if (armPos < (self.BButtonPos - 1) or armPos >
(self.BButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.BButtonPos,
self.armSpeed, self.posGain)
else:
self.lastBButton = False
elif (self.lastAButton):
if (armPos < (self.AButtonPos - 1) or armPos >
(self.AButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.AButtonPos,
self.armSpeed, self.posGain)
else:
self.lastAButton = False
elif (self.lastStartButton):
if (armPos < (self.StartButtonPos - 1) or armPos >
(self.StartButtonPos + 1)):
self.armSet = rf.goToPos(armPos, self.StartButtonPos,
self.armSpeed, self.posGain)
else:
self.lastBButton = False
else:
self.armSet = (armJoyValY * 0.75)
#Intake Motor Control
if (self.intakeInButton):
self.intakeSet = 1
elif (self.intakeOutButton):
self.intakeSet = -1
else:
self.intakeSet = 0
if (highShiftButton):
self.shiftSet = 1
elif (lowShiftButton):
self.shiftSet = 2
#FrontFlip
if (self.pastFrontFlipButton == False and frontFlipButton):
self.flip = not self.flip
self.pastFrontFlipButton = frontFlipButton
leftMotVal, rightMotVal = rf.flip(self.flip, leftJoyValY, rightJoyValY)
self.armMotor.set(self.armSet)
self.shifter.set(self.shiftSet)
self.leftIntakeMotor.set(self.intakeSet)
self.rightIntakeMotor.set(self.intakeSet * -1)
self.myRobot.tankDrive(rightMotVal, leftMotVal)