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 __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)
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)
def __init__(self, l_a=0, l_b=1, r_a=2, r_b=3):
self.left = Encoder(l_a, l_b, reverseDirection=True)
self.right = Encoder(r_a, r_b)
self.left.setDistancePerPulse(self.dpp)
self.right.setDistancePerPulse(self.dpp)
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 __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 __init__(self, robot):
Subsystem.__init__(self, 'Turret')
self.robot = robot
self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X)
motors = {}
self.map = self.robot.botMap
self.tmotors = motors
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.tmotors:
self.tmotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast)
self.kP = 0.05
self.kI = 0.000
self.kD = 0.002
self.turretPID = PID(self.kP, self.kI, self.kD)
self.turretPID.limitVal(0.3)
self.setPow = 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.k4x):
"""
Initializes the encoder with two channels,
distance per pulse (default 1), no reversing,
on module number 1, 128 CPR, and with 4x counting.
"""
super().__init__()
self.e = WEncoder(modnum, channel_a, channel_b, reverse, enctype)
self.cpr = cpr
self.pulse_dist = pulse_dist
def poll(self):
dist = self.e.GetDistance()
self.distance = dist
self.rate = self.e.GetRate()
self.stopped = self.e.GetStopped()
self.direction = self.e.GetDirection()
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 __init__(self, robot):
super().__init__("Arm")
self.robot = robot
self.peakCurrentLimit = 30
self.PeaKDuration = 50
self.continuousLimit = 15
motor = {}
for name in self.robot.RobotMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(self.robot.RobotMap.motorMap.motors[name])
self.motors = motor
for name in self.motors:
self.motors[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.motors[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.motors[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.motors[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.motors[name].enableCurrentLimit(True)
self.AEnc = Encoder(4, 5, False, Encoder.EncodingType.k4X)
self.Zero = DigitalInput(6)
self.kp = 0.00035
self.ki = 0.00000000001
self.kd = 0.0000001
self.ArmPID = PID(self.kp, self.ki, self.kd)
class Encoders:
"""
Encoder group class for drivetrain
- We use 2 E4T encoders
"""
dpp = 6 * math.pi / 1440 # 6 in. wheels, 1440 ppr on e4ts
def __init__(self, l_a=0, l_b=1, r_a=2, r_b=3):
self.left = Encoder(l_a, l_b, reverseDirection=True)
self.right = Encoder(r_a, r_b)
self.left.setDistancePerPulse(self.dpp)
self.right.setDistancePerPulse(self.dpp)
def getDist(self):
return (self.left.GetDistance + self.right.getDistance) / 2
def get_R_Dist(self):
return self.right.GetDistance
def get_L_Dist(self):
return self.left.gGetDistance()
def reset(self):
self.right.reset()
self.left.reset()
def __init__(self, robot):
super().__init__("Drive")
self.robot = robot
self.peakCurrentLimit = 25
self.PeaKDuration = 50
self.continuousLimit = 15
self.map = robot.RobotMap
# drive motor, sensors, and pistons
motor = {}
pistons = {}
# create all drive motors, Sensors, and pistons
for name in self.map.motorMap.motors:
motor[name] = robot.Creator.createMotor(self.map.motorMap.motors[name])
for name in self.map.PneumaticMap.pistons:
if name == 'Shifter':
pistons[name] = robot.Creator.createPistons(self.map.PneumaticMap.pistons[name])
self.REnc = Encoder(0, 1, True, Encoder.EncodingType.k4X)
self.LEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X)
self.Gyro = ADXRS450_Gyro()
# make motors, Sensors, and pistons local to subsystem
self.Dmotor = motor
self.Dpiston = pistons
for name in self.Dmotor:
self.Dmotor[name].setInverted(self.robot.RobotMap.motorMap.motors[name]['inverted'])
# drive current limit
self.Dmotor[name].configPeakCurrentLimit(self.peakCurrentLimit, 10)
self.Dmotor[name].configPeakCurrentDuration(self.PeaKDuration, 10)
self.Dmotor[name].configContinuousCurrentLimit(self.continuousLimit, 10)
self.Dmotor[name].enableCurrentLimit(True)
self.KtorqueMode = DoubleSolenoid.Value.kReverse
self.KspeedMode = DoubleSolenoid.Value.kForward
if wpilib.RobotBase.isSimulation():
self.kp = 0.001
self.ki = 0.00001
self.kd = 0.000000001
else:
self.kp = 0.001
self.ki = 0.00001
self.kd = 0.000000001
self.DrivePID = PID(self.kp, self.ki, self.kd)
class Indexer():
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def update(self):
self.realValue = self.encoder.get()
offset = (self.targetValue - self.realValue) % self.totalValues - (self.totalValues / 2)
self.speed = clamp(abs(offset) * self.speedmult, self.minspeed, self.maxspeed)
self.dashboard.putString("Indexer", "{} offset".format(offset))
if (offset > self.allowedError):
self.motor.set(ControlMode.PercentOutput, -self.speed)
elif (offset < -self.allowedError):
self.motor.set(ControlMode.PercentOutput, self.speed)
else:
self.motor.set(ControlMode.PercentOutput, 0)
if (abs(offset) < 15):
if (self.operator.getIndexUp()):
self.hole = (self.hole + 1) % 5
elif (self.operator.getIndexDown()):
self.hole = (self.hole + 4) % 5
self.setRotation(self.hole * 72 + self.holeOffset)
def getRotation(self) -> float:
return self.realValue / self.totalValues * 360
def setRotation(self, degrees):
self.targetValue = clamp(degrees/360*self.totalValues,
0, self.totalValues)
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 __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
class Chassis(Subsystem):
def __init__(self):
super().__init__("Chassis")
self.spark_L1 = Spark(robotmap.spark_L1)
self.spark_L2 = Spark(robotmap.spark_L2)
self.spark_R1 = Spark(robotmap.spark_R1)
self.spark_R2 = Spark(robotmap.spark_R2)
self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2)
self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2)
self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R)
self.gyro = ADXRS450_Gyro(robotmap.gyro)
self.encoder_L = Encoder(0, 1)
self.encoder_R = Encoder(2, 3)
self.dist_pulse_L = pi * 6 / 2048
self.dist_pulse_R = pi * 6 / 425
@classmethod
def setDriveSpd(cls, spd_drive_new):
robotmap.spd_chassis_drive = spd_drive_new
@classmethod
def setRotateSpd(cls, spd_rotate_new):
robotmap.spd_chassis_rotate = spd_rotate_new
def stop(self):
self.drive.stopMotor()
def curvatureDrive(self, spd_x, spd_z):
self.drive.curvatureDrive(spd_x, spd_z, True)
def joystickDrive(self):
self.drive.curvatureDrive(
-(oi.joystick.getRawAxis(1)) * robotmap.spd_chassis_drive,
oi.joystick.getRawAxis(4) * robotmap.spd_chassis_rotate, True)
def setupEncoder(self):
self.encoder_L.setDistancePerPulse(self.dist_pulse_L)
self.encoder_R.setDistancePerPulse(self.dist_pulse_R)
self.encoder_L.reset()
self.encoder_R.reset()
def getGyroAngle(self):
return self.gyro.getAngle()
def resetGyro(self):
self.gyro.reset()
def gyroDrive(self, spd_temp, amt_turn=None):
if spd_temp and amt_turn is not None:
self.curvatureDrive(spd_temp, amt_turn)
elif amt_turn is None:
self.curvatureDrive(spd_temp, 0.0)
else:
raise ("GyroDrive() failed!")
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 __init__(self):
super().__init__("Chassis")
self.spark_L1 = Spark(robotmap.spark_L1)
self.spark_L2 = Spark(robotmap.spark_L2)
self.spark_R1 = Spark(robotmap.spark_R1)
self.spark_R2 = Spark(robotmap.spark_R2)
self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2)
self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2)
self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R)
self.gyro = ADXRS450_Gyro(robotmap.gyro)
self.encoder_L = Encoder(0, 1)
self.encoder_R = Encoder(2, 3)
self.dist_pulse_L = pi * 6 / 2048
self.dist_pulse_R = pi * 6 / 425
def __init__(self, operator: OperatorControl):
self.operator = operator
self.motor = WPI_VictorSPX(7)
self.encoder = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.encoder.setIndexSource(2)
self.limit = DigitalInput(4)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.totalValues = 2048 # * self.encoder.getEncodingScale() - 1
self.targetValue = 10
self.realValue = 0
self.allowedError = 10
self.hole = 4
self.holeOffset = 36 - 15
self.maxspeed = .25
self.minspeed = .1
self.speedmult = 1/300
self.speed = .1
def __init__(self, robot):
Subsystem.__init__(self, 'Flywheel')
self.CurPos = 0
self.PasPos = 0
self.robot = robot
self.Fenc = Encoder(6, 7, False, Encoder.EncodingType.k4X)
self.map = self.robot.botMap
motor = {}
piston = {}
for name in self.robot.botMap.motorMap.motors:
motor[name] = self.robot.Creator.createMotor(
self.robot.botMap.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
piston[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.fmotor = motor
self.fpiston = piston
for name in self.fmotor:
self.fmotor[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.fmotor[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.fmotor[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Fly']), 40)
self.kP = 0.008
self.kI = 0.00002
self.kD = 0.00018
self.kF = 0.0 # tune me when testing
self.flywheelPID = PID(self.kP, self.kI, self.kD, self.kF)
self.flywheelPID.MaxIn(680)
self.flywheelPID.MaxOut(1)
self.flywheelPID.limitVal(0.95) # Limit PID output power to 50%
self.feetToRPS = 51.111
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 __init__(self, channel_a, channel_b, pulse_dist=1.0,
reverse=False, modnum=1, cpr=128,
enctype=CounterBase.k4x):
"""
Initializes the encoder with two channels,
distance per pulse (default 1), no reversing,
on module number 1, 128 CPR, and with 4x counting.
"""
super().__init__()
self.e = WEncoder(modnum, channel_a, channel_b, reverse, enctype)
self.cpr = cpr
self.pulse_dist = pulse_dist
class Robot(TimedRobot):
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightSensor1 = SensorCollection(self.RightMotor1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
# self.drive = DifferentialDrive(self.leftDriveMotors,
# self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3, Encoder.EncodingType.k4X)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def disabledInit(self):
pass
def autonomousInit(self):
pass
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
self.dashboard.putNumber("Encoder", self.RightSensor1.getQuadratureVelocity()/2048*60)
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
speed = self.testEncoder.get() / 1028
self.leftDriveMotors.set(speed)
self.rightDriveMotors.set(speed)
def teleopPeriodic(self):
pass
def testPeriodic(self):
pass
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()
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
self.drive = DifferentialDrive(self.leftDriveMotors,
self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def __init__(self, robot):
Subsystem.__init__(self, 'Drive')
self.robot = robot
motors = {}
pistons = {}
self.map = self.robot.botMap
self.rEnc = Encoder(0, 1, False, Encoder.EncodingType.k4X)
self.lEnc = Encoder(2, 3, False, Encoder.EncodingType.k4X)
self.Gyro = ADXRS450_Gyro()
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.robot.botMap.PneumaticMap.pistons:
if name == 'Shifter':
pistons[name] = self.robot.Creator.createPistons(
self.robot.botMap.PneumaticMap.pistons[name])
self.dMotors = motors
self.dPistons = pistons
for name in self.dMotors:
self.dMotors[name].setInverted(
self.robot.botMap.motorMap.motors[name]['inverted'])
self.dMotors[name].setNeutralMode(ctre.NeutralMode.Coast)
if self.map.motorMap.motors[name]['CurLimit'] is True:
self.dMotors[name].configStatorCurrentLimit(
self.robot.Creator.createCurrentConfig(
self.robot.botMap.currentConfig['Drive']), 40)
self.kP = 0.0
self.kI = 0.0
self.kD = 0.0
self.DrivePID = PID(self.kP, self.kI, self.kD)
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 Robot(TimedRobot):
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
self.drive = DifferentialDrive(self.leftDriveMotors,
self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def disabledInit(self):
pass
def autonomousInit(self):
pass
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
self.dashboard.putNumber("Encoder", self.testEncoder.getRate())
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
self.drive.arcadeDrive(.5, 0)
def teleopPeriodic(self):
pass
def testPeriodic(self):
pass
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"])
class Turret(Subsystem):
def __init__(self, robot):
Subsystem.__init__(self, 'Turret')
self.robot = robot
self.tEncoder = Encoder(4, 5, False, Encoder.EncodingType.k4X)
motors = {}
self.map = self.robot.botMap
self.tmotors = motors
for name in self.map.motorMap.motors:
motors[name] = robot.Creator.createMotor(
self.map.motorMap.motors[name])
for name in self.tmotors:
self.tmotors[name].setInverted(
self.map.motorMap.motors[name]['inverted'])
self.tmotors[name].setNeutralMode(ctre.NeutralMode.Coast)
self.kP = 0.05
self.kI = 0.000
self.kD = 0.002
self.turretPID = PID(self.kP, self.kI, self.kD)
self.turretPID.limitVal(0.3)
self.setPow = 0
def set(self, pw):
self.tmotors['turret'].set(ctre.ControlMode.PercentOutput, pw)
def setPower(self, pow):
self.setPow = pow
def getEnc(self):
return self.tEncoder.get()
def periodic(self):
if self.robot.Limelight.isExisting():
self.set(0) # self.turretPID.outVal(self.robot.Limelight.getX()))
else:
self.set(self.setPow)
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 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()
