def __init__(self):
"""Initilize the Odemetry class"""
super().__init__()
self.drive = drive.Drive()
self.timestamp = 0
self.last_timestamp = 0
# Gyroscope
self.gyro = adxrs450_gyro.ADXRS450_Gyro()
self.gyro.calibrate()
self.pose = pose.Pose()
self.last_left_encoder_distance = 0
self.last_right_encoder_distance = 0
self.last_angle = 0
def update_sim(self, hal_data, now, tm_diff):
pos = self.controller.get_position()
self.odemetry.pose = pose.Pose(units.feetToInches(pos[0]),
units.feetToInches(pos[1]), pos[2])
l_signal = hal_data['CAN'][Constants.LM_MOTOR_ID]['value']
r_signal = hal_data['CAN'][Constants.RM_MOTOR_ID]['value']
speed, rotation = self.drivetrain.get_vector(-l_signal, r_signal)
hal_data['CAN'][Constants.LM_MOTOR_ID]['quad_position'] += int(
self.drivetrain.l_speed * tm_diff * self.kl_encoder)
hal_data['CAN'][Constants.RM_MOTOR_ID]['quad_position'] += int(
self.drivetrain.r_speed * tm_diff * self.kr_encoder)
self.controller.drive(speed, rotation, tm_diff)
hal_data['custom']['Pose'] = [
round(i, 2) for i in self.controller.get_position()
]
def load(self, filename):
"""Load a json path file into a spline. The paths folder is searched by default."""
path = jsonfinder.getPath(filename)
try:
with open(path) as path_data:
path_json = json.load(path_data)
for point_data in path_json:
try:
pose_data = point_data["pose"]
pose0 = pose.Pose(pose_data["x"], pose_data["y"],
pose_data["heading"])
self.addPose(pose0)
except KeyError:
logging.error("Json {} is invalid".format(filename))
return
self.updateCurves()
except FileNotFoundError:
logging.error("{} json file not found".format(filename))
def __init__(self):
"""Initilize the Odemetry class."""
super().__init__()
self.drive = drive.Drive()
self.timestamp = 0
self.last_timestamp = 0
self.dt = 0
# Gyroscope
if hal.isSimulation():
self.gyro = analoggyro.AnalogGyro(0)
self.pidgyro = pidanaloggyro.PIDAnalogGyro(self.gyro)
else:
self.gyro = PigeonIMU(intake.Intake().l_motor)
self.pidgyro = pidpigeon.PIDPigeon(self.gyro)
self.pose = pose.Pose()
self.last_left_encoder_distance = 0
self.last_right_encoder_distance = 0
self.last_angle = 0
def __init__(self):
"""Initilize the Odemetry class."""
super().__init__()
self.drive = drive.Drive()
self.timestamp = 0
self.last_timestamp = 0
self.dt = 0
self.ir_motor = ctre.WPI_TalonSRX(Constants.IR_MOTOR_ID)
# Gyroscope
if hal.isSimulation():
self.gyro = analoggyro.AnalogGyro(0)
else:
self.gyro = PigeonIMU(self.ir_motor)
self.calibrate()
self.pose = pose.Pose()
self.last_left_encoder_distance = 0
self.last_right_encoder_distance = 0
self.last_angle = 0
def reset(self):
if hal.isSimulation():
self.gyro.reset()
else:
self.gyro.setYaw(0, 0)
self.pose = pose.Pose()
