I was editing code and the drive malfunctioned. The 3pi+ does not boot properly and only the start light and reset light turn on. When connected via USB the drive now appears as a “DVD” drive and is empty.
I’m assuming the drive corrupted or something but I would like a
fix ASAP. I have backed up the code but I need to figure out how to reinstall firmware. Any help is appreciated.
if i cant get it to go into bootloader mode is there nothing i can do?
I was able to get it into bootloader mode by doing the B + Reset method. Reset + plugging in did not work. (My issue is resolved)
I am worried about this repeating. This happened after I experimented with threading on the 3pi+. Could it happen again with more threading stuff?
Hello.
I moved your post to the “Robots” section of the forum since it seems more appropriate.
It looks like something corrupted the filesystem on the robot. It is not entirely clear to me how that happened, but if it keeps happening could you post the code you are running so we can try to reproduce the problem?
Brandon
from pololu_3pi_2040_robot import robot
display = robot.Display()
imu = robot.IMU()
motors = robot.Motors()
imu.reset()
imu.enable_default()
encoders = robot.Encoders()
import _thread
import time
# Your calibration data
hard_iron = [-0.953304, -0.154999, -0.537719] # offsets (hard iron)
soft_iron = [
[1.665072, -0.032199, -0.112787],
[-0.032199, 2.168006, -0.078278],
[-0.112787, -0.078278, 1.973728]
] # soft iron matrix
current_angle = None
angle_lock = _thread.allocate_lock()
def mat_vec_mul(matrix, vector):
return [
matrix[0][0]*vector[0] + matrix[0][1]*vector[1] + matrix[0][2]*vector[2],
matrix[1][0]*vector[0] + matrix[1][1]*vector[1] + matrix[1][2]*vector[2],
matrix[2][0]*vector[0] + matrix[2][1]*vector[1] + matrix[2][2]*vector[2],
]
def compensate_magnetometer(m_raw):
# Subtract hard iron offsets
corrected = [m_raw[i] - hard_iron[i] for i in range(3)]
# Apply soft iron correction matrix
compensated = mat_vec_mul(soft_iron, corrected)
return compensated
import math
class Madgwick:
def __init__(self, beta=0.1, sample_period=0.01):
self.beta = beta
self.sample_period = sample_period
self.q = [1.0, 0.0, 0.0, 0.0]
self.initial_yaw = None
def update(self, gx, gy, gz, ax, ay, az, mx, my, mz):
q1, q2, q3, q4 = self.q
norm_acc = math.sqrt(ax*ax + ay*ay + az*az)
if norm_acc == 0:
return
ax /= norm_acc
ay /= norm_acc
az /= norm_acc
norm_mag = math.sqrt(mx*mx + my*my + mz*mz)
if norm_mag == 0:
return
mx /= norm_mag
my /= norm_mag
mz /= norm_mag
# Auxiliary variables to avoid repeated arithmetic
_2q1mx = 2.0 * q1 * mx
_2q1my = 2.0 * q1 * my
_2q1mz = 2.0 * q1 * mz
_2q2mx = 2.0 * q2 * mx
# Compute reference direction of Earth's magnetic field
hx = mx * q1*q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2*q2 + 2.0 * q2 * my * q3 + 2.0 * q2 * mz * q4 - mx * q3*q3 - mx * q4*q4
hy = 2.0 * q1 * mx * q4 + my * q1*q1 - 2.0 * q1 * mz * q2 + 2.0 * q2 * mx * q3 - my * q2*q2 + my * q3*q3 + 2.0 * q3 * mz * q4 - my * q4*q4
bx = math.sqrt(hx * hx + hy * hy)
bz = -2.0 * q1 * mx * q3 + 2.0 * q1 * my * q2 + mz * q1*q1 + 2.0 * q2 * mx * q4 - mz * q2*q2 + 2.0 * q3 * my * q4 - mz * q3*q3 + mz * q4*q4
# Estimated direction of gravity and magnetic field
vx = 2.0 * (q2 * q4 - q1 * q3)
vy = 2.0 * (q1 * q2 + q3 * q4)
vz = q1*q1 - q2*q2 - q3*q3 + q4*q4
wx = 2.0 * bx * (0.5 - q3*q3 - q4*q4) + 2.0 * bz * (q2*q4 - q1*q3)
wy = 2.0 * bx * (q2*q3 - q1*q4) + 2.0 * bz * (q1*q2 + q3*q4)
wz = 2.0 * bx * (q1*q3 + q2*q4) + 2.0 * bz * (0.5 - q2*q2 - q3*q3)
# Error is sum of cross product between estimated and measured direction of gravity and magnetic field
ex = (ay * vz - az * vy) + (my * wz - mz * wy)
ey = (az * vx - ax * vz) + (mz * wx - mx * wz)
ez = (ax * vy - ay * vx) + (mx * wy - my * wx)
# Apply feedback terms
gx += self.beta * ex
gy += self.beta * ey
gz += self.beta * ez
# Integrate quaternion rate and normalize
qDot1 = -0.5 * (q2 * gx + q3 * gy + q4 * gz)
qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy)
qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx)
qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx)
q1 += qDot1 * self.sample_period
q2 += qDot2 * self.sample_period
q3 += qDot3 * self.sample_period
q4 += qDot4 * self.sample_period
norm_q = math.sqrt(q1*q1 + q2*q2 + q3*q3 + q4*q4)
self.q = [q1 / norm_q, q2 / norm_q, q3 / norm_q, q4 / norm_q]
def get_dps(self):
q1, q2, q3, q4 = self.q
yaw = math.atan2(2*(q1*q4 + q2*q3), 1 - 2*(q3*q3 + q4*q4))
if self.initial_yaw is None:
self.initial_yaw = yaw # Store first reading
yaw -= self.initial_yaw # Subtract offset
yaw_deg = math.degrees(yaw)
# Keep in range [-180, 180]
if yaw_deg > 180:
yaw_deg -= 360
elif yaw_deg < -180:
yaw_deg += 360
return yaw_deg
def get_yaw_wrapped(self):
q1, q2, q3, q4 = self.q
yaw = math.atan2(2*(q1*q4 + q2*q3), 1 - 2*(q3*q3 + q4*q4))
if self.initial_yaw is None:
self.initial_yaw = yaw # Store first reading
yaw -= self.initial_yaw # Subtract offset
# Convert to degrees
yaw_deg = math.degrees(-yaw)
# Wrap yaw to 0-360 clockwise
yaw_deg = yaw_deg % 360
if yaw_deg < 0:
yaw_deg += 360
return yaw_deg
madgwick = Madgwick(beta=0.05, sample_period=0.05) # set your sample period (higher to reduce drift)
def calibrate_gyro(imu, samples=200):
print("Calibrating gyroscope... Keep robot completely still.")
gx_bias = 0
gy_bias = 0
gz_bias = 0
for i in range(samples):
imu.read()
gx, gy, gz = imu.gyro.last_reading_dps
gx_bias += gx
gy_bias += gy
gz_bias += gz
time.sleep(0.01) # 10 ms delay between samples
gx_bias /= samples
gy_bias /= samples
gz_bias /= samples
print("Gyro bias (dps):", gx_bias, gy_bias, gz_bias)
return gx_bias, gy_bias, gz_bias
programFinished = False
last_time = time.ticks_ms()
calibrated = False
gx_bias = 0
gy_bias = 0
gz_bias = 0
if not calibrated:
gx_bias, gy_bias, gz_bias = calibrate_gyro(imu)
calibrated = True
def angle_thread():
global current_angle
last_time = time.ticks_ms()
while True:
imu.read()
# Get time difference for Madgwick update
now = time.ticks_ms()
dt = time.ticks_diff(now, last_time) / 1000.0
last_time = now
# Read sensors
gx, gy, gz = imu.gyro.last_reading_dps
ax, ay, az = imu.acc.last_reading_g
raw_mag = imu.mag.last_reading_gauss
# Normalize and compensate magnetometer as before (your code)
gx, gy, gz = imu.gyro.last_reading_dps
gx = (gx - gx_bias) * (math.pi /180)
gy = (gy - gy_bias) * (math.pi /180)
gz = (gz - gz_bias) * (math.pi /180)
ax, ay, az = imu.acc.last_reading_g
norm = math.sqrt(ax**2 + ay**2 + az**2)
ax /= norm
ay /= norm
az /= norm
calibrated_mag = compensate_magnetometer(raw_mag)
mx, my, mz = calibrated_mag
norm = math.sqrt(mx**2 + my**2 + mz**2)
mx /= norm
my /= norm
mz /= norm
# Update Madgwick filter
madgwick.sample_period = dt
madgwick.update(gx, gy, gz, ax, ay, az, mx, my, mz)
# Get the yaw angle
angle = madgwick.get_yaw_wrapped()
# Store it thread-safely
with angle_lock:
current_angle = angle
time.sleep(0.01) # Adjust delay as needed
if not calibrated:
gx_bias, gy_bias, gz_bias = calibrate_gyro(imu)
calibrated = True
_thread.start_new_thread(angle_thread, ())
while not programFinished:
with angle_lock:
angle = current_angle
if angle is not None:
display.text("angle:", 0, 0)
print(angle)
# Example: call move_forward at some point or based on angle
# move_forward(500)
time.sleep(0.1) # Main loop delay
while not programFinished:
imu.read()
raw_mag = imu.mag.last_reading_gauss
gx, gy, gz = imu.gyro.last_reading_dps
gx = (gx - gx_bias) * (math.pi /180)
gy = (gy - gy_bias) * (math.pi /180)
gz = (gz - gz_bias) * (math.pi /180)
ax, ay, az = imu.acc.last_reading_g
norm = math.sqrt(ax**2 + ay**2 + az**2)
ax /= norm
ay /= norm
az /= norm
calibrated_mag = compensate_magnetometer(raw_mag)
mx, my, mz = calibrated_mag
norm = math.sqrt(mx**2 + my**2 + mz**2)
mx /= norm
my /= norm
mz /= norm
now = time.ticks_ms()
dt = time.ticks_diff(now, last_time) / 1000.0
last_time = now
madgwick.sample_period = dt
display.fill(0)
madgwick.update(gx, gy, gz, ax, ay, az, mx, my, mz)
currDps = (madgwick.get_yaw_wrapped())
display.text("this:", 0, 0)
display.text(str(currDps), 0, 10)
display.show()
I should note I was using the Serial terminal for this (thats where the printed values were given).
I have been told a possible root cause is that I don’t safely eject the drive, I just disconnect it… I was doing this before implementing threading but maybe because of something to do with threading, disconnecting during program running causes corruption. That is my only working hypothesis right now.