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], }