def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.F = 0.25
SmartDashboard.putNumber("F Gain", self.F)
#self.angle = 0
#SmartDashboard.putNumber("angle", self.angle)
self.range = -1200
self.povPressed = False
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
SmartDashboard.putNumber("Wrist Up Volts", self.wristUpVolts)
SmartDashboard.putNumber("Wrist Down Volts", self.wristDownVolts)
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
def initialize(self):
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.joystick1 = oi.getJoystick(1)
self.kicker = Solenoid(map.hatchKick)
self.slider = Solenoid(map.hatchSlide)
self.kick("in")
self.slide("in")
self.lastKick = False
self.lastSlide = False
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def robotInit(self):
'''Robot-wide initialization code should go here'''
#start camera
wpilib.CameraServer.launch()
Command.getRobot = lambda x=0: self
#Variables that are used by the code
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.intake = Intake()
self.table = networktables.NetworkTables.getTable("String")
self.joystick = getJoystick()
self.joystick1 = getJoystick1()
#self.auto = driveForward.DriveForward(16)
#self.auto = AutoEncoders(20)
#self.auto = ProfiledForward(10)
self.auto = StateSpaceDriveCommand("straight3m.tra")
#self.auto = autonomous.Autonomuscc()
#self.auto = AutoEncoders()
self.elevatorSwitch = ElevatorSwitch()
self.elevatorScale = ElevatorScale()
self.elevatorIntake = ElevatorIntake()
self.driveForward = driveForward.DriveForward(10)
def robotInit(self):
'''Robot-wide initialization code should go here'''
#start camera
wpilib.CameraServer.launch()
Command.getRobot = lambda x=0: self
#Variables that are used by the code
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.intake = Intake()
self.table = networktables.NetworkTables.getTable("String")
self.joystick = getJoystick()
#self.auto = autonomous.ForwardOnly()
self.auto = autonomous.SwitchCommands()
#self.auto = TurnRight(90)
#self.auto = Wait9ThenForward()
#self.auto = RightPosRightSwitchAuto()
#self.auto = AutoEncodersTurnLeft(90)
#self.auto = MiddlePosLeftSwitchAuto()
self.elevatorSwitch = ElevatorSwitch()
self.elevatorScale = ElevatorScale()
self.elevatorIntake = ElevatorIntake()
self.driveForward = driveForward.DriveForward(10)
def __init__(self, name, position):
super().__init__(name)
self.joystick = getJoystick()
self.elevator = self.getRobot().elevator
self.requires(self.elevator)
self.position = position
self.diff = 500
def robotInit(self):
"""This function initiates the robot's components and parts"""
# Here we create a function for the command class to return the robot
# instance, so that we don't have to import the robot module for each
# command.
Command.getRobot = lambda _: self
# This launches the camera server between the robot and the computer
wpilib.CameraServer.launch()
self.joystick = wpilib.Joystick(0)
self.lr_motor = ctre.WPI_TalonSRX(1)
self.lf_motor = ctre.WPI_TalonSRX(2)
self.rr_motor = ctre.WPI_TalonSRX(5)
self.rf_motor = ctre.WPI_TalonSRX(6)
self.left = wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor)
self.right = wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor)
self.drivetrain_solenoid = wpilib.DoubleSolenoid(2, 3)
self.drivetrain_gyro = wpilib.AnalogGyro(1)
# Here we create the drivetrain as a whole, combining all the different
# robot drivetrain compontents.
self.drivetrain = drivetrain.Drivetrain(self.left, self.right,
self.drivetrain_solenoid,
self.drivetrain_gyro,
self.rf_motor)
self.l_gripper = wpilib.VictorSP(0)
self.r_gripper = wpilib.VictorSP(1)
self.grippers = grippers.Grippers(self.l_gripper, self.r_gripper)
self.elevator_motor = wpilib.VictorSP(2)
self.elevator_top_switch = wpilib.DigitalInput(4)
self.elevator_bot_switch = wpilib.DigitalInput(5)
self.elevator = elevator.Elevator(self.elevator_motor,
self.elevator_top_switch,
self.elevator_bot_switch)
self.handles_solenoid = wpilib.DoubleSolenoid(0, 1)
self.handles = handles.Handles(self.handles_solenoid)
# This creates the instance of the autonomous program that will run
# once the autonomous period begins.
self.autonomous = AutonomousProgram()
# This gets the instance of the joystick with the button function
# programmed in.
self.josytick = oi.getJoystick()
def execute(self):
joystick = getJoystick()
closeArm_trigger = joystick.getRawAxis(3) #Right Trigger
openArm_trigger = joystick.getRawAxis(2) #Left Trigger
if (closeArm_trigger < 0): # right trigger triggered
self.intake.motor_closeOpen_set(-1) #set to full reverse power
elif (openArm_trigger < 0): #left trigger triggered
self.intake.motor_closeOpen_set(1) #set to full forward power
else:
self.intake.motor_closeOpen_set(0) #turn off motor
def __init__(self, setpoint):
super().__init__(p=.003, i=0.0001, d=0.06, period=.010)
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
pid = self.getPIDController()
pid.setInputRange(minimumInput=-180, maximumInput=180)
pid.setOutputRange(minimumOutput=-1, maximumOutput=1)
self.setSetpoint(setpoint)
self.logger = None
self.joystick = getJoystick()
self.pdp = wpilib.PowerDistributionPanel(16)
def initialize(self):
#makes control objects
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.joystick1 = oi.getJoystick(1)
#makes solenoid objects to be used in kick and slide functions
self.kicker = Solenoid(map.hatchKick)
self.slider = Solenoid(map.hatchSlide)
self.maxVolts = 10
timeout = 0
self.wheels = Talon(map.hatchWheels)
self.wheels.setNeutralMode(2)
self.wheels.configVoltageCompSaturation(self.maxVolts)
self.wheels.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wheels.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wheels.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wheels.enableCurrentLimit(True)
self.wheels.setInverted(True)
self.powerIn = 0.9
self.powerOut = -0.9
#sets kicker and slide solenoids to in
self.kick("in")
self.slide("in")
#starts lastkick/slide booleans
self.lastKick = False
self.lastSlide = False
self.hasHatch = False
self.outPower = 0
def robotInit(self):
'''Robot-wide initialization code should go here'''
Command.getRobot = lambda x=0: self
#Variables that are used by the code
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.intake = Intake()
self.table = networktables.NetworkTables.getTable("String")
self.joystick = getJoystick()
self.angle = turnlikeistuesday.Turnlikeistuesday(90)
self.DriveForward = driveForward.DriveForward()
'''
def initialize(self):
timeout = 15
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.motor = Talon(map.intake)
self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.intakeSpeed = 0.9
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def robotInit(self):
'''
This is a good place to set up your subsystems and anything else that
you will need to access later.
'''
Command.getRobot = lambda x=0: self
self.motor = singlemotor.SingleMotor()
self.autonomousProgram = AutonomousProgram()
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
'''
self.joystick = oi.getJoystick()
def initialize(self):
timeout = 15
self.wristPowerSet = 0
SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet)
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
self.xbox = oi.getJoystick(2)
self.out = 0
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
self.bottomSwitch = DI(map.bottomSwitch)
self.topSwitch = DI(map.topSwitch)
def execute(self):
joystick = getJoystick()
fw = joystick.getRawAxis(1)
lr = joystick.getRawAxis(0)
fw2 = joystick.getRawAxis(5)
lr2 = joystick.getRawAxis(4)
if abs(fw2) > 0.2 or abs(lr2) > 0.2:
self.drivetrain.drive.arcadeDrive(fw2 * .3,
lr2 * .5,
squaredInputs=False)
elif abs(fw) > 0.2 or abs(lr) > 0.2:
self.drivetrain.drive.arcadeDrive(fw, lr, squaredInputs=False)
else:
self.drivetrain.drive.arcadeDrive(0, 0)
def robotInit(self):
"""
This is a good place to set up your subsystems and anything else that
you will need to access later.
"""
Command.getRobot = lambda x=0: self
self.doublemotor = doublemotor.DoubleMotor()
self.singlemotor = singlemotor.SingleMotor()
self.drive = ArcadeDrive.Arcade()
self.autonomousProgram = AutonomousProgram()
"""
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
"""
self.joystick = oi.getJoystick()
def robotInit(self):
'''
Since OI instantiates commands and commands need access to subsystems,
OI must be initialized after subsystems.
So - instantiate in this order
1) Subsystems
2) Commands
3) OI (operator interface)
'''
Command.getRobot = lambda x=0: self
#Instantiate a subsystem
#self.autonomousProgram = somecommand(self.logger)
#self.teleopProgram = somecommand(self.logger)
self.joystick = oi.getJoystick()
def robotInit(self):
'''Robot-wide initialization code should go here'''
Command.getRobot = lambda x=0: self
#Variables that are used by the code
self.startSide = "l" #starting side
self.gamecode = "rlr" #wpilib.DriverStation.getGameSpecificMessage()
self.drivetrain = Drivetrain()
self.elevator = Elevator()
self.intake = Intake()
self.table = networktables.NetworkTables.getTable("String")
self.joystick = getJoystick()
#self.angle = turnlikeistuesday.Turnlikeistuesday(90)
self.angle = turn_profiled.TurnProfiled(90)
self.auto = driveForward.DriveForward(10)
'''self.elevatorZero = elevatorZero.elevatorZero()'''
#self.driveForward = driveForward.DriveForward(10)
self.driveForward = automous.Test()
'''
def execute(self):
joystick = getJoystick()
turnOffWheels = True
closeArm_trigger = joystick.getRawAxis(3) #Right Trigger
openArm_trigger = joystick.getRawAxis(2) #Left Trigger
if abs(closeArm_trigger) > 0.1: # right trigger triggered
self.intake.closeGrabber(closeArm_trigger)
elif abs(openArm_trigger) > 0.1: #left trigger triggered
self.intake.openGrabber()
elif joystick.getRawButton(3):
self.intake.open2Grabber()
turnOffWheels = False
else:
self.intake.grabberOff()
if joystick.getRawButton(5):
self.intake.cubeOut()
elif joystick.getRawButton(6):
self.intake.cubeIn()
elif turnOffWheels:
self.intake.intakeWheelsOff()
def periodic(self):
self.updateSensors()
if oi.getJoystick(0).getRawButton(1):
self.switch = True
else:
self.switch = False
def initialize(self):
timeout = 15
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.1
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.05
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.motor = Talon(map.intake)
self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.intakeSpeed = 0.9
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#choose sensor
self.wrist.configSelectedFeedbackSensor(
Talon.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,)
self.wrist.setSensorPhase(False) #!!!!!
# Set relevant frame periods to be at least as fast as periodic rate
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs)
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs)
# set the peak and nominal outputs
self.wrist.configNominalOutputForward(0, self.kTimeoutMs)
self.wrist.configNominalOutputReverse(0, self.kTimeoutMs)
self.wrist.configPeakOutputForward(0.6, self.kTimeoutMs)
self.wrist.configPeakOutputReverse(-0.25, self.kTimeoutMs)
self.kF = self.getFeedForward(self.getNumber("Wrist F Gain" , 0))
self.kP = self.getNumber("Wrist kP" , 0)
self.kI = self.getNumber("Wrist kI" , 0)
self.kD = self.getNumber("Wrist kD" , 0)
# set closed loop gains in slot0 - see documentation */
self.wrist.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.wrist.config_kF(0, self.kF, self.kTimeoutMs)
self.wrist.config_kP(0, self.kP, self.kTimeoutMs)
self.wrist.config_kI(0, 0, self.kTimeoutMs)
self.wrist.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.wrist.configMotionCruiseVelocity(400, self.kTimeoutMs) #!!!!!
self.wrist.configMotionAcceleration(500, self.kTimeoutMs) #!!!!!
# zero the sensor
self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def initialize(self, robot):
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
if map.robotId == map.astroV1:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10, 0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backBottomSensor)
self.frontSwitch = DigitalInput(map.frontBottomSensor)
self.upSwitch = DigitalInput(map.upSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = 0 #degrees
self.climbSpeed = 0.5
self.wheelSpeed = 0.5
self.backHold = -0.15 #holds back stationary if extended ADJUST**
self.frontHold = -0.1 #holds front stationary if extended
self.kP = 0.4 #proportional gain for angle to power
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def __init__(self):
super().__init__('ElevatorTest')
self.elevator = self.getRobot().elevator
self.joystick = getJoystick()
self.requires(self.elevator)
def initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.input = 0
self.input2 = 0
self.lastCargoCommand = "unknown"
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
self.F = 0
#should be 0.4
SmartDashboard.putNumber("F Gain", self.F)
[self.kP, self.kI, self.kD, self.kF] = [0, 0, 0, 0]
cargoController = PIDController(self.kP, self.kI, self.kD, self.kF,
self, self)
self.cargoController = cargoController
self.cargoController.disable()
self.pidValuesForMode = {
"resting": [-50, self.kP, self.kI, self.kD, 0.15 / -50],
"cargoship": [-28, self.kP, self.kI, self.kD, 0.0],
"intake": [50, self.kP, self.kI, self.kD, 0.0],
"rocket": [5, self.kP, self.kI, self.kD, 0.19 / 5],
}
