def config_talon(talon: WPI_TalonSRX, motor_config: TalonMotorConfig): # when deploying it might be a somewhat good idea to actually uncomment this next line talon.configFactoryDefault() talon.enableVoltageCompensation(True) talon.configVoltageCompSaturation(motor_config.voltage_saturation) talon.enableCurrentLimit(True) talon.configPeakCurrentLimit(motor_config.peak_current) talon.configContinuousCurrentLimit(motor_config.continuous_current) talon.setNeutralMode(motor_config.default_mode) talon.configNeutralDeadband(motor_config.deadband) # talon.configOpenLoopRamp(motor_config.ramp_rate) talon.setInverted(motor_config.reversed)
class CargoMech(): kSlotIdx = 0 kPIDLoopIdx = 0 kTimeoutMs = 10 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 intake(self, mode): #Intake/Outtake/Stop, based on the mode it changes the speed of the motor if mode == "intake": self.motor.set(0.9) elif mode == "outtake": self.motor.set(-0.9) elif mode == "stop": self.motor.set(0) def moveWrist(self, mode): #move wrist in and out of robot '''replace modes up and down with pid and set angles''' if mode == "up": self.input = (self.getPowerSimple("up")) + self.getGravity() self.wrist.set(self.getPowerSimple("up") + self.getGravity()) elif mode == "down": self.input = (-1 * self.getPowerSimple("down")) + self.getGravity() self.wrist.set((-1 * self.getPowerSimple("down")) + self.getGravity()) elif mode == "upVolts": self.input = (self.wristUpVolts / self.maxVolts) + self.getGravity() self.wrist.set((self.wristUpVolts / self.maxVolts) + self.getGravity()) elif mode == "downVolts": self.input = (-1 * self.wristDownVolts / self.maxVolts) + self.getGravity() self.wrist.set((-1 * self.wristDownVolts / self.maxVolts) + self.getGravity()) elif mode == "stop": self.input = 0 self.wrist.set(0) else: #self.input = self.getGravity#() #self.input = self.getNumber("#Wrist Power Set", 0) self.input = 0 self.wrist.set(self.input) #self.cargoController.setSetpoint(self.getAngle()) #self.cargoController.enable() #self.wrist.set(self.gPower) '''replace pid here with kF''' def periodic(self): #0.4 as a deadband if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.intake("intake") elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4: self.intake("outtake") else: self.intake("stop") if self.xbox.getRawButton(map.wristUp): self.moveWrist("up") elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down") elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts") elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts") else: self.moveWrist("gravity") #disables intake def disable(self): self.intake("stop") #gets the angle, used in other support functions def getAngle(self): pos = self.getPosition() angle = abs(pos * 115 / self.targetPosDown) return (angle - 25) '''cargo mech vertical is 0 degrees''' '''cargo mech down completely is 90 degrees''' '''cargo mech has range of -25 to 85 degrees''' def getGravity(self): '''angle = self.getAngle() power = 0 if angle < -20: power = 0 elif angle > 80: power = 0 else: power = math.sin(math.radians(angle)) / 2 return power''' return 0 def getPosition(self): return self.wrist.getQuadraturePosition() def getFeedForward(self, gain): angle = self.getAngle() return angle * gain def __setGravity__(self, output): self.gPower = output def getPowerSimple(self, direction): '''true direction is up into robot false direction is down out of robot''' angle = self.getAngle() power = abs(self.simpleGain) if angle > 80: if direction == "down": power = 0 #if angle < -20: # if direction == "up": # power = 0 return power def setWristPower(self, power): self.wrist.set(power) def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): #self.wristUp = self.getNumber("WristUpSpeed" , 0.5) #self.wristDown = self.getNumber("WristDownSpeed" , 0.2) #self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) #self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) #self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain) #self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity) self.kP = self.getNumber("Wrist kP", 0) self.kI = self.getNumber("Wrist kI", 0) self.kD = self.getNumber("Wrist kD", 0) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putNumber("Wrist Angle", self.getAngle()) #SmartDashboard.putNumber("Wrist Power", self.input) SmartDashboard.putNumber("Wrist Power Up", self.getPowerSimple("up")) SmartDashboard.putNumber("Wrist Power Down", self.getPowerSimple("down"))
class CargoMech(): 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 setIntake(self, mode): #Intake/Outtake/Stop, based on the mode it changes the speed of the motor if mode == "intake": self.intake.set(0.9) elif mode == "outtake": self.intake.set(-0.9) else: self.intake.set(0) def setWrist(self, mode): [setpoint, error] = [10, 5] mult = abs( self.getAngle() - 50 ) / 100 + 0.5 #increase constant if the arm is not moving enough close to the setpoint if mode == "rocket" and self.getAngle() < setpoint - error: self.moveWrist(self.wristDownVolts / self.maxVolts * mult, mode) elif mode == "rocket" and self.getAngle() > setpoint + error: self.moveWrist(self.wristUpVolts / self.maxVolts * mult, mode) elif mode == "up": self.moveWrist(self.wristUpVolts / self.maxVolts, mode) elif mode == "down": self.moveWrist(self.wristDownVolts / self.maxVolts, mode) elif mode == "rocket" or mode == "gravity": self.moveWrist(0, mode) else: self.moveWrist(-self.getGravity(), "stop") def moveWrist(self, power, mode): self.out = self.wristBounds(power + self.getGravity(), mode) self.wrist.set(self.out) def wristBounds(self, power, mode): angle = self.getAngle() if angle > 80 and (mode == "down" or mode == "gravity"): power = 0 elif angle < -20 and (mode == "up" or mode == "gravity"): power = 0 elif angle < 10 and mode == "gravity": power = self.F #if operator lets go of buttons while at top, arm goes back to upper limit sqitch return power def getGravity(self): return math.sin(math.radians(self.getAngle())) * self.F def getAngle(self): pos = 0 if self.wrist.isRevLimitSwitchClosed(): pos = self.range self.wrist.setQuadraturePosition(pos) else: pos = self.getPosition() angle = abs(pos * 115 / self.range) return (angle - 25) ''' def getAngle(self): return self.angle ''' def getPosition(self): return self.wrist.getQuadraturePosition() def periodic(self): if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.setIntake("intake") elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4: self.setIntake("outtake") else: self.setIntake("stop") if self.xbox.getPOV() >= 0 and self.povPressed: self.povPressed = False elif self.xbox.getPOV() >= 0 and not self.povPressed: self.povPressed = True if self.povPressed: self.setWrist("rocket") elif self.xbox.getRawButton(map.wristUp): self.setWrist("up") elif self.xbox.getRawButton(map.wristDown): self.setWrist("down") else: self.setWrist("gravity") #disables intake def disable(self): self.setIntake("stop") self.setWrist("stop") def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): #commented out some values. DON'T DELETE THESE VALUES #SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putNumber("Wrist Angle", self.getAngle()) #SmartDashboard.putNumber("Output", self.out) #self.F = SmartDashboard.getNumber("F Gain", 0) #self.wristUpVolts = SmartDashboard.getNumber("Wrist Up Volts", 0) #self.wristDownVolts = SmartDashboard.getNumber("Wrist Down Volts", 0) SmartDashboard.putBoolean("Limit Switch", self.wrist.isFwdLimitSwitchClosed()) SmartDashboard.putBoolean("Limit Switch Reverse", self.wrist.isRevLimitSwitchClosed())
class CargoMech(): kSlotIdx = 0 kPIDLoopIdx = 0 kTimeoutMs = 10 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 intake(self, mode): ''' Intake/Outtake/Stop Intake the cargo (turn wheels inward)''' if mode == "intake": self.motor.set(self.intakeSpeed) elif mode == "outtake": self.motor.set(-1 * self.intakeSpeed) elif mode == "stop": self.motor.set(0) def moveWrist(self,mode): '''move wrist in and out of robot''' if mode == "up": self.wrist.set(self.getPowerSimple("up")) elif mode == "down": self.wrist.set(-1 * self.getPowerSimple("down")) elif mode == "upVolts": self.wrist.set(self.wristUpVolts/self.maxVolts) elif mode == "downVolts": self.wrist.set(-1 * self.wristDownVolts/ self.maxVolts) elif mode == "stop": self.wrist.set(0) else: self.wrist.set(self.getGravitySimple()) def periodic(self): deadband = 0.4 if self.xbox.getRawAxis(map.intakeCargo)>deadband: self.intake("intake") elif self.xbox.getRawAxis(map.outtakeCargo)>deadband: self.intake("outtake") else: self.intake("stop") if self.xbox.getRawButton(map.wristUp): self.moveWrist("up") elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down") elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts") elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts") else: self.moveWrist("gravity") def disable(self): self.intake("stop") def getAngle(self): pos = self.getPosition() angle = abs(pos * 115/self.targetPosDown) return angle - 25 def getPosition(self): return self.wrist.getQuadraturePosition() def getFeedForward(self, gain): angle = self.getAngle() return angle*gain def getPowerSimple(self, direction): '''true direction is up into robot false direction is down out of robot''' angle = self.getAngle() power = abs(self.simpleGainGravity * math.sin(math.radians(angle)) + self.simpleGain) if angle > 80: if direction == "down": power = 0 if angle < -20: if direction == "up": power = 0 return power def getGravitySimple(self): angle = self.getAngle() power = self.simpleGainGravity * math.sin(math.radians(angle)) return power def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): self.wristUp = self.getNumber("WristUpSpeed" , 0.5) self.wristDown = self.getNumber("WristDownSpeed" , 0.2) self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain) self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putNumber("Wrist Angle" , self.getAngle()) SmartDashboard.putNumber("Wrist Power Up" , self.getPowerSimple("up")) SmartDashboard.putNumber("Wrist Power Down" , self.getPowerSimple("down"))
class CargoMech(): 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) def setIntake(self, mode): #Intake/Outtake/Stop, based on the mode it changes the speed of the motor if mode == "intake": self.intake.set(0.9) elif mode == "outtake": self.intake.set(-0.9) else: self.intake.set(0) def setWrist(self, mode): if mode == "up": self.out = self.wristUpVolts / self.maxVolts elif mode == "down": self.out = self.wristDownVolts / self.maxVolts else: self.out = 0 self.setWristPower(self.out) def setWristPower(power): self.wrist.set(power) def periodic(self): if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.setIntake("intake") elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4: self.setIntake("outtake") else: self.setIntake("stop") if self.xbox.getRawButton(map.wristUp): self.setWrist("up") elif self.xbox.getRawButton(map.wristDown): self.setWrist("down") else: self.setWrist("stop") #disables intake def disable(self): self.setIntake("stop") self.setWrist("stop") def dashboardInit(self): pass def dashboardPeriodic(self): SmartDashboard.putNumber("Cargomech Wrist Output", self.out) self.wristPowerSet = SmartDashboard.getNumber("Wrist Power Set", 0)
class CargoMech(): kSlotIdx = 0 kPIDLoopIdx = 0 kTimeoutMs = 10 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 intake(self, mode): ''' Intake/Outtake/Stop Intake the cargo (turn wheels inward)''' if mode == "intake": self.motor.set(self.intakeSpeed) elif mode == "outtake": self.motor.set(-1 * self.intakeSpeed) elif mode == "stop": self.motor.set(0) def moveWrist(self,mode): '''move wrist in and out of robot''' if mode == "up": self.wrist.set(self.getPowerSimple("up")) elif mode == "down": self.wrist.set(-1 * self.getPowerSimple("down")) elif mode == "upVolts": self.wrist.set(self.wristUpVolts/self.maxVolts) elif mode == "downVolts": self.wrist.set(-1 * self.wristDownVolts/ self.maxVolts) elif mode == "upMagic": if self.lastMode != mode: 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) # Motion Magic - 4096 ticks/rev * 10 Rotations in either direction self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosUp) # append more signals to print when in speed mode. self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx)) self.sb.append("\ttrg: %.3f" % self.targetPosUp) elif mode == "downMagic": if self.lastMode != mode: 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) # Motion Magic - 4096 ticks/rev * 10 Rotations in either direction self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosDown) # append more signals to print when in speed mode. self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx)) self.sb.append("\ttrg: %.3f" % self.targetPosDown) elif mode == "stop": self.wrist.set(0) else: self.wrist.set(self.getGravitySimple()) self.lastMode = mode def periodic(self): deadband = 0.4 if self.xbox.getRawAxis(map.intakeCargo)>deadband: self.intake("intake") elif self.xbox.getRawAxis(map.outtakeCargo)>deadband: self.intake("outtake") else: self.intake("stop") if self.xbox.getRawButton(map.wristUp): self.moveWrist("up") elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down") elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts") elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts") elif self.joystick0.getRawButton(map.wristUpMagic): self.moveWrist("upMagic") elif self.joystick0.getRawButton(map.wristDownMagic): self.moveWrist("downMagic") else: self.moveWrist("gravity") # calculate the percent motor output motorOutput = self.wrist.getMotorOutputPercent() self.sb = [] # prepare line to print self.sb.append("\tOut%%: %.3f" % motorOutput) self.sb.append("\tVel: %.3f" % self.wrist.getSelectedSensorVelocity(self.kPIDLoopIdx)) # instrumentation self.processInstrumentation(self.wrist, self.sb) def disable(self): self.intake("stop") def processInstrumentation(self, tal, sb): # smart dash plots wpilib.SmartDashboard.putNumber("SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx)) wpilib.SmartDashboard.putNumber("SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx)) wpilib.SmartDashboard.putNumber("MotorOutputPercent", tal.getMotorOutputPercent()) wpilib.SmartDashboard.putNumber("ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx)) # check if we are motion-magic-ing if tal.getControlMode() == Talon.ControlMode.MotionMagic: self.timesInMotionMagic += 1 else: self.timesInMotionMagic = 0 if self.timesInMotionMagic > 10: # print the Active Trajectory Point Motion Magic is servoing towards wpilib.SmartDashboard.putNumber( "ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx) ) if not wpilib.RobotBase.isSimulation(): wpilib.SmartDashboard.putNumber( "ActTrajVelocity", tal.getActiveTrajectoryVelocity() ) wpilib.SmartDashboard.putNumber( "ActTrajPosition", tal.getActiveTrajectoryPosition() ) wpilib.SmartDashboard.putNumber( "ActTrajHeading", tal.getActiveTrajectoryHeading() ) # periodically print to console self.loops += 1 if self.loops >= 10: self.loops = 0 print(" ".join(self.sb)) # clear line cache self.sb.clear() def getAngle(self): pos = self.getPosition() angle = abs(pos * 115/self.targetPosDown) return angle - 25 def getPosition(self): return self.wrist.getQuadraturePosition() def getFeedForward(self, gain): angle = self.getAngle() return angle*gain def getPowerSimple(self, direction): '''true direction is up into robot false direction is down out of robot''' angle = self.getAngle() power = abs(self.simpleGainGravity * math.sin(math.radians(angle)) + self.simpleGain) if angle > 80: if direction == "down": power = 0 if angle < -20: if direction == "up": power = 0 return power def getGravitySimple(self): angle = self.getAngle() power = self.simpleGainGravity * math.sin(math.radians(angle)) return power def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): self.wristUp = self.getNumber("WristUpSpeed" , 0.5) self.wristDown = self.getNumber("WristDownSpeed" , 0.2) self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) 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) self.simpleGain = self.getNumber("Wrist Simple Gain", 0.5) self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", 0.07) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putNumber("Wrist Angle" , self.getAngle()) SmartDashboard.putNumber("Wrist Power Up" , self.getPowerSimple("up")) SmartDashboard.putNumber("Wrist Power Down" , self.getPowerSimple("down"))
class CargoMech(): kSlotIdx = 0 kPIDLoopIdx = 0 kTimeoutMs = 10 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], } def intake(self, mode): #Intake/Outtake/Stop, based on the mode it changes the speed of the motor if mode == "intake": self.motor.set(0.9) elif mode == "outtake": self.motor.set(-0.9) elif mode == "stop": self.motor.set(0) def pidWrite(self, output): maxPower = 0.3 if output < -maxPower: output = -maxPower elif output > maxPower: output = maxPower self.wrist.set(output) self.input = output def pidGet(self): return self.getAngle() def setPIDSourceType(self): pass def getPIDSourceType(self): return 0 def moveWrist(self, mode): if mode != self.lastCargoCommand: self.lastCargoCommand = mode if mode in self.pidValuesForMode: array = self.pidValuesForMode[mode] self.cargoController.setP(array[1]) self.cargoController.setI(array[2]) self.cargoController.setD(array[3]) self.cargoController.setF( math.sin(self.getAngle() / 180 * math.pi) * self.F) self.cargoController.setSetpoint(array[0]) self.cargoController.enable() else: self.cargoController.disable() elif (mode not in self.pidValuesForMode): self.wrist.set(math.sin(self.getAngle() / 180 * math.pi) * self.F) self.input2 = math.sin(self.getAngle() / 180 * math.pi) * self.F def periodic(self): #0.4 as a deadband if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.intake("intake") elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4: self.intake("outtake") else: self.intake("stop") if self.xbox.getPOV() == 0: self.moveWrist("resting") self.lastCargoCommand = "resting" elif self.xbox.getPOV() == 90: self.moveWrist("cargoship") self.lastCargoCommand = "cargoship" elif self.xbox.getPOV() == 180: self.moveWrist("intake") self.lastCargoCommand = "intake" elif self.xbox.getPOV() == 270: self.moveWrist("rocket") self.lastCargoCommand = "rocket" else: self.moveWrist(self.lastCargoCommand) #disables intake def disable(self): self.intake("stop") #gets the angle, used in other support functions def getAngle(self): pos = self.getPosition() angle = abs(pos * 115 / self.targetPosDown) return (angle - 20) def getPosition(self): return self.wrist.getQuadraturePosition() def getNumber(self, key, defVal): val = SmartDashboard.getNumber(key, None) if val == None: val = defVal SmartDashboard.putNumber(key, val) return val def dashboardInit(self): pass def dashboardPeriodic(self): #self.wristUp = self.getNumber("WristUpSpeed" , 0.5) #self.wristDown = self.getNumber("WristDownSpeed" , 0.2) #self.wristUpVolts = self.getNumber("WristUpVoltage" , 5) #self.wristDownVolts = self.getNumber("WristDownVoltage" , 2) #self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain) #self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity) SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition()) SmartDashboard.putData("PID Controller", self.cargoController) SmartDashboard.putNumber("Wrist Angle", self.getAngle()) SmartDashboard.putNumber("Wrist Power", self.input) SmartDashboard.putNumber("Wrist Power2", self.input2) SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand) SmartDashboard.putBoolean("Wrist PinState Quad A", self.wrist.getPinStateQuadA()) SmartDashboard.putBoolean("Wrist PinState Quad B", self.wrist.getPinStateQuadB()) self.F = SmartDashboard.getNumber("F Gain", 0)