Hey Guys, I’m coding some of the tracking algorithms for doing odometry. I’ve read the 5225A document, and I’ve looked at the recently posted template from 2775 (It seems to work pretty well, but I really don’t code in C++ or Vexcode pro, so I can’t use that). Here is my code for normal Vexcode Python. This is kinda pseudocode, but I’d like some feedback on the other things I need to program (I’m planning to use a 2 encoder + IMU config). Any and all help is appreciated!

```
WHEEL_RADIUS = 2.0 # Wheel radius in cm
COUNTS_PER_REV = 360 # Encoder counts per revolution
# Global Variables
encoder_x = 0
encoder_y = 0
gyro_heading = 0
x = 0
y = 0
theta = 0
def setup():
# Set up encoder and gyro objects
global encoder_x, encoder_y, gyro
encoder_x = Encoder(port1, port2)
encoder_y = Encoder(port3, port4)
gyro = Gyro(port5)
# Set up encoder properties
encoder_x.setCountsPerRevolution(COUNTS_PER_REV)
encoder_y.setCountsPerRevolution(COUNTS_PER_REV)
encoder_x.resetRotation()
encoder_y.resetRotation()
# Set up gyro properties
gyro.calibrate()
def loop():
global encoder_x, encoder_y, gyro_heading, x, y, theta
# Read encoder and gyro values
x_ticks = encoder_x.rotation()
y_ticks = encoder_y.rotation()
gyro_heading = gyro.heading()
# Calculate linear displacement in X and Y direction
linear_displacement_x = x_ticks * WHEEL_RADIUS
linear_displacement_y = y_ticks * WHEEL_RADIUS
# Update position and orientation
theta = gyro_heading
x += linear_displacement_x * cos(theta)
y += linear_displacement_y * sin(theta)
# Print position in the cartesian coordinate system
print("(x, y) = (%.2f, %.2f)" % (x, y))
```

Code for tracking where the robot is ^

```
def navigate_to_center():
global x, y, theta
# Set target position
target_x = 0
target_y = 0
# Set control parameters
kp_x = 0.1
kp_y = 0.1
kp_theta = 0.5
while True:
# Calculate error in X, Y, and orientation
error_x = target_x - x
error_y = target_y - y
error_theta = 0 - theta
# Calculate control inputs
v_x = kp_x * error_x
v_y = kp_y * error_y
omega = kp_theta * error_theta
# Calculate motor speeds for each wheel
v_left = v_x - v_y + omega
v_right = v_x + v_y - omega
# Set motor speeds
motor1.set_velocity(v_left)
motor2.set_velocity(v_left)
motor3.set_velocity(v_right)
motor4.set_velocity(v_right)
# Check if target position has been reached
if abs(error_x) < 0.1 and abs(error_y) < 0.1 and abs(error_theta) < 0.1:
break
```

Code for driving to the center of the field ^

I’m tagging @2775Josh because of their knowledge of PID and some of my code basically being a translation from their new template. As stated previously, I really want feedback for this, and to see what I’m missing. Thanks in advance!