def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(0.75, 0.02, 1.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(self.distance_pid_abs_error)
self.distance_pid.setToleranceBuffer(3)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-5.0, 5.0)
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
self.logger = logging.getLogger("chassis")
class AlignmentController:
drivetrain = Drivetrain
targeting = Targeting
rate = .5
kP = 0.05
kI = 0
kD = 0.005
kF = 0
def __init__(self):
self.angle = None
self.angle_pid_controller = PIDController(Kp=self.kP,
Ki=self.kI,
Kd=self.kD,
Kf=self.kF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.rate, self.rate)
self.nt = NetworkTables.getTable('limelight')
def get_position(self):
return self.drivetrain.get_position()
def get_angle(self):
angle = self.targeting.get_data().x
if angle != 0.0:
self.angle = angle
return self.angle
def found(self):
fnd = self.targeting.get_data().found
if fnd == 1:
return True
else:
return False
def move_to(self, position):
self.setpoint = position
self.angle_pid_controller.enable()
def pidWriteAngle(self, rate):
self.rate = rate
def execute(self):
if self.rate is not None:
if self.found():
self.drivetrain.differential_drive(0)
self.stop()
else:
self.drivetrain.manual_drive(-self.rate, self.rate)
def stop(self):
self.angle_pid_controller.disable()
def on_disable(self):
self.stop()
def config(self, robot):
self.pid = PIDController(*self.get_pid(robot.dashboard),
self.get_disalignment, self.output)
self.pid.setOutputRange(-1, 1)
self.pid.enable()
self.pid.setSetpoint(0)
self.turn = 0
self.robot = robot
def __init__(self):
self.angle = None
self.angle_pid_controller = PIDController(Kp=self.kP,
Ki=self.kI,
Kd=self.kD,
Kf=self.kF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.rate, self.rate)
self.nt = NetworkTables.getTable('limelight')
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=3.0,
Ki=0.0,
Kd=5.0,
source=self.imu.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
alphas = []
ls = []
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
alphas.append(module_dist)
module_angle = math.atan2(module.y_pos, module.x_pos)
ls.append(module_angle)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.read_steer_pos()
self.icre = ICREstimator(np.zeros(shape=(3, 1)), np.array(alphas),
np.array(ls), np.zeros(shape=(4, )))
# TODO: re-enable if we end up not using callback method
self.imu.imu.ahrs.registerCallback(self.update_odometry)
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=0.1,
Ki=0.0,
Kd=0.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
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):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(1.0, 0.02, 0.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(0.05)
self.distance_pid.setToleranceBuffer(5)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-10.0, 10.0) # TODO check that this range is good for us
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
def __init__(self):
super().__init__(self.get_position, 'position_controller')
self.set_abs_output_range(0.18, 0.7)
# Angle correction PID controller - used to maintain a straight
# heading while the encoders track distance traveled.
self._angle_offset = 0
self.angle_pid_controller = PIDController(Kp=self.kAngleP,
Ki=self.kAngleI,
Kd=self.kAngleD,
Kf=self.kAngleF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.kAngleMax,
self.kAngleMax)
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-0.7, 0.7)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0, Ki=0.0, Kd=0.2,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1/50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-3, 3)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [self.module_a, self.module_b, self.module_c, self.module_d]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
logging.debug(
"Turn to angle constructed with angle {}".format(setpoint))
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def __init__(self, pid_values: PIDValue, f_in, f_out, f_feedforwards=None, pid_key=None):
self.ff_enabled = f_feedforwards != None
if self.ff_enabled:
ff = f_feedforwards
else:
ff = lambda x: 0
self.pid_dash_enabled = pid_key != None
if self.pid_dash_enabled:
self.pid_key = pid_key
else:
self.pid_key = ""
self.pid_values = pid_values
self.pid_controller = PIDController(
0, 0, 0,
f_in,
lambda x: f_out(x + ff(self.get_target(), x))
)
self.pid_values.update_controller(self.pid_controller)
def __init__(self, kP, kI, kD, kF, tolerance, encoder_function,
update_function, direction, period, is_enabled):
self.is_enabled = is_enabled
self.direction = direction
self.encoder_function = encoder_function
self.motor_output = 0
self.pid_controller = PIDController(kP,
kI,
kD,
kF,
source=self,
output=self,
period=period)
#self.pid_controller.controlLoop._thread.setDaemon(True)
self.pid_controller.setInputRange(-5, 5)
self.pid_controller.setOutputRange(-1.0, 1.0)
self.pid_controller.setAbsoluteTolerance(tolerance)
self.update_function = update_function
self.pid_controller.setContinuous(True)
def start(self):
# Setting up the left controller
self.left_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_left_encoder,
self.left_output)
self.left_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.left_holder,
output=self.left_holder,
period=self.period)
self.left_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.left_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.left_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.left_pid_contorller.setContinuous(True)
# Setting up the right controller
self.right_holder = IOEncoderHolder(self.encoder_ticks_per_revolution,
self.wheel_diameter,
self.chassis.get_right_encoder,
self.right_output)
self.right_pid_contorller = PIDController(self.kp,
self.ki,
self.kd,
self.kf,
source=self.right_holder,
output=self.right_holder,
period=self.period)
self.right_pid_contorller.setInputRange(-self.input_range,
self.input_range)
self.right_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.right_pid_contorller.setAbsoluteTolerance(self.tolerance)
self.right_pid_contorller.setContinuous(True)
# Setting up the turn controller
self.turn_holder = IOGyroHolder(self.chassis.get_angle,
self.turn_output)
self.turn_pid_contorller = PIDController(0.02,
self.ki,
self.kd,
self.kf,
source=self.turn_holder,
output=self.turn_holder,
period=self.period)
self.turn_pid_contorller.setInputRange(0, 360)
self.turn_pid_contorller.setOutputRange(-self.output_range,
self.output_range)
self.turn_pid_contorller.setAbsoluteTolerance(10)
self.turn_pid_contorller.setContinuous(True)
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0,
Ki=0.0,
Kd=0.2,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-3, 3)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
self.momentum = False
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold_on()
# matrix which translates column vector of [x, y, z] in robot frame of
# reference to module [x, y] movement
self.A_matrix = np.array([[1, 0, -1], [0, 1, 1], [1, 0, -1],
[0, 1, -1], [1, 0, 1], [0, 1, -1], [1, 0, 1],
[0, 1, 1]])
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in our
# matrix
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
z_comp = module_dist / 2
# third column in A matrix already encodes direction of robot's
# vz index upon the module's axis, just need to multiply to
# encode magnitude
self.A_matrix[i * 2, 2] = z_comp * self.A_matrix[i * 2, 2]
self.A_matrix[i * 2 + 1, 2] = z_comp * self.A_matrix[i * 2 + 1, 2]
for module in self.modules:
module.reset_steer_setpoint()
self.last_heading = self.bno055.getAngle()
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if self.vz is None:
self.momentum = False
if not self.momentum:
pid_z = self.heading_pid.get()
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
vz = input_vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
heading = self.bno055.getAngle()
heading_delta = heading - self.last_heading
timestep_average_heading = heading - heading_delta / 2
delta_x, delta_y, delta_theta = self.robot_movement_from_odometry(
odometry_outputs, timestep_average_heading)
v_x, v_y, v_z = self.robot_movement_from_odometry(
velocity_outputs, heading)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_theta += delta_theta
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
SmartDashboard.putNumber('module_a_speed',
self.modules[0].current_speed)
SmartDashboard.putNumber('module_b_speed',
self.modules[1].current_speed)
SmartDashboard.putNumber('module_c_speed',
self.modules[2].current_speed)
SmartDashboard.putNumber('module_d_speed',
self.modules[3].current_speed)
SmartDashboard.putNumber('module_a_pos',
self.modules[0].current_measured_azimuth)
SmartDashboard.putNumber('module_b_pos',
self.modules[1].current_measured_azimuth)
SmartDashboard.putNumber('module_c_pos',
self.modules[2].current_measured_azimuth)
SmartDashboard.putNumber('module_d_pos',
self.modules[3].current_measured_azimuth)
SmartDashboard.putNumber('odometry_x', self.odometry_x)
SmartDashboard.putNumber('odometry_y', self.odometry_y)
SmartDashboard.putNumber('odometry_theta', self.odometry_theta)
SmartDashboard.putNumber('odometry_delta_x', delta_x)
SmartDashboard.putNumber('odometry_delta_y', delta_y)
SmartDashboard.putNumber('odometry_delta_theta', delta_theta)
SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel)
SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel)
SmartDashboard.putNumber('odometry_z_vel', self.odometry_z_vel)
NetworkTables.flush()
self.last_heading = heading
def robot_movement_from_odometry(self, odometry_outputs, angle):
lstsq_ret = np.linalg.lstsq(self.A_matrix, odometry_outputs, rcond=-1)
x, y, theta = lstsq_ret[0].reshape(3)
x_field, y_field = self.field_orient(x, y, 2 * math.pi - angle)
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
"""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def field_orient(vx, vy, heading):
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = False
self.hold_heading = True
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=0.1,
Ki=0.0,
Kd=0.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
def on_enable(self):
self.heading_pid.reset()
self.set_heading_sp_current()
# matrix which translates column vector of [x, y, z] in robot frame of
# reference to module [x, y] movement
self.A_matrix = np.array([[1, 0, -1], [0, 1, 1], [1, 0, -1],
[0, 1, -1], [1, 0, 1], [0, 1, -1], [1, 0, 1],
[0, 1, 1]])
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in our
# matrix
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
z_comp = module_dist / 2
# third column in A matrix already encodes direction of robot's
# vz index upon the module's axis, just need to multiply to
# encode magnitude
self.A_matrix[i * 2, 2] = z_comp * self.A_matrix[i * 2, 2]
self.A_matrix[i * 2 + 1, 2] = z_comp * self.A_matrix[i * 2 + 1, 2]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
for module in self.modules:
module.reset_steer_setpoint()
def execute(self):
pid_z = 0
if self.hold_heading:
pid_z = self.heading_pid.get()
vz = self.vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
if hal.isSimulation():
from hal_impl.data import hal_data
angle = math.radians(-hal_data['robot']['bno055'])
else:
angle = self.bno055.getAngle()
vx, vy = self.field_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
delta_x, delta_y, delta_theta = self.robot_movement_from_odometry(
odometry_outputs)
v_x, v_y, v_z = self.robot_movement_from_odometry(velocity_outputs)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_theta += delta_theta
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
def robot_movement_from_odometry(self, odometry_outputs):
lstsq_ret = np.linalg.lstsq(self.A_matrix, odometry_outputs, rcond=-1)
x, y, theta = lstsq_ret[0].reshape(3)
angle = self.bno055.getAngle()
x_field, y_field = self.field_orient(x, y, angle)
return x_field, y_field, theta
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
:param vx: The vx (forward) component of the robot's desired velocity. In m/s.
:param vy: The vy (leftward) component of the robot's desired velocity. In m/s.
:param vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s."""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def field_orient(vx, vy, heading):
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
class TurnToAngle(Command):
def _setMotors(self, signal):
signal = signal if abs(
signal) > Constants.TURN_TO_ANGLE_MIN_OUTPUT else math.copysign(
Constants.TURN_TO_ANGLE_MIN_OUTPUT, signal)
Dash.putNumber("Turn To Angle Output", signal)
self.drive.setPercentOutput(signal, -signal, signal, -signal)
def __init__(self, setpoint):
"""Turn to setpoint (degrees)."""
super().__init__()
self.drive = drive.Drive()
self.odemetry = odemetry.Odemetry()
self.requires(self.drive)
self.setpoint = setpoint
src = self.odemetry.pidgyro
self.PID = PIDController(Constants.TURN_TO_ANGLE_KP,
Constants.TURN_TO_ANGLE_KI,
Constants.TURN_TO_ANGLE_KD, src,
self._setMotors)
logging.debug(
"Turn to angle constructed with angle {}".format(setpoint))
self.PID.setInputRange(-180.0, 180.0)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setContinuous(True)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setPIDSourceType(PIDController.PIDSourceType.kDisplacement)
def initialize(self):
self.PID.setP(Constants.TURN_TO_ANGLE_KP)
self.PID.setI(Constants.TURN_TO_ANGLE_KI)
self.PID.setD(Constants.TURN_TO_ANGLE_KD)
self.PID.setOutputRange(-Constants.TURN_TO_ANGLE_MAX_OUTPUT,
Constants.TURN_TO_ANGLE_MAX_OUTPUT)
self.PID.setAbsoluteTolerance(Constants.TURN_TO_ANGLE_TOLERANCE)
self.PID.setSetpoint(self.setpoint)
self.PID.enable()
def execute(self):
Dash.putNumber("Turn To Angle Error", self.PID.getError())
# logging.info(f"Turn To Angle Error {self.PID.getError()}")
def isFinished(self):
return self.PID.onTarget()
def end(self):
logging.debug("Finished turning to angle {}".format(self.setpoint))
self.PID.disable()
snaplistener.SnapListener(0).start()
def interrupted(self):
self.end()
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 Chassis:
correct_range = 1.65 # m
length = 498.0 # mm
width = 600.0 # mm
motor_dist = math.sqrt((width / 2) ** 2 + (length / 2) ** 2) # distance of motors from the center of the robot
# x component y component
vz_components = {'x': (width / 2) / motor_dist, 'y': (length / 2) / motor_dist} # multiply both by vz and the
# the number that you need to multiply the vz components by to get them in the appropriate directions
# vx vy
"""module_params = {'a': {'args': {'drive':13, 'steer':14, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}"""
module_params = {'a': {'args': {'drive':13, 'steer':11, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}
# Use the magic here!
bno055 = BNO055
vision = Vision
range_finder = RangeFinder
heading_hold_pid_output = BlankPIDOutput
heading_hold_pid = PIDController
def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(1.0, 0.02, 0.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(0.05)
self.distance_pid.setToleranceBuffer(5)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-10.0, 10.0) # TODO check that this range is good for us
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold = True
self.field_oriented = True
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.heading_hold_pid.reset()
# Update the current module steer setpoint to be the current position
# Stops the unwind problem
for module in self._modules.values():
module._steer.set(module._steer.getPosition())
def onTarget(self):
for module in self._modules.values():
if not abs(module._steer.getError()) < 50:
return False
return True
def toggle_field_oriented(self):
self.field_oriented = not self.field_oriented
def toggle_vision_tracking(self):
self.track_vision = not self.track_vision
if self.track_vision:
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
def toggle_range_holding(self, setpoint=1.65):
if not self.range_setpoint:
self.range_setpoint = setpoint
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
else:
self.range_setpoint = 0.0
def zero_encoders(self):
for module in self._modules.values():
module.zero_distance()
def field_displace(self, x, y):
'''Use the distance PID to displace the robot by x,y
in field reference frame.'''
d = math.sqrt((x ** 2 + y ** 2))
fx, fy = field_orient(x, y, self.bno055.getHeading())
self.distance_pid_heading = math.atan2(fy, fx)
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid.setSetpoint(d)
self.distance_pid.reset()
self.distance_pid.enable()
def pidGet(self):
return self.distance
def getPIDSourceType(self):
return PIDSource.PIDSourceType.kDisplacement
@property
def distance(self):
distances = 0.0
for module in self._modules.values():
distances += abs(module.distance) / module.drive_counts_per_metre
return distances / 4.0
def drive(self, vX, vY, vZ, absolute=False):
motor_vectors = {}
for name, params in Chassis.module_params.items():
motor_vectors[name] = {'x': vX + vZ * params['vz']['x'],
'y': vY + vZ * params['vz']['y']
}
# convert the vectors to polar coordinates
polar_vectors = {}
max_mag = 1.0
for name, motor_vector in motor_vectors.items():
polar_vectors[name] = {'dir': math.atan2(motor_vector['y'],
motor_vector['x']
),
'mag': math.sqrt(motor_vector['x'] ** 2
+ motor_vector['y'] ** 2
)
}
if abs(polar_vectors[name]['mag']) > max_mag:
max_mag = polar_vectors[name]['mag']
for name in polar_vectors.keys():
polar_vectors[name]['mag'] /= max_mag
if absolute:
polar_vectors[name]['mag'] = None
continue
for name, polar_vector in polar_vectors.items():
self._modules[name].steer(polar_vector['dir'], polar_vector['mag'])
def execute(self):
if self.field_oriented and self.inputs[3] is not None:
self.inputs[0:2] = field_orient(self.inputs[0], self.inputs[1], self.bno055.getHeading())
# Are we in setpoint displacement mode?
if self.distance_pid.isEnable():
if self.distance_pid.onTarget():
if self.pid_counter > 10:
self.reset_distance_pid = False
# Let's see if we need to move further
x = y = 0.0
if self.range_setpoint:
x = self.range_finder.pidGet() - self.range_setpoint
if x > 0.5:
x = 0.5
elif x < -0.5:
x = -0.5
if self.track_vision:
y = self.vision.pidGet()*0.3#*self.range_finder.pidGet()*0.3 # TODO we need proper scaling factors here
if y > 0.3:
y = 0.3
elif y < -0.3:
y = -0.3
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid_heading = constrain_angle(math.atan2(y, x)+self.bno055.getAngle())
self.distance_pid.setSetpoint(math.sqrt(x**2+y**2))
self.distance_pid.reset()
self.distance_pid.enable()
self.pid_counter = 0
else:
self.pid_counter += 1
# Keep driving
self.vx = math.cos(self.distance_pid_heading) * self.distance_pid_output.output
self.vy = math.sin(self.distance_pid_heading) * self.distance_pid_output.output
else:
self.vx = self.inputs[0] * self.inputs[3] # multiply by throttle
self.vy = self.inputs[1] * self.inputs[3] # multiply by throttle
if self.heading_hold:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.inputs[2] != 0.0:
self.momentum = True
if not self.momentum:
self.heading_hold_pid.enable()
self.vz = self.heading_hold_pid_output.output
else:
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.vz = self.inputs[2] * self.inputs[3] # multiply by throttle
if self.lock_wheels:
for _, params, module in zip(Chassis.module_params.items(),
self._modules):
direction = constrain_angle(math.atan2(params['vz']['y'],
params['vz']['x']) +
math.pi / 2.0)
module.steer(direction, 0.0)
else:
self.drive(self.vx, self.vy, self.vz)
def toggle_heading_hold(self):
self.heading_hold = not self.heading_hold
def set_heading_setpoint(self, setpoint):
self.heading_hold_pid.setSetpoint(constrain_angle(setpoint))
def on_range_target(self):
return abs(self.range_finder.getDistance() - self.range_setpoint) < 0.1
def on_vision_target(self):
return (self.vision.no_vision_counter == 0.0 and
abs(self.vision.pidGet()) < 0.035)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.input = 0
self.input2 = 0
self.lastCargoCommand = "unknown"
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
self.F = 0
#should be 0.4
SmartDashboard.putNumber("F Gain", self.F)
[self.kP, self.kI, self.kD, self.kF] = [0, 0, 0, 0]
cargoController = PIDController(self.kP, self.kI, self.kD, self.kF,
self, self)
self.cargoController = cargoController
self.cargoController.disable()
self.pidValuesForMode = {
"resting": [-50, self.kP, self.kI, self.kD, 0.15 / -50],
"cargoship": [-28, self.kP, self.kI, self.kD, 0.0],
"intake": [50, self.kP, self.kI, self.kD, 0.0],
"rocket": [5, self.kP, self.kI, self.kD, 0.19 / 5],
}
class Chassis:
correct_range = 1.65 # m
length = 498.0 # mm
width = 600.0 # mm
vision_scale_factor = 0.3 # units of m/(vision unit)
distance_pid_abs_error = 0.05 # metres
motor_dist = math.sqrt((width / 2) ** 2 + (length / 2) ** 2) # distance of motors from the center of the robot
# x component y component
vz_components = {'x': (width / 2) / motor_dist, 'y': (length / 2) / motor_dist} # multiply both by vz and the
# the number that you need to multiply the vz components by to get them in the appropriate directions
# vx vy
"""module_params = {'a': {'args': {'drive':13, 'steer':14, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':True,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':True,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}"""
module_params = {'a': {'args': {'drive':13, 'steer':11, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':30,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y': vz_components['y']}},
'b': {'args': {'drive':8, 'steer':9, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':109,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x':-vz_components['x'], 'y':-vz_components['y']}},
'c': {'args': {'drive':2, 'steer':4, 'absolute':False,
'reverse_drive':False, 'reverse_steer':True, 'zero_reading':536,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y':-vz_components['y']}},
'd': {'args': {'drive':3, 'steer':6, 'absolute':False,
'reverse_drive':True, 'reverse_steer':True, 'zero_reading':389,
'drive_encoder':True, 'reverse_drive_encoder':True},
'vz': {'x': vz_components['x'], 'y': vz_components['y']}}
}
# Use the magic here!
bno055 = BNO055
vision = Vision
range_finder = RangeFinder
heading_hold_pid_output = BlankPIDOutput
heading_hold_pid = PIDController
def __init__(self):
super().__init__()
# A - D
# | |
# B - C
self._modules = {}
for name, params in Chassis.module_params.items():
self._modules[name] = SwerveModule(**(params['args']))
self._modules[name]._drive.setVoltageRampRate(50.0)
self.field_oriented = True
self.inputs = [0.0, 0.0, 0.0, 0.0]
self.vx = self.vy = self.vz = 0.0
self.track_vision = False
self.range_setpoint = None
self.heading_hold = True
self.lock_wheels = False
self.momentum = False
import robot
self.rescale_js = robot.rescale_js
self.distance_pid_heading = 0.0 # Relative to field
self.distance_pid_output = BlankPIDOutput()
# TODO tune the distance PID values
self.distance_pid = PIDController(0.75, 0.02, 1.0,
self, self.distance_pid_output)
self.distance_pid.setAbsoluteTolerance(self.distance_pid_abs_error)
self.distance_pid.setToleranceBuffer(3)
self.distance_pid.setContinuous(False)
self.distance_pid.setInputRange(-5.0, 5.0)
self.distance_pid.setOutputRange(-0.4, 0.4)
self.distance_pid.setSetpoint(0.0)
self.reset_distance_pid = False
self.pid_counter = 0
self.logger = logging.getLogger("chassis")
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold = True
self.field_oriented = True
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.heading_hold_pid.reset()
# Update the current module steer setpoint to be the current position
# Stops the unwind problem
for module in self._modules.values():
module._steer.set(module._steer.getPosition())
def onTarget(self):
for module in self._modules.values():
if not abs(module._steer.getError()) < 50:
return False
return True
def toggle_field_oriented(self):
self.field_oriented = not self.field_oriented
def toggle_vision_tracking(self):
self.track_vision = not self.track_vision
self.logger.info("Vision Tracking: " + str(self.track_vision))
if self.track_vision:
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
def toggle_range_holding(self, setpoint=1.65):
if not self.range_setpoint:
self.range_setpoint = setpoint
self.zero_encoders()
self.distance_pid.setSetpoint(0.0)
self.distance_pid.enable()
else:
self.range_setpoint = 0.0
def zero_encoders(self):
for module in self._modules.values():
module.zero_distance()
def field_displace(self, x, y):
'''Use the distance PID to displace the robot by x,y
in field reference frame.'''
d = math.sqrt((x ** 2 + y ** 2))
fx, fy = field_orient(x, y, self.bno055.getHeading())
self.distance_pid_heading = math.atan2(fy, fx)
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid.setSetpoint(d)
self.distance_pid.reset()
self.distance_pid.enable()
def pidGet(self):
return self.distance
def getPIDSourceType(self):
return PIDSource.PIDSourceType.kDisplacement
@property
def distance(self):
distances = 0.0
for module in self._modules.values():
distances += abs(module.distance) / module.drive_counts_per_metre
return distances / 4.0
def drive(self, vX, vY, vZ, absolute=False):
motor_vectors = {}
for name, params in Chassis.module_params.items():
motor_vectors[name] = {'x': vX + vZ * params['vz']['x'],
'y': vY + vZ * params['vz']['y']
}
# convert the vectors to polar coordinates
polar_vectors = {}
max_mag = 1.0
for name, motor_vector in motor_vectors.items():
polar_vectors[name] = {'dir': math.atan2(motor_vector['y'],
motor_vector['x']
),
'mag': math.sqrt(motor_vector['x'] ** 2
+ motor_vector['y'] ** 2
)
}
if abs(polar_vectors[name]['mag']) > max_mag:
max_mag = polar_vectors[name]['mag']
for name in polar_vectors.keys():
polar_vectors[name]['mag'] /= max_mag
if absolute:
polar_vectors[name]['mag'] = None
continue
for name, polar_vector in polar_vectors.items():
self._modules[name].steer(polar_vector['dir'], polar_vector['mag'])
def execute(self):
if self.field_oriented and self.inputs[3] is not None:
self.inputs[0:2] = field_orient(self.inputs[0], self.inputs[1], self.bno055.getHeading())
# Are we in setpoint displacement mode?
if self.distance_pid.isEnable():
if self.distance_pid.onTarget():
if self.pid_counter > 10:
self.reset_distance_pid = False
# Let's see if we need to move further
x = y = 0.0
if self.range_setpoint and not self.on_range_target():
x = self.range_finder.pidGet() - self.range_setpoint
if x > 0.5:
x = 0.5
elif x < -0.5:
x = -0.5
self.logger.info("X: " + str(x))
if self.track_vision and not self.on_vision_target():
self.logger.info("Tracking Vision")
y = self.vision.pidGet() * self.vision_scale_factor
if y > 0.5:
y = 0.5
elif y < -0.5:
y = -0.5
self.logger.info("Y: " + str(y))
elif self.on_vision_target():
self.track_vision = False
self.distance_pid.disable()
self.zero_encoders()
self.distance_pid_heading = constrain_angle(math.atan2(y, x)+self.bno055.getAngle())
self.distance_pid.setSetpoint(math.sqrt(x**2+y**2))
self.distance_pid.reset()
self.distance_pid.enable()
self.pid_counter = 0
else:
self.pid_counter += 1
# Keep driving
self.vx = math.cos(self.distance_pid_heading) * self.distance_pid_output.output
self.vy = math.sin(self.distance_pid_heading) * self.distance_pid_output.output
else:
self.vx = self.inputs[0] * self.inputs[3] # multiply by throttle
self.vy = self.inputs[1] * self.inputs[3] # multiply by throttle
if self.heading_hold:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.inputs[2] != 0.0:
self.momentum = True
if not self.momentum:
self.heading_hold_pid.enable()
self.vz = self.heading_hold_pid_output.output
else:
self.heading_hold_pid.setSetpoint(self.bno055.getAngle())
self.vz = self.inputs[2] * self.inputs[3] # multiply by throttle
self.logger.info("Vision: %s Rangefinder: %s Distance: %s Setpoint: %s" % (self.vision.pidGet(), self.range_finder.pidGet(), self.distance, self.distance_pid.getSetpoint()))
if self.lock_wheels:
for _, params, module in zip(Chassis.module_params.items(),
self._modules):
direction = constrain_angle(math.atan2(params['vz']['y'],
params['vz']['x']) +
math.pi / 2.0)
module.steer(direction, 0.0)
else:
self.drive(self.vx, self.vy, self.vz)
def toggle_heading_hold(self):
self.heading_hold = not self.heading_hold
def set_heading_setpoint(self, setpoint):
self.heading_hold_pid.setSetpoint(constrain_angle(setpoint))
def on_range_target(self):
return abs(self.range_finder.pidGet() - self.range_setpoint) < self.distance_pid_abs_error * 2.0
def on_vision_target(self):
return (self.vision.no_vision_counter == 0.0 and
abs(self.vision.pidGet() * self.vision_scale_factor) < self.distance_pid_abs_error * 2.0)
class SwerveChassis:
WIDTH = 1
LENGTH = 0.88
imu: IMU
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
# tunables here purely for debugging
odometry_x = tunable(0)
odometry_y = tunable(0)
# odometry_theta = tunable(0)
# odometry_x_vel = tunable(0)
# odometry_y_vel = tunable(0)
# odometry_z_vel = tunable(0)
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.last_vx, self.last_vy = self.vx, self.vy
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=3.0,
Ki=0.0,
Kd=5.0,
source=self.imu.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
alphas = []
ls = []
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
alphas.append(module_dist)
module_angle = math.atan2(module.y_pos, module.x_pos)
ls.append(module_angle)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.read_steer_pos()
self.icre = ICREstimator(np.zeros(shape=(3, 1)), np.array(alphas),
np.array(ls), np.zeros(shape=(4, )))
# TODO: re-enable if we end up not using callback method
self.imu.imu.ahrs.registerCallback(self.update_odometry)
def set_heading_sp_current(self):
self.set_heading_sp(self.imu.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.heading_hold_on()
self.last_heading = self.imu.getAngle()
self.odometry_updated = False
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.imu.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if not self.momentum:
pid_z = self.heading_pid_out.output
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
if abs(pid_z) < 0.1 and math.hypot(self.vx, self.vy) < 0.01:
pid_z = 0
vz = input_vz + pid_z
angle = self.imu.getAngle()
# TODO: re-enable if we end up not using callback method
# self.update_odometry()
# self.odometry_updated = False # reset for next timestep
for module in self.modules:
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module.dist * vz * math.sin(module.angle)
vz_y = module.dist * vz * math.cos(module.angle)
if self.field_oriented:
vx, vy = self.robot_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
def update_odometry(self, o, sensor_timestamp):
# TODO: re-enable if we end up not using callback method
# if self.odometry_updated:
# return
heading = self.imu.getAngle()
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
betas = []
phi_dots = []
for i, module in enumerate(self.modules):
module.update_odometry()
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
betas.append(module.measured_azimuth)
phi_dots.append(module.wheel_angular_vel)
q = np.array(betas)
lambda_e = self.icre.estimate_lmda(q)
print(lambda_e)
vx, vy, vz = self.robot_movement_from_odometry(velocity_outputs,
heading)
delta_x, delta_y, delta_z = self.robot_movement_from_odometry(
odometry_outputs, heading, z_vel=vz)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_x_vel = vx
self.odometry_y_vel = vy
self.odometry_z_vel = vz
self.last_heading = heading
self.odometry_updated = True
def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0):
lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None)
x, y, theta = lstsq_ret[0].reshape(3)
# TODO: re-enable if we move back to running in the same thread
x_field, y_field = self.field_orient(x, y, angle + z_vel * (1 / 200))
# x_field, y_field = self.field_orient(x, y, angle)
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self,
vx: float,
vy: float,
vz: float,
*,
field_oriented: bool = True):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
field_oriented: Whether the inputs are field or robot oriented.
"""
self.last_vx, self.last_vy = self.vx, self.vy
self.vx = vx
self.vy = vy
self.vz = vz
self.field_oriented = field_oriented
@staticmethod
def robot_orient(vx, vy, heading):
"""Turn a vx and vy relative to the field into a vx and vy based on the
robot.
Args:
vx: vx to robot orient
vy: vy to robot orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: robot oriented vx speed
float: robot oriented vy speed
"""
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@staticmethod
def field_orient(vx, vy, heading):
"""Turn a vx and vy relative to the robot into a vx and vy based on the
field.
Args:
vx: vx to field orient
vy: vy to field orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: field oriented vx speed
float: field oriented vy speed
"""
oriented_vx = vx * math.cos(heading) - vy * math.sin(heading)
oriented_vy = vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@property
def position(self):
return np.array([[self.odometry_x], [self.odometry_y]], dtype=float)
@property
def speed(self):
return math.hypot(self.odometry_x_vel, self.odometry_y_vel)
@property
def all_aligned(self):
return all(module.aligned for module in self.modules)
class PositionController(BasePIDComponent):
drivetrain = Drivetrain
kP = tunable(0.05)
kI = tunable(0)
kD = tunable(0.005)
kF = tunable(0.0)
kToleranceInches = tunable(0.2 if hal.HALIsSimulation() else 0.75)
kIzone = tunable(0)
angle_controller = AngleController
kAngleP = 0.05 if hal.HALIsSimulation() else 0.1
kAngleI = 0
kAngleD = 0
kAngleF = 0
kAngleMax = 0.18
# Angle correction factor
angle = 0
def __init__(self):
super().__init__(self.get_position, 'position_controller')
self.set_abs_output_range(0.18, 0.7)
# Angle correction PID controller - used to maintain a straight
# heading while the encoders track distance traveled.
self._angle_offset = 0
self.angle_pid_controller = PIDController(Kp=self.kAngleP,
Ki=self.kAngleI,
Kd=self.kAngleD,
Kf=self.kAngleF,
source=self.get_angle,
output=self.pidWriteAngle)
self.angle_pid_controller.setInputRange(-180, 180)
self.angle_pid_controller.setContinuous(True)
self.angle_pid_controller.setOutputRange(-self.kAngleMax,
self.kAngleMax)
def get_position(self):
return self.drivetrain.get_position()
def get_angle(self):
return self.angle_controller.get_angle() - self._angle_offset
def reset_position_and_heading(self):
self.drivetrain.shift_low_gear()
self.drivetrain.reset_position()
self._angle_offset = self.angle_controller.get_angle()
self.angle_pid_controller.setSetpoint(0)
def move_to(self, position):
self.setpoint = position
self.angle_pid_controller.enable()
def is_at_location(self):
return self.enabled and \
abs(self.get_position() - self.setpoint) < self.kToleranceInches
def pidWrite(self, output):
self.rate = -output
def pidWriteAngle(self, angle):
self.angle = angle
def execute(self):
super().execute()
if self.rate is not None:
if self.is_at_location():
self.stop()
else:
self.drivetrain.differential_drive(self.rate,
self.angle,
squared=False,
force=True)
def stop(self):
self.drivetrain.differential_drive(0)
self.angle_pid_controller.disable()
def on_disable(self):
self.stop()
class PIDVisionAlign(Segment):
def get_pid(self, sd):
return [
# float(sd.getNumber("KP", 0.553) * 4),
# float(sd.getNumber("KI", 0.033) * 4),
# float(sd.getNumber("KD", 0.073) * 4)
0.503 * 4,
0.033 * 4,
0.073 * 4
]
def config(self, robot):
self.pid = PIDController(*self.get_pid(robot.dashboard),
self.get_disalignment, self.output)
self.pid.setOutputRange(-1, 1)
self.pid.enable()
self.pid.setSetpoint(0)
self.turn = 0
self.robot = robot
def get_disalignment(self):
return self.robot.dashboard.getNumber("CENTROID", 0.5) - 0.5
def output(self, n):
self.turn = n
def free(self):
self.pid.free()
def update(self, robot):
err = self.pid.getError()
self.robot.chassis.arcade_drive(-0.6, self.turn)
print(self.turn)
print("err", err)
self.pid.setPID(*self.get_pid(robot.dashboard))
class SuperPIDController:
def __init__(self, pid_values: PIDValue, f_in, f_out, f_feedforwards=None, pid_key=None):
self.ff_enabled = f_feedforwards != None
if self.ff_enabled:
ff = f_feedforwards
else:
ff = lambda x: 0
self.pid_dash_enabled = pid_key != None
if self.pid_dash_enabled:
self.pid_key = pid_key
else:
self.pid_key = ""
self.pid_values = pid_values
self.pid_controller = PIDController(
0, 0, 0,
f_in,
lambda x: f_out(x + ff(self.get_target(), x))
)
self.pid_values.update_controller(self.pid_controller)
def get_target(self):
if self.pid_controller.isEnabled():
return self.pid_controller.getSetpoint()
else:
return 0
def configure_controller(self, output_range=(-1, 1), percent_tolerance=1):
self.pid_controller.setOutputRange(output_range[0], output_range[1])
self.pid_controller.setPercentTolerance(percent_tolerance)
def update_values(self, pid_values: PIDValue):
self.pid_values = pid_values
self.pid_values.update_controller(self.pid_controller)
def update_from_dash(self):
if self.pid_dash_enabled:
self.update_values(get_pid(self.pid_key))
def push_to_dash(self):
if self.pid_dash_enabled:
put_pid(self.pid_key, self.pid_values)
def get_on_target(self):
return self.pid_controller.onTarget() if self.pid_controller.isEnabled() else True
def stop(self):
self.reset()
self.pid_controller.disable()
def start(self):
self.reset()
self.pid_controller.enable()
def reset(self):
self.pid_controller.reset()
def run_setpoint(self, value):
self.pid_controller.setSetpoint(value)
self.start()
class SwerveChassis:
bno055: BNO055
module_a: SwerveModule
module_b: SwerveModule
module_c: SwerveModule
module_d: SwerveModule
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.field_oriented = True
self.hold_heading = True
self.momentum = False
def setup(self):
# Heading PID controller
self.heading_pid_out = ChassisPIDOutput()
self.heading_pid = PIDController(Kp=6.0,
Ki=0.0,
Kd=1.0,
source=self.bno055.getAngle,
output=self.heading_pid_out,
period=1 / 50)
self.heading_pid.setInputRange(-math.pi, math.pi)
self.heading_pid.setOutputRange(-2, 2)
self.heading_pid.setContinuous()
self.heading_pid.enable()
self.modules = [
self.module_a, self.module_b, self.module_c, self.module_d
]
self.odometry_x = 0
self.odometry_y = 0
self.odometry_theta = 0
self.odometry_x_vel = 0
self.odometry_y_vel = 0
self.odometry_z_vel = 0
def set_heading_sp_current(self):
self.set_heading_sp(self.bno055.getAngle())
def set_heading_sp(self, setpoint):
self.heading_pid.setSetpoint(setpoint)
self.heading_pid.enable()
def heading_hold_on(self):
self.set_heading_sp_current()
self.heading_pid.reset()
self.hold_heading = True
def heading_hold_off(self):
self.heading_pid.disable()
self.hold_heading = False
def on_enable(self):
self.bno055.resetHeading()
self.heading_hold_on()
self.A = np.array([[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1],
[1, 0, 1], [0, 1, 1], [1, 0, 1], [0, 1, 1]],
dtype=float)
# figure out the contribution of the robot's overall rotation about the
# z axis to each module's movement, and encode that information in a
# column vector
# self.z_axis_adjustment = np.zeros((8, 1))
for i, module in enumerate(self.modules):
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# self.z_axis_adjustment[i*2, 0] = -module_dist*math.sin(module_angle)
# self.z_axis_adjustment[i*2+1, 0] = module_dist*math.cos(module_angle)
self.A[i * 2, 2] = -module_dist * math.sin(module_angle)
self.A[i * 2 + 1, 2] = module_dist * math.cos(module_angle)
module.reset_encoder_delta()
module.reset_steer_setpoint()
self.last_heading = self.bno055.getAngle()
self.odometry_updated = False
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if not self.momentum:
pid_z = self.heading_pid_out.output
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
vz = input_vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist * vz * math.sin(module_angle)
vz_y = module_dist * vz * math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
angle = self.bno055.getAngle()
vx, vy = self.robot_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx + vz_x, vy + vz_y)
self.update_odometry()
self.odometry_updated = False # reset for next timestep
SmartDashboard.putNumber('module_a_speed',
self.modules[0].current_speed)
SmartDashboard.putNumber('module_b_speed',
self.modules[1].current_speed)
SmartDashboard.putNumber('module_c_speed',
self.modules[2].current_speed)
SmartDashboard.putNumber('module_d_speed',
self.modules[3].current_speed)
SmartDashboard.putNumber('module_a_pos',
self.modules[0].current_measured_azimuth)
SmartDashboard.putNumber('module_b_pos',
self.modules[1].current_measured_azimuth)
SmartDashboard.putNumber('module_c_pos',
self.modules[2].current_measured_azimuth)
SmartDashboard.putNumber('module_d_pos',
self.modules[3].current_measured_azimuth)
SmartDashboard.putNumber('odometry_x', self.odometry_x)
SmartDashboard.putNumber('odometry_y', self.odometry_y)
SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel)
SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel)
SmartDashboard.putNumber('odometry_z_vel', self.odometry_z_vel)
NetworkTables.flush()
def update_odometry(self):
if self.odometry_updated:
return
heading = self.bno055.getAngle()
heading_delta = constrain_angle(heading - self.last_heading)
heading_adjustment_factor = 1
adjusted_heading = heading - heading_adjustment_factor * heading_delta
timestep_average_heading = adjusted_heading - heading_delta / 2
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i * 2, 0] = odometry_x
odometry_outputs[i * 2 + 1, 0] = odometry_y
velocity_outputs[i * 2, 0] = velocity_x
velocity_outputs[i * 2 + 1, 0] = velocity_y
module.reset_encoder_delta()
v_x, v_y, v_z = self.robot_movement_from_odometry(
velocity_outputs, heading)
delta_x, delta_y, delta_z = self.robot_movement_from_odometry(
odometry_outputs, heading, z_vel=v_z)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
self.last_heading = heading
SmartDashboard.putNumber('odometry_delta_x', delta_x)
SmartDashboard.putNumber('odometry_delta_y', delta_y)
SmartDashboard.putNumber('imu_heading', heading)
SmartDashboard.putNumber('heading_delta', heading_delta)
SmartDashboard.putNumber('average_heading', timestep_average_heading)
self.odometry_updated = True
def robot_movement_from_odometry(self, odometry_outputs, angle, z_vel=0):
lstsq_ret = np.linalg.lstsq(self.A, odometry_outputs, rcond=None)
x, y, theta = lstsq_ret[0].reshape(3)
x_field, y_field = self.field_orient(x, y, angle + z_vel * (2.5 / 50))
return x_field, y_field, theta
def set_velocity_heading(self, vx, vy, heading):
"""Set a translational velocity and a rotational orientation to achieve.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
heading: the heading the robot is to face.
"""
self.vx = vx
self.vy = vy
self.vz = None
self.set_heading_sp(heading)
def set_inputs(self, vx, vy, vz):
"""Set chassis vx, vy, and vz components of inputs.
Args:
vx: (forward) component of the robot's desired velocity. In m/s.
vy: (leftward) component of the robot's desired velocity. In m/s.
vz: The vz (counter-clockwise rotation) component of the robot's
desired [angular] velocity. In radians/s.
"""
self.vx = vx
self.vy = vy
self.vz = vz
def set_field_oriented(self, field_oriented):
self.field_oriented = field_oriented
@staticmethod
def robot_orient(vx, vy, heading):
"""Turn a vx and vy relative to the field into a vx and vy based on the
robot.
Args:
vx: vx to robot orient
vy: vy to robot orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: robot oriented vx speed
float: robot oriented vy speed
"""
oriented_vx = vx * math.cos(heading) + vy * math.sin(heading)
oriented_vy = -vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@staticmethod
def field_orient(vx, vy, heading):
"""Turn a vx and vy relative to the robot into a vx and vy based on the
field.
Args:
vx: vx to field orient
vy: vy to field orient
heading: current heading of the robot. In radians CCW from +x axis.
Returns:
float: field oriented vx speed
float: field oriented vy speed
"""
oriented_vx = vx * math.cos(heading) - vy * math.sin(heading)
oriented_vy = vx * math.sin(heading) + vy * math.cos(heading)
return oriented_vx, oriented_vy
@property
def position(self):
return np.array([[self.odometry_x], [self.odometry_y]], dtype=float)
@property
def speed(self):
return math.hypot(self.odometry_x_vel, self.odometry_y_vel)
def __init__(self):
print('TankDrive: init called')
super().__init__('TankDrive')
self.debug = False
self.logPrefix = "TankDrive: "
# Speed controllers
if robotmap.driveLine.speedControllerType == "VICTORSP":
try:
self.leftSpdCtrl = wpilib.VictorSP(robotmap.driveLine.leftMotorPort)
if robotmap.driveLine.invertLeft:
self.leftSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating left speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightSpdCtrl = wpilib.VictorSP(robotmap.driveLine.rightMotorPort)
if robotmap.driveLine.invertRight:
self.rightSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating right speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
elif robotmap.driveLine.speedControllerType == "TALON":
try:
self.leftSpdCtrl = wpilib.Talon(robotmap.driveLine.leftMotorPort)
if robotmap.driveLine.invertLeft:
self.leftSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating left speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightSpdCtrl = wpilib.Talon(robotmap.driveLine.rightMotorPort)
if robotmap.driveLine.invertRight:
self.rightSpdCtrl.setInverted(True)
except Exception as e:
print("{}Exception caught instantiating right speed controller. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
else:
print("{}Configured speed controller type in robotmap not recognized - {}".format(self.logPrefix, robotmap.driveLine.speedControllerType))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise RuntimeError('Driveline speed controller specified in robotmap not valid: ' + robotmap.driveLine.speedControllerType)
# Encoders
try:
self.leftEncoder = wpilib.Encoder(robotmap.driveLine.leftEncAPort, robotmap.driveLine.leftEncBPort,
robotmap.driveLine.leftEncReverse, robotmap.driveLine.leftEncType)
self.leftEncoder.setDistancePerPulse(robotmap.driveLine.inchesPerTick)
except Exception as e:
print("{}Exception caught instantiating left encoder. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
try:
self.rightEncoder = wpilib.Encoder(robotmap.driveLine.rightEncAPort, robotmap.driveLine.rightEncBPort,
robotmap.driveLine.rightEncReverse, robotmap.driveLine.rightEncType)
self.rightEncoder.setDistancePerPulse(robotmap.driveLine.inchesPerTick)
except Exception as e:
print("{}Exception caught instantiating left encoder. {}".format(self.logPrefix, e))
if not wpilib.DriverStation.getInstance().isFmsAttached():
raise
# PID Setup
self.tankPID = TankPID()
self.pidController = PIDController(0.0, 0.0, 0.0, self.tankPID, self.tankPID)
self.tankPIDIndirect = TankPIDIndirect()
self.pidControllerIndirect = PIDController(0.0, 0.0, 0.0, self.tankPIDIndirect, self.tankPIDIndirect)
self.turnPID = TankPIDTurn(0.0, 0.5)
self.pidTurnController = PIDController(0.0, 0.0, 0.0, self.turnPID, self.turnPID)
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
class DriveController:
wheel_radius = 0.1 # meters
encoder_ticks_per_revolution = 256 * 3 * 1.8
def __init__(self, kP, kI, kD, kF, tolerance, encoder_function,
update_function, direction, period, is_enabled):
self.is_enabled = is_enabled
self.direction = direction
self.encoder_function = encoder_function
self.motor_output = 0
self.pid_controller = PIDController(kP,
kI,
kD,
kF,
source=self,
output=self,
period=period)
#self.pid_controller.controlLoop._thread.setDaemon(True)
self.pid_controller.setInputRange(-5, 5)
self.pid_controller.setOutputRange(-1.0, 1.0)
self.pid_controller.setAbsoluteTolerance(tolerance)
self.update_function = update_function
self.pid_controller.setContinuous(True)
def start(self, setpoint):
self.setpoint = setpoint
self.pid_controller.setSetpoint(setpoint)
self.pid_controller.enable()
def pidWrite(self, output):
print("output voltage", output)
# print("distance for cotroller", self.pid_controller.getError())
self.pid_controller.setSetpoint(self.setpoint)
self.update_function(self.direction * output)
def is_finished(self):
return self.pid_controller.onTarget()
def close(self):
self.pid_controller.close()
def pidGet(self):
distance_in_encoder = self.encoder_function()
distance_in_meters = (
distance_in_encoder /
self.encoder_ticks_per_revolution) * self.wheel_radius
print("dist", distance_in_meters)
return distance_in_meters
def getPIDSourceType(self):
return "meter"
def getPIDOutputType(self):
return "output"
def update_controller(self, controller: PIDController) -> None:
controller.P = self.p
controller.I = self.i
controller.D = self.d
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
class DriveTrain(Subsystem):
"""DriveTrain: is the subsystem responsible for motors and
devices associated with driving subystem.
As a subsystem, we represent the single point of contact
for all drivetrain-related controls. Specifically, commands
that manipulate the drivetrain should 'require' the singleton
instance (via require(robot.driveTrain)). Unless overridden,
our default command, JoystickDriver, is the means by which
driving occurs.
"""
k_minThrottleScale = 0.5
k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally
k_maxDriveSpeed = 150.0 # ~20 ft/sec
k_maxTurnSpeed = 40.0 # ~3-4 ft/sec
k_fastTurn = -1
k_mediumTurn = -.72
k_slowTurn = -.55
k_quadTicksPerWheelRev = 9830
k_wheelDiameterInInches = 14.0
k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi
k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches
k_turnKp = .1
k_turnKi = 0
k_turnKd = .3
k_turnKf = .001
k_ControlModeSpeed = 0
k_ControlModeVBus = 1
k_ControlModeDisabled = 2
class _turnHelper(PIDOutput):
"""a private helper class for PIDController-based
imu-guided turning.
"""
def __init__(self, driveTrain):
super().__init__()
self.driveTrain = driveTrain
def pidWrite(self, output):
self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed)
def __init__(self, robot, name=None):
super().__init__(name=name)
self.robot = robot
# STEP 1: instantiate the motor controllers
self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId)
self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId)
self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId)
self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId)
# Step 2: Configure the follower Talons: left & right back motors
self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID())
self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower)
self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID())
# STEP 3: Setup speed control mode for the master Talons
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
# STEP 4: Indicate the feedback device used for closed-loop
# For speed mode, indicate the ticks per revolution
self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder)
self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev)
# STEP 5: Set PID values & closed loop error
self.leftMasterMotor.setPID(0.22, 0, 0)
self.rightMasterMotor.setPID(0.22, 0, 0)
# Add ramp up rate
self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage
# change /sec: reach to
# 12V after 1sec
self.rightMasterMotor.setVoltageRampRate(48.0)
# Add SmartDashboard controls for testing
# Add SmartDashboard live windowS
LiveWindow.addActuator("DriveTrain",
"Left Master %d" % robot.map.k_DtLeftMasterId,
self.leftMasterMotor)
LiveWindow.addActuator("DriveTrain",
"Right Master %d" % robot.map.k_DtRightMasterId,
self.rightMasterMotor)
# init RobotDrive - all driving should occur through its methods
# otherwise we expect motor saftey warnings
self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor)
self.robotDrive.setSafetyEnabled(True)
# init IMU - used for driver & vision feedback as well as for
# some autonomous modes.
self.visionState = self.robot.visionState
self.imu = BNO055()
self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf,
self.imu, DriveTrain._turnHelper(self))
self.turnPID.setOutputRange(-1, 1)
self.turnPID.setInputRange(-180, 180)
self.turnPID.setPercentTolerance(2)
self.turnMultiplier = DriveTrain.k_mediumTurn
self.maxSpeed = DriveTrain.k_defaultDriveSpeed
# self.setContinuous() ?
robot.info("Initialized DriveTrain")
def initForCommand(self, controlMode):
self.leftMasterMotor.setEncPosition(0) # async call
self.rightMasterMotor.setEncPosition(0)
self.robotDrive.stopMotor()
self.robotDrive.setMaxOutput(self.maxSpeed)
if controlMode == self.k_ControlModeSpeed:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed)
elif controlMode == self.k_ControlModeVBus:
self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus)
elif controlMode == self.k_ControlModeDisabled:
# unverified codepath
self.leftMasterMotor.disableControl()
self.rightMasterMotor.disableControl()
else:
self.robot.error("Unexpected control mode")
self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" %
(self.leftMasterMotor.getControlMode(),
self.rightMasterMotor.getControlMode()))
def initDefaultCommand(self):
# control modes are integers values:
# 0 percentvbux
# 1 position
# 2 velocity
self.setDefaultCommand(JoystickDriver(self.robot))
self.robotDrive.stopMotor();
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
def stop(self):
self.robotDrive.stopMotor()
def joystickDrive(self, jsY, jsX, throttle):
""" joystickDrive - called by JoystickDriver command. Inputs
are always on the range [-1, 1]... These values can be scaled
for a better "feel", but should always be in a subset of this
range.
"""
if self.robot.isAutonomous or \
(math.fabs(jx) < 0.075 and math.fabs(jy) < .075):
# joystick dead zone or auto (where we've been scheduled via
# default command machinery)
self.robotDrive.stopMotor()
else:
st = self.scaleThrottle(throttle)
self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st)
def drive(self, outputmag, curve):
""" drive - used by drivestraight command..
"""
self.robotDrive.drive(outputmag, curve)
def driveStraight(self, speed):
"""driveStraight: invoked from AutoDriveStraight..
"""
# NB: maxOuput isn't applied via set so
# speed is supposedly measured in rpm but this depends on our
# initialization establishing encoding ticks per revolution.
# This is approximate so we rely on the observed values above.
# (DEFAULT_SPEED_MAX_OUTPUT)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = speed / self.maxSpeed
rotateValue = 0
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def startAutoTurn(self, degrees):
self.robot.info("start autoturn from %d to %d" %
(self.getHeading(), degrees))
self.turnPID.reset()
self.turnPID.setSetpoint(degrees)
self.turnPID.enable()
def endAutoTurn(self):
if self.turnPID.isEnabled():
self.turnPID.disable()
def isAutoTurning(self):
return self.turnPID.isEnabled()
def isAutoTurnFinished(self):
return self.turnPID.onTarget()
def turn(self, speed):
"""turn takes a speed, not an angle...
A negative speed is interpretted as turn left.
Note that we bypass application of setMaxOutput Which
only applies to calls to robotDrive.
"""
# In order to turn left, we want the right wheels to go
# forward and left wheels to go backward (cf: tankDrive)
# Oddly, our right master motor is reversed, so we compensate here.
# speed < 0: turn left: rightmotor(negative) (forward),
# leftmotor(negative) (backward)
# speed > 0: turn right: rightmotor(positive) (backward)
# leftmotor(positive) (forward)
if 0:
self.leftMasterMotor.set(speed)
self.rightMasterMotor.set(speed)
else:
# TODO: validate this codepath
moveValue = 0
rotateValue = speed / self.maxSpeed
self.robotDrive.arcadeDrive(moveValue, rotateValue)
def trackVision(self):
"""presumed called by either autonomous or teleop commands to
allow the vision subsystem to guide the robot
"""
if self.visionState.DriveLockedOnTarget:
self.stop()
else:
if self.isAutoTurning():
if self.isAutoTurnFinished():
self.endAutoTurn()
self.visionState.DriveLockedOnTarget = True
else:
# setup for an auto-turn
h = self.getCurrentHeading()
tg = self.getTargetHeading(h)
self.startAutoTurn(tg)
def getCurrentHeading(self):
""" getCurrentHeading returns a value between -180 and 180
"""
return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi
def scaleThrottle(self, rawThrottle):
""" scaleThrottle:
returns a scaled value between MIN_THROTTLE_SCALE and 1.0
MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation
Scale the joystick values by throttle before passing to the driveTrain
+1=bottom position; -1=top position
"""
# Throttle in the range of -1 to 1. We would like to change that to a
# range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle
# value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1))
# Then, add 1 to make the range 0-2 rather than -1 to +1
# Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale)
# Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1
#
# Check the results are in the range of k_minThrottleScale to 1, and clip
# it in case the math went horribly wrong.
result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale
if result < self.k_minThrottleScale:
# Somehow our math was wrong. Our value was too low, so force it to the minimum
result = self.k_mintThrottleScale
elif result > 1:
# Somehow our math was wrong. Our value was too high, so force it to the maximum
result = 1.0
return result
def modifyTurnSpeed(self, speedUp):
if speedUp:
self.turnMultiplier = self.k_mediumTurn
else:
self.turnMultiplier = self.k_slowTurn
def inchesToTicks(self, inches):
return int(self.k_quadTicksPerInch * inches)
def destinationReached(self, distance):
return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \
math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
