class Arm(Subsystem):
"""Raise and lower the robot's arm."""
def __init__(self):
"""Assign ports and save them for use in the move and stop methods."""
super().__init__()
self.arm = ctre.WPI_TalonSRX(11)
self.armencoder = Encoder(0, 1)
self.armencoder.setDistancePerPulse(0.14)
def move(self, value):
"""Move the arm according to the left and right Xbox
controller triggers."""
if self.armencoder.getDistance() < 40:
self.arm.set(value)
if value > 0:
direction = "up"
elif value < 0:
direction = "down"
else:
direction = "stopped"
print("Arm moving", direction, "at", value)
print("Arm angle is " + "%3f" % self.armencoder.getDistance())
def stop(self):
"""Stop the arm."""
self.arm.set(0.0)
class Encoder(Sensor):
"""
Sensor wrapper for a quadrature encoder.
Has double attributes distance, rate (distance/second);
boolean attributes stopped and direction.
"""
distance = rate = 0
stopped = direction = True
def __init__(self, channel_a, channel_b, pulse_dist=1.0,
reverse=False, modnum=1, cpr=128,
enctype=CounterBase.EncodingType.k4X):
"""
Initializes the encoder with two channels,
distance per pulse (usu. feet, default 1), no reversing,
on module number 1, 128 CPR, and with 4x counting.
"""
super().__init__()
self.e = WEncoder(channel_a, channel_b, reverse, enctype)
self.cpr = cpr
self.e.setDistancePerPulse(pulse_dist)
def poll(self):
self.distance = self.e.getDistance()
self.rate = self.e.getRate()
self.stopped = self.e.getStopped()
self.direction = self.e.getDirection()
class Arm(Subsystem):
"""Raise and lower the robot's arm."""
def __init__(self):
"""Assign ports and save them for use in the move and stop methods."""
super().__init__()
self.arm = ctre.WPI_TalonSRX(11)
self.armencoder = Encoder(0, 1)
self.armencoder.setDistancePerPulse(0.07)
def move(self, value):
"""Move the arm according to the left and right Xbox
controller triggers."""
if self.armencoder.getDistance() > -120:
self.arm.set(value)
else:
self.arm.set(0.05)
print("Arm angle is " + "%3f" % self.armencoder.getDistance())
def stop(self):
"""Stop the arm."""
self.arm.set(0.0)
class Drive:
def __init__(self, Robot):
#self.navx = Robot.navx
self.speedLimit = 0.999
self._leftRamp = 0.0
self._rightRamp = 0.0
self._rampSpeed = 6.0
self._kOonBalanceAngleThresholdDegrees = 5.0
self._autoBalance = True
self._quickstop_accumulator = 0.0
self._old_wheel = 0.0
self._driveReversed = True
self._drivePool = DataPool("Drive")
# setup drive train motors
self.rightDrive = SyncGroup(RIGHT_DRIVE_MOTOR_IDS, WPI_VictorSPX)
self.leftDrive = SyncGroup(LEFT_DRIVE_MOTOR_IDS, WPI_VictorSPX)
self.rightDrive.setInverted(True)
# setup drive train gear shifter
self.shifter = Solenoid(DRIVE_SHIFTER_PORT)
self.shifter.set(False)
# setup drive train encoders
self.leftEncoder = Encoder(LEFT_DRIVE_ENCODER_A, LEFT_DRIVE_ENCODER_B,
False, Encoder.EncodingType.k4X)
self.rightEncoder = Encoder(RIGHT_DRIVE_ENCODER_A,
RIGHT_DRIVE_ENCODER_B, False,
Encoder.EncodingType.k4X)
self.leftEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK)
self.leftEncoder.setReverseDirection(True)
self.rightEncoder.setReverseDirection(False)
self.rightEncoder.setDistancePerPulse(DRIVE_INCHES_PER_TICK)
def cheesyDrive(self, wheel, throttle, quickturn, altQuickturn, shift):
throttle = Util.deadband(throttle)
wheel = Util.deadband(wheel)
if self._driveReversed:
wheel *= -1
neg_inertia = wheel - self._old_wheel
self._old_wheel = wheel
wheel = Util.sinScale(wheel, 0.9, 1, 0.9)
if wheel * neg_inertia > 0:
neg_inertia_scalar = 2.5
else:
if abs(wheel) > .65:
neg_inertia_scalar = 6
else:
neg_inertia_scalar = 4
neg_inertia_accumulator = neg_inertia * neg_inertia_scalar
wheel += neg_inertia_accumulator
if altQuickturn:
if abs(throttle) < 0.2:
alpha = .1
self._quickstop_accumulator = (
1 -
alpha) * self._quickstop_accumulator + alpha * self.limit(
wheel, 1.0) * 5
over_power = -wheel * .75
angular_power = -wheel * 1
elif quickturn:
if abs(throttle) < 0.2:
alpha = .1
self._quickstop_accumulator = (
1 -
alpha) * self._quickstop_accumulator + alpha * self.limit(
wheel, 1.0) * 5
over_power = 1
angular_power = -wheel * 1
else:
over_power = 0
sensitivity = .9
angular_power = throttle * wheel * sensitivity - self._quickstop_accumulator
self._quickstop_accumulator = Util.wrap_accumulator(
self._quickstop_accumulator)
if shift:
if not self.shifter.get():
self.shifter.set(True)
else:
if self.shifter.get():
self.shifter.set(False)
right_pwm = left_pwm = throttle
left_pwm += angular_power
right_pwm -= angular_power
if left_pwm > 1:
right_pwm -= over_power * (left_pwm - 1)
left_pwm = 1
elif right_pwm > 1:
left_pwm -= over_power * (right_pwm - 1)
right_pwm = 1
elif left_pwm < -1:
right_pwm += over_power * (-1 - left_pwm)
left_pwm = -1
elif right_pwm < -1:
left_pwm += over_power * (-1 - right_pwm)
right_pwm = -1
if self._driveReversed:
left_pwm *= -1
right_pwm *= -1
if self.shifter.get(): # if low gear
#leftDrive.set(left_pwm)
#rightDrive.set(right_pwm)
self.moveRamped(left_pwm, right_pwm)
else:
self.moveRamped(left_pwm, right_pwm)
def setGear(self, gear):
if self.shifter.get() != gear:
self.shifter.set(gear)
def tankDrive(self, left, right):
scaledBalance = self.autoBalance()
left = self.limit(left + scaledBalance, self.speedLimit)
right = self.limit(right + scaledBalance, self.speedLimit)
self.leftDrive.set(left * LEFT_DRIFT_OFFSET)
self.rightDrive.set(right * RIGHT_DRIFT_OFFSET)
def limit(self, wheel, d):
return Util.limit(wheel, d)
def moveRamped(self, desiredLeft, desiredRight):
self._leftRamp += (desiredLeft - self._leftRamp) / self._rampSpeed
self._rightRamp += (desiredRight - self._rightRamp) / self._rampSpeed
self.tankDrive(self._leftRamp, self._rightRamp)
def autoShift(self, b):
if self.shifter.get() != b:
self.shifter.set(b)
def periodic(self):
self._drivePool.logDouble("gyro_angle", self.getRotation())
self._drivePool.logDouble("left_enc", self.rightEncoder.getDistance())
self._drivePool.logDouble("right_enc", self.leftEncoder.getDistance())
def setDrivetrainReversed(self, rev):
self.driveReversed = rev
def driveReversed(self):
return self.driveReversed
def getRotation(self):
return self.navx.getAngle()
def getLeftDistance(self):
return self.leftEncoder.getDistance() * 2.54 * ROBOT_INVERTED
def getRightDistance(self):
return self.rightEncoder.getDistance() * 2.54 * ROBOT_INVERTED
def resetDistance(self):
self.leftEncoder.reset()
self.rightEncoder.reset()
def autoBalance(self):
'''if self._autoBalance:
pitchAngleDegrees = self.navx.getPitch()
scaledPower = 1 + (0 - pitchAngleDegrees - self._kOonBalanceAngleThresholdDegrees) / self._kOonBalanceAngleThresholdDegrees
if scaledPower > 2:
scaledPower = 2
#return scaledPower;'''
return 0
def setSpeedLimit(self, speedLimit):
self.speedLimit = self.limit(speedLimit, DRIVE_SPEED_LIMIT)
class Lift:
def __init__(self, Robot):
self._intake = Robot.intake
self._drive = Robot.drive
self._ramp = 0
self._rampSpeed = 6
self._LiftMotorLeft = SyncGroup(LIFT_MOTOR_LEFT_IDS, WPI_VictorSPX)
self._LiftMotorRight = SyncGroup(LIFT_MOTOR_RIGHT_IDS, WPI_VictorSPX)
self._LiftMotorLeft.setInverted(True)
self._liftEncoder = Encoder(LIFT_ENCODER_A, LIFT_ENCODER_B, False,
Encoder.EncodingType.k4X)
self._liftEncoder.setDistancePerPulse(1)
self._liftHallaffect = DigitalInput(HALL_DIO)
self.zeroEncoder()
self._liftPID = MiniPID(*LIFT_PID)
self._liftPID.setOutputLimits(-0.45, 1)
self.disableSpeedLimit = False
def setSpeed(self, speed):
self._LiftMotorLeft.set(speed)
self._LiftMotorRight.set(speed)
def rampSpeed(self, speed):
self._ramp += (speed - self._ramp) / self._rampSpeed
if False and speed > 0: #is max limit hit
self._ramp = 0
self._LiftMotorLeft.set(self._ramp)
self._LiftMotorRight.set(self._ramp)
def setLoc(self, target):
target = abs(target)
if target > 1:
target = 1
elif target <= 0.1:
target = 0
SmartDashboard.putNumber("loc dfliusafusd", target)
self._liftPID.setSetpoint(target)
def periodic(self):
if self.isBottom(): #zero switch is active zero encoder
self.zeroEncoder()
else:
if self._intake.getSpeed() >= 0:
self._intake.rampSpeed(0.3)
scaledLift = self.getEncoderVal() / LIFT_HEIGHT
speed = self._liftPID.getOutput(scaledLift)
if not self.disableSpeedLimit:
speedLimit = pow(0.25, scaledLift)
self._drive.setSpeedLimit(speedLimit)
else:
self._drive.setSpeedLimit(1)
self.rampSpeed(speed)
def getEncoderVal(self):
return abs(self._liftEncoder.getDistance())
def zeroEncoder(self):
self._liftEncoder.reset()
def isBottom(self):
return not self._liftHallaffect.get()
class Drivetrain(SubsystemBase):
def __init__(self):
super().__init__()
# Create the motor controllers and their respective speed controllers.
self.leftMotors = SpeedControllerGroup(
PWMSparkMax(constants.kLeftMotor1Port),
PWMSparkMax(constants.kLeftMotor2Port),
)
self.rightMotors = SpeedControllerGroup(
PWMSparkMax(constants.kRightMotor1Port),
PWMSparkMax(constants.kRightMotor2Port),
)
# Create the differential drivetrain object, allowing for easy motor control.
self.drive = DifferentialDrive(self.leftMotors, self.rightMotors)
# Create the encoder objects.
self.leftEncoder = Encoder(
constants.kLeftEncoderPorts[0],
constants.kLeftEncoderPorts[1],
constants.kLeftEncoderReversed,
)
self.rightEncoder = Encoder(
constants.kRightEncoderPorts[0],
constants.kRightEncoderPorts[1],
constants.kRightEncoderReversed,
)
# Configure the encoder so it knows how many encoder units are in one rotation.
self.leftEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
self.rightEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
# Create the gyro, a sensor which can indicate the heading of the robot relative
# to a customizable position.
self.gyro = ADXRS450_Gyro()
# Create the an object for our odometry, which will utilize sensor data to
# keep a record of our position on the field.
self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d())
# Reset the encoders upon the initilization of the robot.
self.resetEncoders()
def periodic(self):
"""
Called periodically when it can be called. Updates the robot's
odometry with sensor data.
"""
self.odometry.update(
self.gyro.getRotation2d(),
self.leftEncoder.getDistance(),
self.rightEncoder.getDistance(),
)
def getPose(self):
"""Returns the current position of the robot using it's odometry."""
return self.odometry.getPose()
def getWheelSpeeds(self):
"""Return an object which represents the wheel speeds of our drivetrain."""
speeds = DifferentialDriveWheelSpeeds(self.leftEncoder.getRate(),
self.rightEncoder.getRate())
return speeds
def resetOdometry(self, pose):
""" Resets the robot's odometry to a given position."""
self.resetEncoders()
self.odometry.resetPosition(pose, self.gyro.getRotation2d())
def arcadeDrive(self, fwd, rot):
"""Drive the robot with standard arcade controls."""
self.drive.arcadeDrive(fwd, rot)
def tankDriveVolts(self, leftVolts, rightVolts):
"""Control the robot's drivetrain with voltage inputs for each side."""
# Set the voltage of the left side.
self.leftMotors.setVoltage(leftVolts)
# Set the voltage of the right side. It's
# inverted with a negative sign because it's motors need to spin in the negative direction
# to move forward.
self.rightMotors.setVoltage(-rightVolts)
# Resets the timer for this motor's MotorSafety
self.drive.feed()
def resetEncoders(self):
"""Resets the encoders of the drivetrain."""
self.leftEncoder.reset()
self.rightEncoder.reset()
def getAverageEncoderDistance(self):
"""
Take the sum of each encoder's traversed distance and divide it by two,
since we have two encoder values, to find the average value of the two.
"""
return (self.leftEncoder.getDistance() +
self.rightEncoder.getDistance()) / 2
def getLeftEncoder(self):
"""Returns the left encoder object."""
return self.leftEncoder
def getRightEncoder(self):
"""Returns the right encoder object."""
return self.rightEncoder
def setMaxOutput(self, maxOutput):
"""Set the max percent output of the drivetrain, allowing for slower control."""
self.drive.setMaxOutput(maxOutput)
def zeroHeading(self):
"""Zeroes the gyro's heading."""
self.gyro.reset()
def getHeading(self):
"""Return the current heading of the robot."""
return self.gyro.getRotation2d().getDegrees()
def getTurnRate(self):
"""Returns the turning rate of the robot using the gyro."""
# The minus sign negates the value.
return -self.gyro.getRate()
class DriveTrainSubsystem(Subsystem):
def __init__(self):
self.robot = SpartanRobot.getRobotObject(self)
self.setDefaultCommand(ArcadeDriveCommand())
#Power output to motors in range of -1 to 1
self.leftPower = 0
self.rightPower = 0
self.leftEncoder = Encoder(RobotMap.leftDriveEncoder1, RobotMap.leftDriveEncoder2, False,
Encoder.EncodingType.k4X)
self.rightEncoder = Encoder(RobotMap.rightDriveEncoder1, RobotMap.rightDriveEncoder2, True,
Encoder.EncodingType.k4X)
self.gyro = ADXRS450_Gyro()
self.rightDriveMotor1 = VictorSP(RobotMap.rightDriveMotor1)
self.rightDriveMotor2 = VictorSP(RobotMap.rightDriveMotor2)
# self.rightDriveMotor3 = VictorSP(rightDriveMotor3)
self.leftDriveMotor1 = VictorSP(RobotMap.leftDriveMotor1)
self.leftDriveMotor2 = VictorSP(RobotMap.leftDriveMotor2)
# self.leftDriveMotor3 = VictorSP(leftDriveMotor3)
self.leftEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks)
self.rightEncoder.setDistancePerPulse(RobotMap.wheelCircumference / RobotMap.numberOfTicks)
self.leftEncoder.setMaxPeriod(5)
self.rightEncoder.setMaxPeriod(5)
self.leftEncoder.setMinRate(0)
self.rightEncoder.setMinRate(0)
self.leftEncoder.setSamplesToAverage(1)
self.rightEncoder.setSamplesToAverage(1)
self.gyro.calibrate()
def setLeftDrivePower(self, power):
self.leftPower = power
def setRightDrivePower(self, power):
self.rightPower = power
def updateMotorOutputs(self):
self.leftDriveMotor1 = -self.leftPower
self.leftDriveMotor2 = -self.leftPower
# self.leftDriveMotor3 = -self.leftPower
self.rightDriveMotor1 = self.rightPower
self.rightDriveMotor2 = self.rightPower
# self.rightDriveMotor3 = self.rightPower
def putEncoderValues(self):
SmartDashboard.putNumber("Left Encoder Raw", self.leftEncoder.getRaw())
SmartDashboard.putNumber("Right Encoder Raw", self.rightEncoder.getRaw())
SmartDashboard.putNumber("Left Encoder Dist Per Pulse", self.leftEncoder.getDistancePerPulse())
SmartDashboard.putNumber("Right Encoder Dist Per Pulse", self.rightEncoder.getDistancePerPulse())
def getLeftDistance(self):
return self.leftEncoder.getDistance()
def getRightDistance(self):
return self.rightEncoder.getDistance()
def resetGyro(self):
self.gyro.reset()
def resetEncoders(self):
self.leftEncoder.reset()
self.rightEncoder.reset()
def accelerateDrive(self):
if self.robot.oi.xBoxController.getRawAxis(1) > 0:
return math.pow(self.robot.oi.xBoxController.getRawAxis(1), 2)
else:
return -math.pow(self.robot.oi.xBoxController.getRawAxis(1), 2)
class Drive(Subsystem):
distPID = 0
anglePID = 0
prevDist = [0, 0]
maxSpeed = 0.85
yaw = 0
pitch = 0
roll = 0
leftVal = 0
rightVal = 0
leftConv = 6 / 12 * math.pi / 256
rightConv = -6 / 12 * math.pi / 256
def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.driveStyle = "Tank"
SmartDashboard.putString("DriveStyle", self.driveStyle)
#SmartDashboard.putData("Mode", self.mode)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
self.zero()
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
self.k = 1
self.sensitivity = 1
SmartDashboard.putNumber("K Value", self.k)
SmartDashboard.putNumber("sensitivity", self.sensitivity)
self.prevLeft = 0
self.prevRight = 0
def setGains(self, p, i, d, f):
self.distController.setPID(p, i, d, f)
def setGainsAngle(self, p, i, d, f):
self.angleController.setPID(p, i, d, f)
def periodic(self):
self.updateSensors()
def __getDistance__(self):
return self.getAvgDistance()
def __setDistance__(self, output):
self.distPID = output
def __getAngle__(self):
return self.getAngle()
def __setAngle__(self, output):
self.anglePID = output
def setMode(self, mode, name=None, distance=0, angle=0):
self.distPID = 0
self.anglePID = 0
if (mode == "Angle"):
self.angleController.setSetpoint(angle)
self.distController.disable()
self.angleController.enable()
elif (mode == "Combined"):
self.distController.setSetpoint(distance)
self.angleController.setSetpoint(angle)
self.distController.enable()
self.angleController.enable()
self.prevLeft = 0
self.prevRight = 0
elif (mode == "Direct"):
self.distController.disable()
self.angleController.disable()
self.mode = mode
def setAngle(self, angle):
self.setMode("Angle", angle=angle)
def setCombined(self, distance, angle):
self.setMode("Combined", distance=distance, angle=angle)
def setDirect(self):
self.setMode("Direct")
def sign(self, num):
if (num > 0): return 1
if (num == 0): return 0
return -1
def tankDrive(self, left=0, right=0):
if (self.mode == "Angle"):
nom = self.nominalPIDAngle
if self.anglePID < 0:
nom = -nom
left = self.getMaximum(self.anglePID, nom)
right = self.getMaximum(-self.anglePID, -nom)
elif (self.mode == "Combined"):
nom = self.nominalPID
if self.distPID < 0:
nom = -nom
left = self.getMaximum(self.distPID + self.anglePID, nom)
right = self.getMaximum(self.distPID - self.anglePID, nom)
left = self.getMinimum3(left, 0.30, self.prevLeft + 0.01)
right = self.getMinimum3(right, 0.30, self.prevRight + 0.01)
self.prevLeft = left
self.prevRight = right
elif (self.mode == "Direct"):
[left, right] = [left, right]
else:
[left, right] = [0, 0]
left = min(abs(left), self.maxSpeed) * self.sign(left)
right = min(abs(right), self.maxSpeed) * self.sign(right)
self.__tankDrive__(left, right)
def __tankDrive__(self, left, right):
deadband = 0.1
if (abs(left) > abs(deadband)): self.left.set(left)
else: self.left.set(0)
if (abs(right) > abs(deadband)): self.right.set(right)
else: self.right.set(0)
def getOutputCurrent(self):
return (self.right.getOutputCurrent() +
self.left.getOutputCurrent()) * 3
def updateSensors(self):
if self.navx == None:
self.yaw = 0
self.pitch = 0
self.roll = 0
else:
self.yaw = self.navx.getYaw()
self.pitch = self.navx.getPitch()
self.roll = self.navx.getRoll()
self.leftVal = self.leftEncoder.get()
self.rightVal = self.rightEncoder.get()
def getAngle(self):
angle = self.yaw
if RobotBase.isSimulation(): return -angle
if self.robot.teleop:
if angle == 0:
angle = 0
elif angle < 0:
angle = angle + 180
else:
angle = angle - 180
return angle
def getRoll(self):
return self.roll
def getPitch(self):
return self.pitch
def getRaw(self):
return [self.leftVal, self.rightVal]
def getDistance(self):
return [self.leftVal * self.leftConv, self.rightVal * self.rightConv]
def getAvgDistance(self):
return (self.getDistance()[0] + self.getDistance()[1]) / 2
def getVelocity(self):
dist = self.getDistance()
velocity = [
self.robot.frequency * (dist[0] - self.prevDist[0]),
self.robot.frequency * (dist[1] - self.prevDist[1])
]
self.prevDist = dist
return velocity
def getAvgVelocity(self):
return (self.getVelocity()[0] + self.getVelocity()[1]) / 2
def getAvgAbsVelocity(self):
return (abs(self.getVelocity()[0]) + abs(self.getVelocity()[1])) / 2
def zeroEncoders(self):
self.leftEncoder.reset()
self.rightEncoder.reset()
simComms.resetEncoders()
def zeroNavx(self):
if self.navx == None: pass
else: self.navx.zeroYaw()
def zero(self):
self.zeroEncoders()
self.zeroNavx()
def disable(self):
self.__tankDrive__(0, 0)
def initDefaultCommand(self):
self.setDefaultCommand(SetSpeedDT(timeout=300))
def dashboardInit(self):
SmartDashboard.putData("Turn Angle", TurnAngle())
SmartDashboard.putData("Drive Combined", DriveStraight())
def getMaximum(self, number, comparison):
if math.fabs(number) > math.fabs(comparison): return number
else: return comparison
def getMinimum3(self, val1, val2, val3):
return self.getMinimum(self.getMinimum(val1, val2), val3)
def getMinimum(self, number, comparison):
if math.fabs(number) < math.fabs(comparison): return number
else: return comparison
def isCargoPassed(self):
if self.getAvgDistance() > 16.1: return True
else: return False
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
SmartDashboard.putNumber("Left Counts", self.leftEncoder.get())
SmartDashboard.putNumber("Left Distance",
self.leftEncoder.getDistance())
SmartDashboard.putNumber("Right Counts", self.rightEncoder.get())
#SmartDashboard.putNumber("Right Distance", self.rightEncoder.getDistance())
SmartDashboard.putNumber("DT_DistanceAvg", self.getAvgDistance())
SmartDashboard.putNumber("DT_DistanceLeft", self.getDistance()[0])
SmartDashboard.putNumber("DT_DistanceRight", self.getDistance()[1])
SmartDashboard.putNumber("DT_Angle", self.getAngle())
#SmartDashboard.putNumber("DT_PowerLeft", self.left.get())
#SmartDashboard.putNumber("DT_PowerRight", self.right.get())
#SmartDashboard.putNumber("DT_VelocityLeft", self.getVelocity()[0])
#SmartDashboard.putNumber("DT_VelocityRight", self.getVelocity()[1])
#SmartDashboard.putNumber("DT_CounLeft", self.getRaw()[0])
#SmartDashboard.putNumber("DT_CountRight", self.getRaw()[1])
#SmartDashboard.putNumber("angle correction", self.anglePID)
#SmartDashboard.putNumber("DriveAmps",self.getOutputCurrent())
#self.mode = SmartDashboard.getData("Mode", "Tank")
self.k = SmartDashboard.getNumber("K Value", 1)
self.sensitivity = SmartDashboard.getNumber("sensitivity", 1)
self.driveStyle = SmartDashboard.getString("DriveStyle", "Tank")
class Shooter(Subsystem):
# ----------------- INITIALIZATION -----------------------
def __init__(self, robot):
super().__init__("shooter")
self.robot = robot
self.counter = 0 # used for updating the log
self.feed_forward = 3.5 # default volts to give the flywheel to get close to setpoint, optional
# motor controllers
self.sparkmax_flywheel = rev.CANSparkMax(5, rev.MotorType.kBrushless)
self.spark_hood = Talon(5)
self.spark_feed = Talon(7)
# encoders and PID controllers
self.hood_encoder = Encoder(
4,
5) # generic encoder - we'll have to install one on the 775 motor
self.hood_encoder.setDistancePerPulse(1 / 1024)
self.hood_controller = wpilib.controller.PIDController(Kp=0.005,
Ki=0,
Kd=0.0)
self.hood_setpoint = 5
self.hood_controller.setSetpoint(self.hood_setpoint)
self.flywheel_encoder = self.sparkmax_flywheel.getEncoder(
) # built-in to the sparkmax/neo
self.flywheel_controller = self.sparkmax_flywheel.getPIDController(
) # built-in PID controller in the sparkmax
# limit switches, use is TBD
self.limit_low = DigitalInput(6)
self.limit_high = DigitalInput(7)
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
#self.setDefaultCommand(ShooterHoodAxis(self.robot))
def set_flywheel(self, velocity):
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0, self.feed_forward)
self.flywheel_controller.setReference(velocity,
rev.ControlType.kSmartVelocity,
0)
#self.flywheel_controller.setReference(velocity, rev.ControlType.kVelocity, 0)
def stop_flywheel(self):
self.flywheel_controller.setReference(0, rev.ControlType.kVoltage)
def set_feed_motor(self, speed):
self.spark_feed.set(speed)
def stop_feed_motor(self):
self.spark_feed.set(0)
def set_hood_motor(self, power):
power_limit = 0.2
if power > power_limit:
new_power = power_limit
elif power < -power_limit:
new_power = -power_limit
else:
new_power = power
self.spark_hood.set(new_power)
def change_elevation(
self, power
): # open loop approach - note they were wired to be false when contacted
if power > 0 and self.limit_high.get():
self.set_hood_motor(power)
elif power < 0 and self.limit_low.get():
self.set_hood_motor(power)
else:
self.set_hood_motor(0)
def set_hood_setpoint(self, setpoint):
self.hood_controller.setSetpoint(setpoint)
def get_angle(self):
elevation_minimum = 30 # degrees
conversion_factor = 1 # encoder units to degrees
# return elevation_minimum + conversion_factor * self.hood_encoder.getDistance()
return self.hood_encoder.getDistance()
def periodic(self) -> None:
"""Perform necessary periodic updates"""
self.counter += 1
if self.counter % 5 == 0:
# pass
# ten per second updates
SmartDashboard.putNumber('elevation',
self.hood_encoder.getDistance())
SmartDashboard.putNumber('rpm',
self.flywheel_encoder.getVelocity())
watch_axis = False
if watch_axis:
self.hood_scale = 0.2
self.hood_offset = 0.0
power = self.hood_scale * (self.robot.oi.stick.getRawAxis(2) -
0.5) + self.hood_offset
self.robot.shooter.change_elevation(power)
maintain_elevation = True
if maintain_elevation:
self.error = self.get_angle() - self.hood_setpoint
pid_out = self.hood_controller.calculate(self.error)
output = 0.03 + pid_out
SmartDashboard.putNumber('El PID', pid_out)
self.change_elevation(output)
if self.counter % 50 == 0:
SmartDashboard.putBoolean("hood_low", self.limit_low.get())
SmartDashboard.putBoolean("hood_high", self.limit_high.get())
class DriveBase:
left_motor_one = CANTalon
left_motor_two = CANTalon
right_motor_one = CANTalon
right_motor_two = CANTalon
left_encoder = Encoder
right_encoder = Encoder
navx = AHRS
def __init__(self):
super().__init__()
"""
Motor objects init
Reason for recall is because MagicRobot is looking for the CANTalon
Object instance before init
"""
self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor)
self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor)
self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor)
self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor)
self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two,
False, Encoder.EncodingType.k4X)
self.navx = AHRS(SPI.Port.kMXP)
self.left_motor_one.enableBrakeMode(True)
self.left_motor_two.enableBrakeMode(True)
self.right_motor_one.enableBrakeMode(True)
self.right_motor_two.enableBrakeMode(True)
self.base = RobotDrive(self.left_motor_one, self.left_motor_two,
self.right_motor_one, self.right_motor_two)
self.dpp = sensor_map.wheel_diameter * math.pi / 360
self.left_encoder.setDistancePerPulse(self.dpp)
self.right_encoder.setDistancePerPulse(self.dpp)
self.left_encoder.setSamplesToAverage(sensor_map.samples_average)
self.right_encoder.setSamplesToAverage(sensor_map.samples_average)
self.left_encoder.setMinRate(sensor_map.min_enc_rate)
self.right_encoder.setMinRate(sensor_map.min_enc_rate)
self.auto_pid_out = AutoPIDOut()
self.pid_d_controller = PIDController(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F,
self.navx, self.auto_pid_out, 0.005)
self.type_flag = ("DRIVE", "TURN")
self.current_flag = self.type_flag[0]
self.auto_pid_out.drive_base = self
self.auto_pid_out.current_flag = self.current_flag
def drive(self, left_power, right_power):
self.base.tankDrive(left_power, right_power)
def execute(self):
if int(self.base.getNumMotors()) < 3:
self.base.drive(0, 0)
def get_drive_distance(self):
return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance())
def rest_base(self):
self.left_encoder.reset()
self.right_encoder.reset()
def pid_drive(self, speed, distance, to_angle=None):
self.navx.isCalibrating()
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.drive_P,
sensor_map.drive_I,
sensor_map.drive_D,
sensor_map.drive_F)
self.pid_d_controller.setOutputRange(speed - distance, speed + distance)
if to_angle is None:
set_angle = self.navx.getYaw()
else:
set_angle = to_angle
self.pid_d_controller.setSetpoint(float(set_angle))
self.drive(speed + 0.03, speed)
self.pid_d_controller.enable()
self.current_flag = self.type_flag[0]
self.auto_pid_out.current_flag = self.current_flag
def pid_turn(self, angle):
self.pid_d_controller.reset()
self.pid_d_controller.setPID(sensor_map.turn_P,
sensor_map.turn_I,
sensor_map.turn_D,
sensor_map.turn_F)
self.pid_d_controller.setOutputRange(sensor_map.output_range_min,
sensor_map.output_range_max)
self.pid_d_controller.setSetpoint(float(angle))
self.pid_d_controller.enable()
self.current_flag = self.type_flag[1]
self.auto_pid_out.current_flag = self.current_flag
def stop_pid(self):
self.pid_d_controller.disable()
self.pid_d_controller.reset()
class Drive(Component):
left_pwm = 0
right_pwm = 0
# Cheesy Drive Stuff
quickstop_accumulator = 0
old_wheel = 0
sensitivity = .9
# Gyro & encoder stuff
gyro_timer = Timer()
driving_angle = False
driving_distance = False
gyro_goal = 0
gyro_tolerance = 2 # Degrees
encoder_goal = 0
encoder_tolerance = 1 # Inches
auto_speed_limit = 0.5
def __init__(self):
super().__init__()
self.l_motor = SyncGroup(Talon, constants.motor_drive_l)
self.r_motor = SyncGroup(Talon, constants.motor_drive_r)
self.l_encoder = Encoder(*constants.encoder_drive_l)
self.r_encoder = Encoder(*constants.encoder_drive_r)
DISTANCE_PER_REV = 4 * math.pi
TICKS_PER_REV = 128
REDUCTION = 30 / 36
self.l_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV)
self.r_encoder.setDistancePerPulse((DISTANCE_PER_REV * REDUCTION) / TICKS_PER_REV)
self.gyro = Gyro(constants.gyro)
self._gyro_p = 0.12
self._gyro_d = 0.005
self._prev_gyro_error = 0
quickdebug.add_printables(self, [
('gyro angle', self.gyro.getAngle),
('left encoder', self.l_encoder.getDistance),
('right encoder', self.r_encoder.getDistance),
'left_pwm', 'right_pwm', 'encoder_goal'
])
quickdebug.add_tunables(self, ['_gyro_p', '_gyro_d'])
def stop(self):
"""Disables EVERYTHING. Only use in case of critical failure."""
self.l_motor.set(0)
self.r_motor.set(0)
def reset_gyro(self):
pass
# self.gyro.reset_encoder()
def cheesy_drive(self, wheel, throttle, quickturn):
"""
Poofs!
:param wheel: The speed that the robot should turn in the X direction. 1 is right [-1.0..1.0]
:param throttle: The speed that the robot should drive in the Y direction. -1 is forward. [-1.0..1.0]
:param quickturn: If the robot should drive arcade-drive style
"""
neg_inertia = wheel - self.old_wheel
self.old_wheel = wheel
wheel = util.sin_scale(wheel, 0.8, passes=3)
if wheel * neg_inertia > 0:
neg_inertia_scalar = 2.5
else:
if abs(wheel) > .65:
neg_inertia_scalar = 5
else:
neg_inertia_scalar = 3
neg_inertia_accumulator = neg_inertia * neg_inertia_scalar
wheel += neg_inertia_accumulator
if quickturn:
if abs(throttle) < 0.2:
alpha = .1
self.quickstop_accumulator = (1 - alpha) * self.quickstop_accumulator + alpha * util.limit(wheel,
1.0) * 5
over_power = 1
angular_power = wheel * .75
else:
over_power = 0
angular_power = abs(throttle) * wheel * self.sensitivity - self.quickstop_accumulator
self.quickstop_accumulator = util.wrap_accumulator(self.quickstop_accumulator)
right_pwm = left_pwm = throttle
left_pwm += angular_power
right_pwm -= angular_power
if left_pwm > 1:
right_pwm -= over_power * (left_pwm - 1)
left_pwm = 1
elif right_pwm > 1:
left_pwm -= over_power * (right_pwm - 1)
right_pwm = 1
elif left_pwm < -1:
right_pwm += over_power * (-1 - left_pwm)
left_pwm = -1
elif right_pwm < -1:
left_pwm += over_power * (-1 - right_pwm)
right_pwm = -1
self.left_pwm = left_pwm
self.right_pwm = right_pwm
def tank_drive(self, left, right):
# Applies a bit of exponential scaling to improve control at low speeds
self.left_pwm = math.copysign(math.pow(left, 2), left)
self.right_pwm = math.copysign(math.pow(right, 2), right)
# Stuff for encoder driving
def set_distance_goal(self, goal):
self.l_encoder.reset()
self.r_encoder.reset()
self.gyro_goal = self.gyro.getAngle()
self.encoder_goal = goal
self.driving_distance = True
self.driving_angle = False
def drive_distance(self):
l_error = self.encoder_goal - self.l_encoder.getDistance()
r_error = self.encoder_goal - self.r_encoder.getDistance()
#
# l_speed = l_error + util.limit(self.gyro_error * self._gyro_p * 0.5, 0.3)
# r_speed = r_error - util.limit(self.gyro_error * self._gyro_p * 0.5, 0.3)
self.left_pwm = util.limit(l_error, self.auto_speed_limit)
self.right_pwm = util.limit(r_error, self.auto_speed_limit)
def at_distance_goal(self):
l_error = self.encoder_goal - self.l_encoder.getDistance()
r_error = self.encoder_goal - self.r_encoder.getDistance()
return abs(l_error) < self.encoder_tolerance and abs(r_error) < self.encoder_tolerance
# Stuff for Gyro driving
def set_angle_goal(self, goal):
self.gyro_timer.stop()
self.gyro_timer.reset()
self.gyro_goal = goal
self.driving_angle = True
self.driving_distance = False
def turn_angle(self):
error = self.gyro_error
result = error * self._gyro_p + ((error - self._prev_gyro_error) / 0.025) * self._gyro_d
self.left_pwm = result
self.right_pwm = -result
self._prev_gyro_error = error
def at_angle_goal(self):
on = abs(self.gyro_error) < self.gyro_tolerance
if on:
if not self.gyro_timer.running:
self.gyro_timer.start()
if self.gyro_timer.hasPeriodPassed(.3):
return True
return False
@property
def gyro_error(self):
"""
Returns gyro error wrapped from -180 to 180
:return:
"""
raw_error = self.gyro_goal + self.gyro.getAngle() # gyro upside down
wrapped_error = raw_error - 360 * round(raw_error / 360)
return wrapped_error
def auto_drive(self):
if self.driving_distance:
if self.at_distance_goal():
self.driving_distance = False
self.drive_distance()
elif self.driving_angle:
if self.at_angle_goal():
self.driving_angle = False
self.turn_angle()
def update(self):
self.l_motor.set(self.left_pwm)
self.r_motor.set(-self.right_pwm)
class Elevator(Component):
ON_TARGET_DELTA = 1 / 4
def __init__(self):
super().__init__()
self._motor = SyncGroup(Talon, constants.motor_elevator)
self._position_encoder = Encoder(*constants.encoder_elevator)
self._photosensor = DigitalInput(constants.photosensor)
self._stabilizer_piston = Solenoid(constants.solenoid_dropper)
self._position_encoder.setDistancePerPulse(
(PITCH_DIAMETER * math.pi) / TICKS_PER_REVOLUTION)
# Trajectory controlling stuff
self._follower = TrajectoryFollower()
self.assure_tote = CircularBuffer(5)
self.auto_stacking = True # Do the dew
self._tote_count = 0 # Keep track of totes!
self._has_bin = False # Do we have a bin on top?
self._new_stack = True # starting a new stack?
self._tote_first = False # Override bin first to grab totes before anything else
self._should_drop = False # Are we currently trying to get a bin ?
self._close_stabilizer = True # Opens the stabilizer manually
self.force_stack = False # manually actuates the elevator down and up
self._follower.set_goal(Setpoints.BIN) # Base state
self._follower_thread = Thread(target=self.update_follower)
self._follower_thread.start()
self._auton = False
quickdebug.add_tunables(Setpoints,
["DROP", "STACK", "BIN", "TOTE", "FIRST_TOTE"])
quickdebug.add_printables(
self,
[('position', self._position_encoder.getDistance),
('photosensor', self._photosensor.get), "has_bin", "tote_count",
"tote_first", "at_goal", "has_game_piece", "auto_stacking"])
def stop(self):
self._motor.set(0)
def update(self):
goal = self._follower.get_goal()
if self.at_goal:
self.assure_tote.append(self.has_game_piece)
if self._should_drop: # Overrides everything else
if self.tote_count < 5 and not self.has_game_piece:
self._follower._max_acc = 100 # Slow down on drop
self._follower.set_goal(Setpoints.DROP)
self._close_stabilizer = False
self._new_stack = True
else:
self._follower._max_acc = 210 # Normal speed
if self._new_stack:
self._new_stack = False
self._close_stabilizer = True
self._tote_count = 0
self._has_bin = False
if goal == Setpoints.STACK: # If we've just gone down to grab something
if self.tote_count == 0 and not self.has_bin and not self.tote_first:
self._has_bin = True # Count the bin
else: # We were waiting for a tote
self.add_tote()
self._follower.set_goal(Setpoints.AUTON if self._auton else
Setpoints.TOTE) # Go back up
if self._tote_count >= 2:
self._close_stabilizer = True
elif (
self.tote_in_long_enough() and self.auto_stacking
) and self.tote_count < 5: # If we try to stack a 6th tote it'll break the robot
if not self.has_bin:
# Without a bin, only stack in these situations:
# - We're picking up a tote, and this is the first tote.
# - We're forcing the elevator to stack (for the bin, usually)
# - We have more than one tote already, so we can pick up more.
if self.tote_first or self.force_stack or self.tote_count > 0:
self._follower.set_goal(Setpoints.STACK)
else: # We have a bin, just auto-stack.
self._follower.set_goal(Setpoints.STACK)
if self.has_bin: # Transfer!
if self._tote_count == 1:
self._close_stabilizer = False
else: # Wait for a game piece & raise the elevator
if self._auton:
self._follower.set_goal(Setpoints.AUTON)
elif self._tote_count == 0 and not self.has_bin:
if self.tote_first:
self._follower.set_goal(Setpoints.FIRST_TOTE)
else:
self._follower.set_goal(Setpoints.BIN)
else:
self._follower.set_goal(Setpoints.TOTE)
self._motor.set(self._follower.output)
self._stabilizer_piston.set(self._close_stabilizer)
self._should_drop = False
self._tote_first = False
self._auton = False
def reset_encoder(self):
self._position_encoder.reset()
self._follower.set_goal(0)
self._follower._reset = True
@property
def has_game_piece(self):
return not self._photosensor.get() or self.force_stack
@property
def position(self):
return self._position_encoder.getDistance()
@property
def at_goal(self):
return self._follower.trajectory_finished(
) # and abs(self._follower.get_goal() - self.position) < 2
def drop_stack(self):
self._should_drop = True
def stack_tote_first(self):
self._tote_first = True
def full(self):
return self._tote_count == 5 and self.has_game_piece
@property
def has_bin(self):
return self._has_bin
@property
def tote_first(self):
return self._tote_first
@property
def tote_count(self):
return self._tote_count
def add_tote(self, number=1):
self._tote_count = max(0, min(5, self._tote_count + number))
def remove_tote(self):
self.add_tote(-1)
def set_bin(self, bin_):
self._has_bin = bin_
def auton(self):
self._auton = True
self._tote_first = True
def tote_in_long_enough(self):
return all(self.assure_tote)
def update_follower(self):
while True:
self._follower.calculate(self.position)
time.sleep(0.005)
