def update_outputs(self, l_enc: int, r_enc: int, angle: float):
self.l_output = -self.left_follower.calculate(l_enc)
self.r_output = self.right_follower.calculate(r_enc)
if self.reverse:
self.l_output = self.left_follower.calculate(-l_enc)
self.r_output = -self.right_follower.calculate(-r_enc)
if self.reverse:
angle *= -1
desired_heading = pathfinder.r2d(self.left_follower.getHeading())
angle_diff = pathfinder.boundHalfDegrees(desired_heading - angle)
angle_diff = angle_diff % 360
if abs(angle_diff) > 180:
if angle_diff > 0:
angle_diff = angle_diff - 360
else:
angle_diff = angle_diff + 360
turn = 0.8 * (1.0 / 80.0) * angle_diff
if not self.reverse:
self.l_output -= turn
self.r_output -= turn
else:
self.l_output += turn
self.r_output += turn
def autonomousPeriodic(self):
l = self.leftFollower.calculate(
self.l_motor_master.getSelectedSensorPosition(0))
r = self.rightFollower.calculate(
self.r_motor_master.getSelectedSensorPosition(0))
gyro_heading = (-self.navx.getAngle()
) # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(
self.leftFollower.getHeading()) # Should also be in degrees
# This is a poor man's P controller
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = 5 * (-1.0 / 80.0) * angleDifference
if self.timer.hasPeriodPassed(0.5):
print("l: " + str(l) + ", r: " + str(r) + ", turn: " + str(turn))
#print("current segment: " + str(self.leftFollower.getSegment().position))
l = l + turn
r = r - turn
if self.leftFollower.isFinished() or self.rightFollower.isFinished():
self.robot_drive.tankDrive(0, 0)
else:
# -1 is forward, so invert both values
self.robot_drive.tankDrive(-l, -r)
def execute(self):
if self.enabled:
angle_speed = self.config[
"navx_p"] / 180 * pathfinder.boundHalfDegrees(
pathfinder.r2d(self.left.getHeading()) -
self.drive.getNavx())
self.drive.setSpeeds(
self.left.calculate(self.drive.getEncLeft()) +
(angle_speed if angle_speed < 0 else 0),
self.right.calculate(self.drive.getEncRight()) -
(angle_speed if angle_speed > 0 else 0))
def autonomousPeriodic(self):
l = self.leftFollower.calculate(self.l_encoder.get())
r = self.rightFollower.calculate(self.r_encoder.get())
gyro_heading = (-self.gyro.getAngle()
) # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(
self.leftFollower.getHeading()) # Should also be in degrees
# This is a poor man's P controller
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = 5 * (-1.0 / 80.0) * angleDifference
l = l + turn
r = r - turn
# -1 is forward, so invert both values
self.robot_drive.tankDrive(-l, -r)
def charge(self, initial_call):
left_speed = self.leftFollower.calculate(self.drive.leftDistance())
right_speed = self.rightFollower.calculate(self.drive.rightDistance())
gyro_heading = (-self.drive.gyro.getAngle()
) # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(
self.leftFollower.getHeading()) # Should also be in degrees
# This is a poor man's P controller
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = 1.2 * (-1.0 / 80.0) * angleDifference
left_speed += turn
right_speed -= turn
print(left_speed, right_speed)
# -1 is forward, so invert both values
self.drive.tank(left_speed, right_speed)
def run(self):
l = self.leftFollower.calculate(sself.sensors.getLeftDistance())
r = self.rightFollower.calculate(self.sensors.getRightDistance())
# REPLACE THIS WITH NAVX
gyro_heading = -self.sensors.getCurrentAngle(
) # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(
self.leftFollower.getHeading()) # Should also be in degrees
# This is a poor man's P controller
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = 5 * (-1.0 / 80.0) * angleDifference
l = l + turn
r = r - turn
# -1 is forward, so invert both values
self.drivetrain().moveRobot(-l, -r)
def autonomousPeriodic(self):
l = self.leftFollower.calculate(self.l_encoder.get())
r = self.rightFollower.calculate(self.r_encoder.get())
gyro_heading = (
-self.gyro.getAngle()
) # Assuming the gyro is giving a value in degrees
desired_heading = pf.r2d(
self.leftFollower.getHeading()
) # Should also be in degrees
# This is a poor man's P controller
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading)
turn = .8 * (-1.0 / 80.0) * angleDifference
l = l + turn
r = r - turn
# -1 is forward, so invert both values
self.robot_drive.tankDrive(-l, -r)
def GeneratePath(path_name, file_name, waypoints, settings, reverse=False, heading_overide=False, headingValue=0.0):
"""
This function will take a set of pathfinder waypoints and create the trajectories to follow a path going through the waypoints. This path is
specific for the drivetrain controllers, so the position will use the CTRE quadrature encoders and the velocity will use the feed-forward in
units of Volts.
"""
# Generate the path using pathfinder.
info, trajectory = pf.generate(waypoints, settings.order, settings.samples, settings.period,
settings.maxVelocity, settings.maxAcceleration, settings.maxJerk)
# Modify the path for the differential drive based on the calibrated wheelbase
modifier = pf.modifiers.TankModifier(trajectory).modify(ROBOT_WHEELBASE_FT)
# Ge the left and right trajectories...left and right are reversed
rightTrajectory = modifier.getLeftTrajectory()
leftTrajectory = modifier.getRightTrajectory()
# Grab the position, velocity + acceleration for feed-forward, heading, and duration
path = {"left": [], "right": []}
headingOut = []
for i in range(len(leftTrajectory)):
if not reverse:
if heading_overide:
heading = headingValue
elif abs(pf.r2d(leftTrajectory[i].heading)) > 180:
heading = -(pf.r2d(leftTrajectory[i].heading) - 360)
else:
heading = -pf.r2d(leftTrajectory[i].heading)
else:
if heading_overide:
heading = headingValue
else:
heading = pf.r2d(leftTrajectory[i].heading) - 180
headingOut.append(heading)
path["left"].append([leftTrajectory[i].position * 4096 / # Position: CTRE SRX Mag encoder: 4096 units per rotation
(ROBOT_WHEEL_DIAMETER_FT * math.pi), # Voltage / Feed-Forward
CalculateFeedForwardVoltage(True,
leftTrajectory[i].velocity,
leftTrajectory[i].acceleration),
heading / 360, # Pigeon IMU setup for 3600 units per rotation
int(leftTrajectory[i].dt * 1000)]) # Duration
path["right"].append([rightTrajectory[i].position * 4096 /
(ROBOT_WHEEL_DIAMETER_FT * math.pi),
CalculateFeedForwardVoltage(False,
rightTrajectory[i].velocity,
rightTrajectory[i].acceleration),
heading / 360,
int(rightTrajectory[i].dt * 1000)])
# Dump the path into a pickle file which will be read up later by the RoboRIO robot code
with open(os.path.join(path_name, file_name+".pickle"), "wb") as fp:
pickle.dump(path, fp)
# Plot the data for review
x = list(i * (settings.period) for i, _ in enumerate(leftTrajectory))
plt.figure()
# plt.plot(aspect=0.5)
plt.title("Trajectory")
drawField(plt)
plt.plot([segment.y for segment in leftTrajectory],
[segment.x for segment in leftTrajectory],
marker='.', color='b')
plt.plot([segment.y for segment in rightTrajectory],
[segment.x for segment in rightTrajectory],
marker='.', color='r')
plt.gca().set_yticks(np.arange(0, 30.1, 1.0), minor=True)
plt.gca().set_yticks(np.arange(0, 30.1, 3))
plt.gca().set_xticks(np.arange(0, 27.1, 1.0), minor=True)
plt.gca().set_xticks(np.arange(0, 27.1, 3))
plt.grid(which='minor', color='grey', linestyle='--', alpha=0.25)
plt.grid(which='major', color='grey', linestyle='-', alpha=0.75)
# Plot the heading
plt.figure()
plt.title("Heading")
plt.plot(x, headingOut, marker='.')
# Plot the velocity and acceleration and look for any discontinuities
plt.figure()
plt.subplot(2, 1, 1)
plt.title("Velocity")
plt.plot(x, [segment.velocity for segment in leftTrajectory], marker='.', color='b')
plt.plot(x, [segment.velocity for segment in rightTrajectory], marker='.', color='r')
plt.yticks(np.arange(0, DRIVETRAIN_MAX_VELOCITY + 0.1, 1.0))
plt.grid()
plt.subplot(2, 1, 2)
plt.title("Acceleration")
plt.plot(x, [segment.acceleration for segment in leftTrajectory], marker='.', color='b')
plt.plot(x, [segment.acceleration for segment in rightTrajectory], marker='.', color='r')
plt.yticks(np.arange(-DRIVETRAIN_MAX_ACCELERATION, DRIVETRAIN_MAX_ACCELERATION + 1.1, 2.0))
plt.grid()
plt.tight_layout()
plt.show()
def exec_path(self):
if self.finished:
return
print('[path controller] [current] L: %s; R: %s' %
(self.drivetrain.get_left_encoder_meters(),
self.drivetrain.get_right_encoder_meters()))
l_dist = self.drivetrain.get_left_encoder_meters() \
- self.position_offset[0]
r_dist = self.drivetrain.get_right_encoder_meters() \
- self.position_offset[1]
if self.reverse:
l_dist *= -1
r_dist *= -1
try:
l_o = self.left.calculate(l_dist)
r_o = self.right.calculate(r_dist)
except Exception:
return
print('[path controller] [calculated] L: %s; R: %s' % (l_o, r_o))
gyro_heading = self.angle_controller.get_angle() - self.angle_offset
desired_heading = -pf.r2d(self.left.getHeading())
if self.reverse:
desired_heading += 180
print('[path controller] [heading] curr: %s, desired: %s' %
(gyro_heading, desired_heading))
if not self.initial_desired_heading:
self.initial_desired_heading = desired_heading
angleDifference = pf.boundHalfDegrees(desired_heading - gyro_heading -
self.initial_desired_heading)
# TURN_FACTOR = 0.025
TURN_FACTOR = 0.01
turn = TURN_FACTOR * angleDifference
if self.reverse:
turn *= -1
# print('[path controller] [angle diff] %s [desired] %s [gyro] ' +
# '%s [init desire] %s' % (angleDifference, desired_heading,
# gyro_heading))
print('[path controller] [calculated w turn] L: %s; R: %s' %
(l_o + turn, r_o - turn))
l_speed = l_o + turn
r_speed = r_o - turn
if self.reverse:
l_speed *= -1
r_speed *= -1
self.drivetrain.manual_drive(l_speed, r_speed)
if self.left.isFinished() and self.right.isFinished():
self.stop()
self.finished = True