''' Car_test_accel : A fork of motor_test.py that adds the mpu6050 accel and gyro Initial version - read in MPU6050 accel info v2 - add reading in the gyro Z axis (change in angle rate in Z) v3 - add integration to find the gyro angle v4 - add integration to accel to find speed and distance v5 - imports library Car_lib = setup devices, motor_move, accel_calibration v6 - fix bug in v5 for distance calculation add delay in reset of integrators to allow for car dynamics from precious mode to settle out add data filtering for "good" samples using standard deviation found on calibration changed dist function to a class (to remove global variables) ''' from math import sqrt import machine from time import sleep_ms, ticks_ms import pca9685 import motor from MPU6050 import MPU6050 from os import listdir, chdir from Car_lib import accel_calibration, motor_move seg_time = 2000 # how long in segment in millisec acc_time = 25 # number milliseconds between accelermeter samples n_acc = int(seg_time / acc_time) # n_acc = 5 scale = 1.0 # initialize car sdaPIN=machine.Pin(13) sclPIN=machine.Pin(14) i2c=machine.I2C(0,sda=sdaPIN, scl=sclPIN) pwm = pca9685.PCA9685(address=0x5f, i2c = i2c) # Set the PWM frequency to 60Hz, good for servos. pwm.freq(60) motors = motor.DCMotors(address=0x5f, i2c = i2c) mpu = MPU6050() # trapazoidal integration def trap(y0, y1, dt): # trapazoid integration ***** doesn't help ***** return (y0 + y1) * (dt/1000) / 2 # low pass filter def low_pass_filter(prev_value, new_value): alpha = 0.85 return alpha * prev_value + (1 - alpha) * new_value class AccelerometerGyro: def __init__(self, mpu, axc, ayc, axsd, aysd, gzc, cols, file): self.mpu = mpu self.axc = axc self.ayc = ayc self.axsd = axsd self.aysd = aysd self.gzc = gzc self.cols = cols self.file = file self.angZold = 0 self.told = 0 self.aXvel_old = 0 self.aYvel_old = 0 self.aXdist_old = 0 self.aYdist_old = 0 def get_accel(self, mode, reset): # get_accel2 includes: # - computing degrees turned with gyro # - computing distance x, y accelerometers # add a delay before taking first sample. Allows car dynamics to settle out delay = 500 # delay in ms before first sample taken if reset == 0: sleep_ms(delay) # Accelerometer Data accel = self.mpu.read_accel_data() # read the accelerometer [ms^-2] aX = accel["x"] - self.axc aY = accel["y"] - self.ayc # threshold the data if abs(aX) < self.axsd: aX = 0 if abs(aY) < self.aysd: aY = 0 gyro = self.mpu.read_gyro_data() # read the gyro [deg/s] gZ = gyro["z"] - self.gzc t = ticks_ms() # integrate the gyro rate to give angle and accel to give velocity if reset == 0: angZ = 0 self.told = t dt = 0 aXvel = 0 aYvel = 0 aXdist = 0 aYdist = 0 dist = 0 elif reset == 1: dt = (t - self.told) / 1000 aXvel = self.aXvel_old + aX * dt # update X velocity aYvel = self.aYvel_old + aY * dt # update Y velocity angZ = self.angZold + gZ * dt # update angle aXdist = 0 aYdist = 0 dist = 0 else: dt = (t - self.told) / 1000 angZ = self.angZold + gZ * dt aXvel = self.aXvel_old + aX * dt # update X velocity aYvel = self.aYvel_old + aY * dt # update Y velocity aXdist = self.aXdist_old + aXvel * dt # update X distance aYdist = self.aYdist_old + aYvel * dt # update Y distance dist = sqrt(aXdist**2 + aYdist**2) self.aXvel_old = aXvel self.aXdist_old = aXdist self.aYvel_old = aYvel self.aYdist_old = aYdist self.angZold = angZ self.told = t # store data data = { "Mode": mode, "AcX": aX, "AcY": aY, "gZ": gZ, "angZ": angZ, "aXvel": aXvel, "aYvel": aYvel, "aXdist": aXdist, "aYdist": aYdist, "dist": dist, "dt": 1000*dt } push = [str(data[k]) for k in self.cols] row = ",".join(push) self.file.write(row + "\n") return aX, aY, gZ # ------------ main program ------------------- # delay to unplug car sleep_ms(1) # create file to save results # List all files directory. print("Root directory: {} \n".format(listdir())) # Change filename and fileformat here. filename = "{}%s.{}".format("data_logs", "txt") # Increment the filename number if the file already exists. i = 0 while (filename % i) in listdir(): i += 1 # open file for writing file = open(filename % i, 'w') # cols = ["Mode", "AcX", "aXfilt", "AcY", "aYfilt", "gZ", "angZ", "dist", "dt"] # save LPF data cols = ["Mode", "AcX", "AcY", "gZ", "angZ", "aXvel", "aYvel", "aXdist", "aYdist", "dist", "dt"] file.write(",".join(cols) + "\n") # find calibration values axc, ayc, axsd, aysd, gzc = accel_calibration(mpu) accel_gyro = AccelerometerGyro(mpu, axc, ayc, axsd, aysd, gzc, cols, file) # move the car and collect data # sequence trough move of the car for n1 in range(1): # stop motor_move(motors) # stops motor as default speed=0 for n in range(n_acc): # aX, aY, gZ = get_accel2('stop',n) aX, aY, gZ = accel_gyro.get_accel('stop',n) # move forward motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=2000, m4_speed=2000, scale = 0.5) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('forward',n) # stop motor_move(motors) # stops motor as default speed=0 for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('stop',n) # move backward motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=-2000, m4_speed=-2000, scale = 0.5) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('backward',n) # stop motor_move(motors) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('stop',n) # turn left motor_move(motors, m1_speed=-2000, m2_speed=-2000, m3_speed=2000, m4_speed=2000, scale = 0.5) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('left',n) # stop motor_move(motors) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('stop',n) # turn right motor_move(motors, m1_speed=2000, m2_speed=2000, m3_speed=-2000, m4_speed=-2000, scale = 0.5) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('right',n) #stop motor_move(motors) for n in range(n_acc): aX, aY, gZ = accel_gyro.get_accel('stop',n) sleep_ms(seg_time) file.close()