class Robot(wpilib.TimedRobot):
def robotInit(self):
self.tankDrive = DifferentialDrive(
wpilib.PWMVictorSPX(0), wpilib.PWMVictorSPX(1)
)
self.leftEncoder = wpilib.Encoder(0, 1)
self.rightEncoder = wpilib.Encoder(2, 3)
self.elevatorMotor = wpilib.PWMVictorSPX(2)
self.elevatorPot = wpilib.AnalogPotentiometer(0)
# Add a 'max speed' widget to a tab named 'Configuration', using a number slider
# The widget will be placed in the second column and row and will be two columns wide
self.maxSpeed = (
Shuffleboard.getTab("Configuration")
.add(title="Max Speed", value=1)
.withWidget("Number Slider")
.withPosition(1, 1)
.withSize(2, 1)
.getEntry()
)
# Add the tank drive and encoders to a 'Drivebase' tab
driveBaseTab = Shuffleboard.getTab("Drivebase")
driveBaseTab.add(title="Tank Drive", value=self.tankDrive)
# Put both encoders in a list layout
encoders = (
driveBaseTab.getLayout(type="List Layout", title="Encoders")
.withPosition(0, 0)
.withSize(2, 2)
)
encoders.add(title="Left Encoder", value=self.leftEncoder)
encoders.add(title="Right Encoder", value=self.rightEncoder)
# Add the elevator motor and potentiometer to an 'Elevator' tab
elevatorTab = Shuffleboard.getTab("Elevator")
elevatorTab.add(title="Motor", value=self.elevatorMotor)
elevatorTab.add(title="Potentiometer", value=self.elevatorPot)
def autonomousInit(self):
# Read the value of the 'max speed' widget from the dashboard
self.tankDrive.setMaxOutput(self.maxSpeed.getDouble(1.0))
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 DriveTrain(Subsystem):
"""
The DriveTrain subsystem controls the robot's chassis and reads in
information about its speed and position.
"""
def __init__(self, robot):
super().__init__("drivetrain")
#Subsystem.__init__("drivetrain")
self.robot = robot
self.error_dict = {}
# Add constants and helper variables
self.twist_power_maximum = 0.5
self.strafe_power_maximum = 0.5
self.thrust_power_maximum = 0.5
self.mecanum_power_limit = 1.0
self.max_velocity = 500 # rpm for mecanum velocity
self.current_thrust = 0
self.current_twist = 0
self.current_strafe = 0
self.acceleration_limit = 0.05
self.counter = 0
# due to limitations in displaying digits in the Shuffleboard, we'll multiply these by 1000 and divide when updating the controllers
self.PID_multiplier = 1000.
self.PID_dict_pos = {
'kP': 0.010,
'kI': 5.0e-7,
'kD': 0.40,
'kIz': 0,
'kFF': 0.002,
'kMaxOutput': 0.99,
'kMinOutput': -0.99
}
self.PID_dict_vel = {
'kP': 2 * 0.00015,
'kI': 1.5 * 8.0e-7,
'kD': 0.00,
'kIz': 0,
'kFF': 0.00022,
'kMaxOutput': 0.99,
'kMinOutput': -0.99
}
self.encoder_offsets = [
0, 0, 0, 0
] # added because the encoders do not reset fast enough for autonomous
# Smart Motion Coefficients - these don't seem to be writing for some reason... python is old? just set with rev's program for now
self.smartmotion_maxvel = 1000 # rpm
self.smartmotion_maxacc = 500
self.current_limit = 100
self.ramp_rate = 0.0
# tracking the robot across the field... easier with WCD
self.x = 0
self.y = 0
self.previous_distance = 0
self.is_limited = False
self.deadband = 0.05
# Configure drive motors
#if True: # or could be if self.robot.isReal():
if self.robot.isReal():
motor_type = rev.MotorType.kBrushless
self.spark_neo_right_front = rev.CANSparkMax(1, motor_type)
self.spark_neo_right_rear = rev.CANSparkMax(2, motor_type)
self.spark_neo_left_front = rev.CANSparkMax(3, motor_type)
self.spark_neo_left_rear = rev.CANSparkMax(4, motor_type)
self.controllers = [
self.spark_neo_left_front, self.spark_neo_left_rear,
self.spark_neo_right_front, self.spark_neo_right_rear
]
self.spark_PID_controller_right_front = self.spark_neo_right_front.getPIDController(
)
self.spark_PID_controller_right_rear = self.spark_neo_right_rear.getPIDController(
)
self.spark_PID_controller_left_front = self.spark_neo_left_front.getPIDController(
)
self.spark_PID_controller_left_rear = self.spark_neo_left_rear.getPIDController(
)
self.pid_controllers = [
self.spark_PID_controller_left_front,
self.spark_PID_controller_left_rear,
self.spark_PID_controller_right_front,
self.spark_PID_controller_right_rear
]
# wpilib.Timer.delay(0.02)
# swap encoders to get sign right
# changing them up for mechanum vs WCD
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_front)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_rear)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_front)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_rear)
self.encoders = [
self.sparkneo_encoder_1, self.sparkneo_encoder_2,
self.sparkneo_encoder_3, self.sparkneo_encoder_4
]
# Configure encoders and controllers
# should be wheel_diameter * pi / gear_ratio - and for the old double reduction gear box
# the gear ratio was 4.17:1. With the shifter (low gear) I think it was a 12.26.
# then new 2020 gearbox is 9.52
gear_ratio = 9.52
#gear_ratio = 12.75
conversion_factor = 8.0 * 3.141 / gear_ratio
for ix, encoder in enumerate(self.encoders):
self.error_dict.update({
'conv_' + str(ix):
encoder.setPositionConversionFactor(conversion_factor)
})
# wpilib.Timer.delay(0.02)
# TODO - figure out if I want to invert the motors or the encoders
inverted = False # needs this to be True for the toughbox
self.spark_neo_left_front.setInverted(inverted)
self.spark_neo_left_rear.setInverted(inverted)
self.spark_neo_right_front.setInverted(inverted)
self.spark_neo_right_rear.setInverted(inverted)
self.configure_controllers()
#self.display_PIDs()
else: # for simulation only, but the CANSpark is getting closer to behaving in sim
# get a pretend drivetrain for the simulator
self.spark_neo_left_front = wpilib.Talon(1)
self.spark_neo_left_rear = wpilib.Talon(2)
self.spark_neo_right_front = wpilib.Talon(3)
self.spark_neo_right_rear = wpilib.Talon(4)
# Not sure if speedcontrollergroups work with the single sparkmax in python - seems to complain
# drive_type = 'mechanum' # comp bot
drive_type = 'wcd' # practice bot, sim as of 1/16/2021
if drive_type == 'wcd':
# WCD
print("Enabling WCD drive!")
if robot.isReal():
err_1 = self.spark_neo_left_rear.follow(
self.spark_neo_left_front)
err_2 = self.spark_neo_right_rear.follow(
self.spark_neo_right_front)
if err_1 != rev.CANError.kOk or err_2 != rev.CANError.kOk:
print(
f"Warning: drivetrain follower issue with neo2 returning {err_1} and neo4 returning {err_2}"
)
self.speedgroup_left = SpeedControllerGroup(
self.spark_neo_left_front)
self.speedgroup_right = SpeedControllerGroup(
self.spark_neo_right_front)
self.differential_drive = DifferentialDrive(
self.speedgroup_left, self.speedgroup_right)
self.drive = self.differential_drive
self.differential_drive.setMaxOutput(1.0)
if drive_type == 'mechanum':
# Mechanum
# TODO: Reset followers in software
print("Enabling mechanum drive!")
self.speedgroup_lfront = SpeedControllerGroup(
self.spark_neo_left_front)
self.speedgroup_lrear = SpeedControllerGroup(
self.spark_neo_left_rear)
self.speedgroup_rfront = SpeedControllerGroup(
self.spark_neo_right_front)
self.speedgroup_rrear = SpeedControllerGroup(
self.spark_neo_right_rear)
self.mechanum_drive = MecanumDrive(self.speedgroup_lfront,
self.speedgroup_lrear,
self.speedgroup_rfront,
self.speedgroup_rrear)
# self.mechanum_drive = MecanumDrive(self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear)
self.mechanum_drive.setMaxOutput(self.mecanum_power_limit)
self.drive = self.mechanum_drive
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(0.1)
# self.differential_drive.setSensitivity(0.5)
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
self.setDefaultCommand(DriveByJoystick(self.robot))
def spark_with_stick(self, thrust=0, strafe=0, z_rotation=0, gyroAngle=0):
"""Simplest way to drive with a joystick"""
# self.differential_drive.arcadeDrive(x_speed, self.twist_sensitivity * z_rotation, False)
if self.robot.isReal():
self.drive.driveCartesian(
xSpeed=thrust * self.thrust_power_maximum,
ySpeed=strafe * self.strafe_power_maximum,
zRotation=self.twist_power_maximum * z_rotation)
else:
self.arcade_drive(thrust * self.thrust_power_maximum,
self.twist_power_maximum * z_rotation)
def mecanum_velocity_cartesian(self,
thrust: float,
strafe: float,
z_rotation: float,
gyroAngle: float = 0.0) -> None:
""" CJH Trying to implement a velocity control loop on the mecanum vs. % output"""
# Compensate for gyro angle
input = Vector2d(strafe, thrust)
input.rotate(gyroAngle)
wheel_speeds = [
# Front Left
input.x + input.y + z_rotation,
# Rear Left
-input.x + input.y + z_rotation,
# Front Right
-input.x + input.y - z_rotation,
# Rear Right
input.x + input.y - z_rotation,
]
#RobotDriveBase.normalize(wheelSpeeds)
maxMagnitude = max(abs(x) for x in wheel_speeds)
if maxMagnitude > 1.0:
for i in range(len(wheel_speeds)):
wheel_speeds[i] = wheel_speeds[i] / maxMagnitude
#wheel_speeds = [speed * self.maxOutput for speed in wheelSpeeds]
# put all this in the zip below
#self.spark_neo_left_front.set(wheel_speeds[0])
#self.spark_neo_left_rear.set(wheel_speeds[1])
#self.spark_neo_right_front.set(wheel_speeds[2] * self.rightSideInvertMultiplier)
#self.spark_neo_right_rear.set(wheel_speeds[3] * self.rightSideInvertMultiplier)
multipliers = [1.0, 1.0, -1.0, -1.0]
debug_speed = []
if math.sqrt(input.x**2 + input.y**2 + z_rotation**2) < self.deadband:
self.mechanum_drive.driveCartesian(0, 0, 0)
else:
for speed, multiplier, controller in zip(wheel_speeds, multipliers,
self.pid_controllers):
controller.setReference(multiplier * speed * self.max_velocity,
rev.ControlType.kVelocity, 1)
debug_speed.append(multiplier * speed * self.max_velocity)
if self.counter % 20 == 0:
pass
# print(f"Wheel speeds set to {wheel_speeds} from thrust {thrust:2.4f} strafe {strafe:2.4f} rot {z_rotation:2.4f}")
self.drive.feed()
def stop(self):
self.differential_drive.arcadeDrive(0, 0)
def smooth_drive(self, thrust, strafe, twist):
"""A less jittery way to drive with a joystick
TODO: See if this can be implemented in hardware - seems like the acceleration limit can be set there
TODO: Should be ok to slow down quickly, but not speed up - check this code
TODO: Set a smartdash to see if we are in software limit mode - like with a boolean
"""
deadzone = 0.05
self.is_limited = False
# thrust section
if math.fabs(thrust) < deadzone:
self.current_thrust = 0
else:
if math.fabs(thrust -
self.current_thrust) < self.acceleration_limit:
self.current_thrust = thrust
else:
if thrust - self.current_thrust > 0:
# accelerating forward
self.current_thrust = self.current_thrust + self.acceleration_limit
self.is_limited = True
elif (thrust - self.current_thrust) < 0 and thrust > 0:
# ok to slow down quickly, although really the deadzone should catch this
self.current_thrust = thrust
elif (thrust - self.current_thrust) > 0 and thrust < 0:
# ok to slow down quickly, although really the deadzone should catch this
self.current_thrust = thrust
else:
# accelerating backward
self.current_thrust = self.current_thrust - self.acceleration_limit
self.is_limited = True
# strafe section
if math.fabs(strafe) < deadzone:
self.current_strafe = 0
else:
if math.fabs(strafe -
self.current_strafe) < self.acceleration_limit:
self.current_strafe = strafe
else:
if strafe - self.current_strafe > 0:
self.current_strafe = self.current_strafe + self.acceleration_limit
else:
self.current_strafe = self.current_strafe - self.acceleration_limit
# twist section
if math.fabs(twist) < deadzone:
self.current_twist = 0
else:
if math.fabs(twist - self.current_twist) < self.acceleration_limit:
self.current_twist = twist
else:
if twist - self.current_twist > 0:
self.current_twist = self.current_twist + self.acceleration_limit
else:
self.current_twist = self.current_twist - self.acceleration_limit
# self.differential_drive.arcadeDrive(self.current_thrust, self.current_twist, True)
# TODO - fix this for mechanum x and y
self.mechanum_drive.driveCartesian(
xSpeed=self.thrust_power_maximum * self.current_thrust,
ySpeed=self.strafe_power_maximum * self.current_strafe,
zRotation=self.twist_power_maximum * self.current_twist)
def tank_drive(self, left, right):
"""This is thrown in for simulation"""
self.differential_drive.tankDrive(left, right)
def arcade_drive(self, speed, rotation):
"""This is thrown in for simulation or WCD"""
self.differential_drive.arcadeDrive(speed, rotation, False)
def get_position(self):
""":returns: The encoder position of one of the Neos"""
return self.sparkneo_encoder_1.getPosition()
def get_positions(self):
""":returns: The encoder position of both of the Neos"""
return self.sparkneo_encoder_1.getPosition(
), self.sparkneo_encoder_3.getPosition()
def set_velocity(self, velocity):
multipliers = [1.0, 1.0, -1.0, -1.0]
for multiplier, controller in zip(multipliers, self.pid_controllers):
controller.setReference(multiplier * velocity,
rev.ControlType.kVelocity, 1)
def goToSetPoint(self, set_point, reset=True):
self.reset()
multipliers = [1.0, 1.0, -1.0, -1.0]
for multiplier, controller in zip(multipliers, self.pid_controllers):
# controller.setReference(multiplier * set_point, rev.ControlType.kPosition)
controller.setReference(multiplier * set_point,
rev.ControlType.kSmartMotion,
pidSlot=1)
def reset(self, hard=True):
# There is an issue with the lag in encoder resets for autonomous.
# TODO: Need to see if it is a CAN lag issue
if hard:
self.encoder_offsets = [0, 0, 0, 0]
if self.robot.isReal():
for ix, encoder in enumerate(self.encoders):
can_error = encoder.setPosition(0)
self.error_dict.update({'ResetPos_' + str(ix): can_error})
if can_error != rev.CANError.kOk:
print(
f"Warning: drivetrain reset issue with {encoder} returning {can_error}"
)
else:
for ix, encoder in enumerate(self.encoders):
self.encoder_offsets[ix] = encoder.getPosition()
self.x = 0
self.y = 0
# wpilib.Timer.delay(0.02)
def configure_controllers(self, pid_only=False):
"""Set the PIDs, etc for the controllers, slot 0 is position and slot 1 is velocity"""
if not pid_only:
for i, controller in enumerate(self.controllers):
# error_dict.append(controller.restoreFactoryDefaults())
self.error_dict.update({
'OpenRamp_' + str(i):
controller.setOpenLoopRampRate(self.ramp_rate)
})
self.error_dict.update({
'ClosedRamp_' + str(i):
controller.setClosedLoopRampRate(self.ramp_rate)
})
self.error_dict.update({
'Idle_' + str(i):
controller.setIdleMode(rev.IdleMode.kBrake)
})
self.error_dict.update({
'CurLimit_' + str(i):
controller.setSmartCurrentLimit(self.current_limit)
})
self.error_dict.update({
'VoltComp_' + str(i):
controller.enableVoltageCompensation(12)
})
# defaults are 10, 20, 20 on the frame rates - trying to cut down a bit on CAN bandwidth
#self.error_dict.update({'PeriodicStatus0_'+str(i):controller.setPeriodicFramePeriod(rev.CANSparkMax.PeriodicFrame.kStatus0, 20)})
#self.error_dict.update({'PeriodicStatus1_'+str(i):controller.setPeriodicFramePeriod(rev.CANSparkMax.PeriodicFrame.kStatus1, 40)})
#self.error_dict.update({'PeriodicStatus2_'+str(i):controller.setPeriodicFramePeriod(rev.CANSparkMax.PeriodicFrame.kStatus2, 20)})
for i, controller in enumerate(self.pid_controllers):
self.error_dict.update(
{'kP0_' + str(i): controller.setP(self.PID_dict_pos['kP'], 0)})
self.error_dict.update(
{'kP1_' + str(i): controller.setP(self.PID_dict_vel['kP'], 1)})
self.error_dict.update(
{'kI0_' + str(i): controller.setI(self.PID_dict_pos['kI'], 0)})
self.error_dict.update(
{'kI1_' + str(i): controller.setI(self.PID_dict_vel['kI'], 1)})
self.error_dict.update(
{'kD0_' + str(i): controller.setD(self.PID_dict_pos['kD'], 0)})
self.error_dict.update(
{'kD_1' + str(i): controller.setD(self.PID_dict_vel['kD'], 1)})
self.error_dict.update({
'kFF_0' + str(i):
controller.setFF(self.PID_dict_pos['kFF'], 0)
})
self.error_dict.update({
'kFF_1' + str(i):
controller.setFF(self.PID_dict_vel['kFF'], 1)
})
self.error_dict.update({
'kIZ_0' + str(i):
controller.setIZone(self.PID_dict_pos['kIz'], 0)
})
self.error_dict.update({
'kIZ_1' + str(i):
controller.setIZone(self.PID_dict_vel['kIz'], 1)
})
self.error_dict.update({
'MinMax0_' + str(i):
controller.setOutputRange(self.PID_dict_pos['kMinOutput'],
self.PID_dict_pos['kMaxOutput'], 0)
})
self.error_dict.update({
'MinMax0_' + str(i):
controller.setOutputRange(self.PID_dict_vel['kMinOutput'],
self.PID_dict_vel['kMaxOutput'], 1)
})
self.error_dict.update({
'Accel0_' + str(i):
controller.setSmartMotionMaxVelocity(self.smartmotion_maxvel,
0)
})
self.error_dict.update({
'Accel0_' + str(i):
controller.setSmartMotionMaxVelocity(self.smartmotion_maxvel,
1)
})
self.error_dict.update({
'Vel0_' + str(i):
controller.setSmartMotionMaxAccel(self.smartmotion_maxacc, 0)
})
self.error_dict.update({
'Vel1_' + str(i):
controller.setSmartMotionMaxAccel(self.smartmotion_maxacc, 1)
})
# if 1 in error_dict or 2 in error_dict:
# print(f'Issue in configuring controllers: {error_dict}')
# else:
#print(f'Results of configuring controllers: {self.error_dict}')
if len(set(self.error_dict)) > 1:
print('\n*Sparkmax setting* *Response*')
for key in sorted(self.error_dict.keys()):
print(f' {key:15} \t {self.error_dict[key]}')
else:
print(f'\n *All SparkMax report {list(set(self.error_dict))[0]}')
burn_flash = False
if burn_flash:
for i, controller in enumerate(self.controllers):
can_error = controller.burnFlash()
print(f'Burn flash on controller {i}: {can_error}')
def change_PIDs(self, factor=1, dict_0=None, dict_1=None):
"""Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change
can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set
slot 0 (position) or slot 1 (velocity) """
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
if dict_0 == None:
self.PID_dict_pos[key] *= factor
else:
self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier
SmartDashboard.putString(
"alert",
f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}"
)
if dict_1 == None:
self.PID_dict_vel[key] *= factor
else:
self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier
SmartDashboard.putString(
"alert",
f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}"
)
self.configure_controllers(pid_only=True)
self.display_PIDs()
def display_PIDs(self):
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
SmartDashboard.putNumber(
key + '_0', self.PID_multiplier * self.PID_dict_pos[key])
SmartDashboard.putNumber(
key + '_1', self.PID_multiplier * self.PID_dict_vel[key])
def print_PIDs(self):
print(
f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}"
)
print(
f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}"
)
def square(self, value):
"""Return the signed square value of a number - for drive sensitivity
:return: the squared value of the input retaining the sign
"""
# sq = lambda x: x**2 if (x > 0) else -1.0 * x**2
sign = 1.0 # could do this all with a copysign but better to be explicit
if value < 0:
sign = -1.0
return sign * value**2
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() -
self.sparkneo_encoder_3.getPosition())
except:
print(
f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}"
)
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
#SmartDashboard.putNumber("Robot X", round(self.x, 2))
#SmartDashboard.putNumber("Robot Y", round(self.y, 2))
for ix, encoder in enumerate(self.encoders):
SmartDashboard.putNumber(
f"Position Enc{str(int(1+ix))}",
round(encoder.getPosition() - self.encoder_offsets[ix], 2))
SmartDashboard.putNumber(f"Velocity Enc{str(int(1+ix))}",
round(encoder.getVelocity(), 2))
SmartDashboard.putNumber(
"Current M1",
round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber(
"Current M3",
round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
# self.display_PIDs()
SmartDashboard.putString(
"alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}"
)
class DriveTrain(Subsystem):
"""
The DriveTrain subsystem controls the robot's chassis and reads in
information about its speed and position.
"""
def __init__(self, robot):
super().__init__()
self.robot = robot
# Add constants and helper variables
self.twist_sensitivity = 0.5
self.current_thrust = 0
self.current_twist = 0
self.current_strafe = 0
self.acceleration_limit = 0.05
self.counter = 0
self.mecanum_power_limit = 0.6
# due to limitations in displaying digits in the Shuffleboard, we'll multiply these by 1000 and divide when updating the controllers
self.PID_multiplier = 1000.
self.PID_dict_pos = {'kP': 0.010, 'kI': 5.0e-7, 'kD': 0.40, 'kIz': 0, 'kFF': 0.002, 'kMaxOutput': 0.99,
'kMinOutput': -0.99}
self.PID_dict_vel = {'kP': 0.00015, 'kI': 8.0e-7, 'kD': 0.00, 'kIz': 0, 'kFF': 0.00022, 'kMaxOutput': 0.99,
'kMinOutput': -0.99}
# Smart Motion Coefficients - these don't seem to be writing for some reason... python is old? just set with rev's program for now
self.maxvel = 500 # rpm
self.maxacc = 500
self.current_limit = 40
self.x = 0
self.y = 0
self.previous_distance = 0
self.is_limited = False
self.deadband = 0.05
# Configure drive motors
try:
if robot.isReal():
self.spark_neo_right_front = rev.CANSparkMax(1, rev.MotorType.kBrushless)
self.spark_neo_right_rear = rev.CANSparkMax(2, rev.MotorType.kBrushless)
self.spark_neo_left_front = rev.CANSparkMax(3, rev.MotorType.kBrushless)
self.spark_neo_left_rear = rev.CANSparkMax(4, rev.MotorType.kBrushless)
self.spark_PID_controller_right_front = self.spark_neo_right_front.getPIDController()
self.spark_PID_controller_right_rear = self.spark_neo_right_rear.getPIDController()
self.spark_PID_controller_left_front = self.spark_neo_left_front.getPIDController()
self.spark_PID_controller_left_rear = self.spark_neo_left_rear.getPIDController()
wpilib.Timer.delay(0.02)
# swap encoders to get sign right
# changing them up for mechanum vs WCD
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(self.spark_neo_left_front)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(self.spark_neo_left_rear)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(self.spark_neo_right_front)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(self.spark_neo_right_rear)
wpilib.Timer.delay(0.02)
# Configure encoders and controllers
# should be wheel_diameter * pi / gear_ratio - and for the old double reduction gear box
# the gear ratio was either 5.67:1 or 4.17:1. With the shifter (low gear) I think it was a 12.26.
conversion_factor = 8.0 * 3.141 / 4.17
err_1 = self.sparkneo_encoder_1.setPositionConversionFactor(conversion_factor)
err_2 = self.sparkneo_encoder_2.setPositionConversionFactor(conversion_factor)
err_3 = self.sparkneo_encoder_3.setPositionConversionFactor(conversion_factor)
err_4 = self.sparkneo_encoder_4.setPositionConversionFactor(conversion_factor)
wpilib.Timer.delay(0.02)
# TODO - figure out if I want to invert the motors or the encoders
self.spark_neo_left_front.setInverted(False)
self.spark_neo_left_rear.setInverted(False)
self.spark_neo_right_front.setInverted(False)
self.spark_neo_right_rear.setInverted(False)
if err_1 != rev.CANError.kOK or err_2 != rev.CANError.kOK:
print(f"Warning: drivetrain encoder issue with neo1 returning {err_1} and neo3 returning {err_2}")
self.configure_controllers()
self.display_PIDs()
else:
# get a pretend drivetrain for the simulator
self.spark_neo_left_front = wpilib.Talon(1)
self.spark_neo_left_rear = wpilib.Talon(2)
self.spark_neo_right_front = wpilib.Talon(3)
self.spark_neo_right_rear = wpilib.Talon(4)
# Not sure if speedcontrollergroups work with the single sparkmax in python - seems to complain
drive_type = 'mechanum'
if drive_type == 'wcd':
# WCD
err_1 = self.spark_neo_left_rear.follow(self.spark_neo_left_front)
err_2 = self.spark_neo_right_rear.follow(self.spark_neo_right_front)
if err_1 != rev.CANError.kOK or err_2 != rev.CANError.kOK:
print(f"Warning: drivetrain follower issue with neo2 returning {err_1} and neo4 returning {err_2}")
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front)
self.speedgroup_right = SpeedControllerGroup(self.spark_neo_right_front)
self.differential_drive = DifferentialDrive(self.speedgroup_left, self.speedgroup_right)
self.drive = self.differential_drive
self.differential_drive.setMaxOutput(1.0)
if drive_type == 'mechanum':
# Mechanum
#TODO: Reset followers in software
self.speedgroup_lfront = SpeedControllerGroup(self.spark_neo_left_front)
self.speedgroup_lrear = SpeedControllerGroup(self.spark_neo_left_rear)
self.speedgroup_rfront = SpeedControllerGroup(self.spark_neo_right_front)
self.speedgroup_rrear = SpeedControllerGroup(self.spark_neo_right_rear)
self.mechanum_drive = MecanumDrive(self.speedgroup_lfront, self.speedgroup_lrear, self.speedgroup_rfront, self.speedgroup_rrear)
#self.mechanum_drive = MecanumDrive(self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear)
self.mechanum_drive.setMaxOutput(self.mecanum_power_limit)
self.drive = self.mechanum_drive
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(0.1)
# self.differential_drive.setSensitivity(0.5)
# wpilib.LiveWindow.addActuator("DriveTrain", "spark_neo_l1", self.spark_neo_l1)
# wpilib.LiveWindow.addActuator("DriveTrain", "spark_neo_r3", self.spark_neo_r3)
# wpilib.LiveWindow.addActuator("DriveTrain", "spark_neo_l2", self.spark_neo_l2)
# wpilib.LiveWindow.addActuator("DriveTrain", "spark_neo_r4", self.spark_neo_r4)
except rev.CANError:
print('Buncha CAN errors)')
def initDefaultCommand(self):
"""
When other commands aren't using the drivetrain, allow arcade drive with the joystick.
"""
self.setDefaultCommand(DriveByJoystick(self.robot))
def spark_with_stick(self, thrust=0, strafe=0, z_rotation=0, gyroAngle=0):
'''Simplest way to drive with a joystick'''
#self.differential_drive.arcadeDrive(x_speed, self.twist_sensitivity * z_rotation, False)
self.mechanum_drive.driveCartesian(xSpeed=thrust, ySpeed=strafe, zRotation=self.twist_sensitivity*z_rotation)
def stop(self):
#self.differential_drive.arcadeDrive(0, 0)
self.mechanum_drive.driveCartesian(0, 0, 0)
def smooth_drive(self, thrust, strafe, twist):
'''A less jittery way to drive with a joystick
TODO: See if this can be implemented in hardware - seems like the acceleration limit can be set there
TODO: Should be ok to slow down quickly, but not speed up - check this code
TODO: Set a smartdash to see if we are in software limit mode - like with a boolean
'''
deadzone = 0.05
self.is_limited = False
# thrust section
if math.fabs(thrust) < deadzone:
self.current_thrust = 0
else:
if math.fabs(thrust - self.current_thrust) < self.acceleration_limit:
self.current_thrust = thrust
else:
if thrust - self.current_thrust > 0:
# accelerating forward
self.current_thrust = self.current_thrust + self.acceleration_limit
self.is_limited = True
elif (thrust - self.current_thrust) < 0 and thrust > 0:
# ok to slow down quickly, although really the deadzone should catch this
self.current_thrust = thrust
elif (thrust - self.current_thrust) > 0 and thrust < 0:
# ok to slow down quickly, although really the deadzone should catch this
self.current_thrust = thrust
else:
# accelerating backward
self.current_thrust = self.current_thrust - self.acceleration_limit
self.is_limited = True
#strafe section
if math.fabs(strafe) < deadzone:
self.current_strafe = 0
else:
if math.fabs(strafe - self.current_strafe) < self.acceleration_limit:
self.current_strafe = strafe
else:
if strafe - self.current_strafe > 0:
self.current_strafe = self.current_strafe + self.acceleration_limit
else:
self.current_strafe = self.current_strafe - self.acceleration_limit
# twist section
if math.fabs(twist) < deadzone:
self.current_twist = 0
else:
if math.fabs(twist - self.current_twist) < self.acceleration_limit:
self.current_twist = twist
else:
if twist - self.current_twist > 0:
self.current_twist = self.current_twist + self.acceleration_limit
else:
self.current_twist = self.current_twist - self.acceleration_limit
#self.differential_drive.arcadeDrive(self.current_thrust, self.current_twist, True)
# TODO - fix this for mechanum x and y
self.mechanum_drive.driveCartesian(xSpeed=self.current_thrust, ySpeed=self.current_strafe, zRotation=self.current_twist)
def tank_drive(self, left, right):
'''Not sure why we would ever need this, but it's here if we do'''
pass
#self.differential_drive.tankDrive(left, right)
def get_position(self):
''':returns: The encoder position of one of the Neos'''
return self.sparkneo_encoder_1.getPosition()
def set_velocity(self, velocity):
controllers = [self.spark_PID_controller_left_front, self.spark_PID_controller_left_rear ,
self.spark_PID_controller_right_front, self.spark_PID_controller_right_rear]
multipliers = [1.0, 1.0, -1.0, -1.0]
for multiplier, controller in zip(multipliers, controllers):
controller.setReference(multiplier * velocity, rev.ControlType.kVelocity, 1)
def goToSetPoint(self, set_point):
self.reset()
controllers = [self.spark_PID_controller_left_front, self.spark_PID_controller_left_rear ,
self.spark_PID_controller_right_front, self.spark_PID_controller_right_rear]
multipliers = [1.0, 1.0, -1.0, -1.0]
for multiplier, controller in zip(multipliers, controllers):
# controller.setReference(multiplier * set_point, rev.ControlType.kPosition)
controller.setReference(multiplier * set_point, rev.ControlType.kSmartMotion, pidSlot=1)
def reset(self):
if self.robot.isReal():
err_1 = self.sparkneo_encoder_1.setPosition(0)
err_2 = self.sparkneo_encoder_2.setPosition(0)
err_3 = self.sparkneo_encoder_3.setPosition(0)
err_4 = self.sparkneo_encoder_4.setPosition(0)
if err_1 != rev.CANError.kOK or err_2 != rev.CANError.kOK or err_3 != rev.CANError.kOK or err_4 != rev.CANError.kOK:
print(f"Warning: drivetrain reset issue with neo1 returning {err_1} and neo3 returning {err_2}")
self.x = 0
self.y = 0
wpilib.Timer.delay(0.02)
def configure_controllers(self, pid_only=False):
'''Set the PIDs, etc for the controllers, slot 0 is position and slot 1 is velocity'''
error_list = []
if not pid_only:
controllers = [self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear]
for controller in controllers:
#error_list.append(controller.restoreFactoryDefaults())
#Timer.delay(0.01)
#looks like they orphaned the setIdleMode - it doesn't work. Try ConfigParameter
#error_list.append(controller.setIdleMode(rev.IdleMode.kBrake))
controller.setParameter(rev.ConfigParameter.kIdleMode, rev.IdleMode.kBrake)
error_list.append(controller.setSmartCurrentLimit(self.current_limit))
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxAccel_0, self.maxacc)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxAccel_1, self.maxacc)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxVelocity_0, self.maxvel)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxVelocity_1, self.maxvel)
Timer.delay(0.01)
#controller.burnFlash()
controllers = [self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear]
for controller in controllers:
error_list.append(controller.setParameter(rev.ConfigParameter.kP_0, self.PID_dict_pos['kP']))
error_list.append(controller.setParameter(rev.ConfigParameter.kP_1, self.PID_dict_vel['kP']))
error_list.append(controller.setParameter(rev.ConfigParameter.kI_0, self.PID_dict_pos['kI']))
error_list.append(controller.setParameter(rev.ConfigParameter.kI_1, self.PID_dict_vel['kI']))
error_list.append(controller.setParameter(rev.ConfigParameter.kD_0, self.PID_dict_pos['kD']))
error_list.append(controller.setParameter(rev.ConfigParameter.kD_1, self.PID_dict_vel['kD']))
error_list.append(controller.setParameter(rev.ConfigParameter.kIZone_0, self.PID_dict_pos['kIz']))
error_list.append(controller.setParameter(rev.ConfigParameter.kIZone_1, self.PID_dict_vel['kIz']))
error_list.append(controller.setParameter(rev.ConfigParameter.kF_0, self.PID_dict_pos['kFF']))
error_list.append(controller.setParameter(rev.ConfigParameter.kF_1, self.PID_dict_vel['kFF']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMax_0, self.PID_dict_pos['kMaxOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMax_1, self.PID_dict_vel['kMaxOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMin_0, self.PID_dict_pos['kMinOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMin_1, self.PID_dict_vel['kMinOutput']))
#controller.burnFlash()
Timer.delay(0.02)
# if 1 in error_list or 2 in error_list:
# print(f'Issue in configuring controllers: {error_list}')
# else:
print(f'Results of configuring controllers: {error_list}')
def change_PIDs(self, factor=1, dict_0=None, dict_1=None):
'''Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change
can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set
slot 0 (position) or slot 1 (velocity) '''
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
if dict_0 == None:
self.PID_dict_pos[key] *= factor
else:
self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier
SmartDashboard.putString("alert",
f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}")
if dict_1 == None:
self.PID_dict_vel[key] *= factor
else:
self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier
SmartDashboard.putString("alert",
f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}")
self.configure_controllers(pid_only=True)
self.display_PIDs()
def display_PIDs(self):
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
SmartDashboard.putNumber(key + '_0', self.PID_multiplier * self.PID_dict_pos[key])
SmartDashboard.putNumber(key + '_1', self.PID_multiplier * self.PID_dict_vel[key])
def print_PIDs(self):
print(f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}")
print(f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}")
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() - self.sparkneo_encoder_3.getPosition())
except:
print(f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}")
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
SmartDashboard.putNumber("Robot X", round(self.x, 2))
SmartDashboard.putNumber("Robot Y", round(self.y, 2))
SmartDashboard.putNumber("Position Enc1", round(self.sparkneo_encoder_1.getPosition(), 2))
SmartDashboard.putNumber("Position Enc2", round(self.sparkneo_encoder_2.getPosition(), 2))
SmartDashboard.putNumber("Position Enc3", round(self.sparkneo_encoder_3.getPosition(), 2))
SmartDashboard.putNumber("Position Enc4", round(self.sparkneo_encoder_4.getPosition(), 2))
SmartDashboard.putNumber("Velocity Enc1", round(self.sparkneo_encoder_1.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc2", round(self.sparkneo_encoder_2.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc3", round(self.sparkneo_encoder_3.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc4", round(self.sparkneo_encoder_4.getVelocity(), 2))
#SmartDashboard.putNumber("Power M1", round(self.spark_neo_left_front.getAppliedOutput(), 2))
#SmartDashboard.putNumber("Power M3", round(self.spark_neo_right_front.getAppliedOutput(), 2))
SmartDashboard.putNumber("Current M1", round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber("Current M3", round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
self.display_PIDs()
SmartDashboard.putString("alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}")
SmartDashboard.putString("Controller1 Idle", str(self.spark_neo_left_front.getIdleMode()))
SmartDashboard.putNumber("Enc1 Conversion", self.sparkneo_encoder_1.getPositionConversionFactor())
