def measure(self, N): X = 0 Y = 0 for i in range(N): X+=IMU.readACCy() Y+=IMU.readACCx() return [(X/N)-self.zeroX,(Y/N)-self.zeroY]
def animate(Q): r_x = IMU.readGYRx() r_y = IMU.readGYRy() r_z = IMU.readGYRz() r_x_matrix = np.array( [[1, 0, 0], [0, np.cos(np.rad2deg(r_x)), -np.sin(np.rad2deg(r_x))], [0, np.sin(np.rad2deg(r_x)), np.cos(np.rad2deg(r_x))]]) r_y_matrix = np.array( [[np.cos(np.rad2deg(r_y)), 0, np.sin(np.rad2deg(r_y))], [0, 1, 0], [-np.sin(np.rad2deg(r_y)), 0, np.cos(np.rad2deg(r_y))]]) r_z_matrix = np.array( [[np.cos(np.rad2deg(r_z)), -np.sin(np.rad2deg(r_z)), 0], [np.sin(np.rad2deg(r_z)), np.cos(np.rad2deg(r_z)), 0], [0, 0, 1]]) r_xy = np.matmul(r_x_matrix, r_y_matrix) r_xyz = np.matmul(r_xy, r_z_matrix) e_x = np.matmul(r_xyz, [1, 0, 0]) e_y = np.matmul(r_xyz, [0, 1, 0]) e_z = np.matmul(r_xyz, [0, 0, 1]) print("X: %i, Y: %i , Z: %i" % (r_x, r_y, r_z)) ax.clear() return ax.quiver(0, 0, 0, e_x, e_y, e_z, length=0.1, normalize=True)
def reInit(self): IMU.detectIMU() IMU.initIMU() self.RAD_TO_DEG = 57.29578 self.M_PI = 3.14159265358979323846 self.G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly self.AA = 0.40 # Complementary filter constant self.MAG_LPF_FACTOR = 0.4 # Low pass filter constant magnetometer self.ACC_LPF_FACTOR = 0.4 # Low pass filter constant for accelerometer self.ACC_MEDIANTABLESIZE = 9 # Median filter table size for accelerometer. Higher = smoother but a longer delay self.MAG_MEDIANTABLESIZE = 9 # Median filter table size for magnetometer. Higher = smoother but a longer delay #Kalman filter variables self.Q_angle = 0.02 self.Q_gyro = 0.0015 self.R_angle = 0.005 self.y_bias = 0.0 self.x_bias = 0.0 self.XP_00 = 0.0 self.XP_01 = 0.0 self.XP_10 = 0.0 self.XP_11 = 0.0 self.YP_00 = 0.0 self.YP_01 = 0.0 self.YP_10 = 0.0 self.YP_11 = 0.0 self.KFangleX = 0.0 self.KFangleY = 0.0
def __init__(self, interval=1): self.RAD_TO_DEG = 57.29578 self.M_PI = 3.14159265358979323846 self.G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly self.AA = 0.40 # Complementary filter constant self.magXmin = -76 self.magYmin = -766 self.magZmin = -508 self.magXmax = 1917 self.magYmax = 1414 self.magZmax = 1330 self.gyroXangle = 0.0 self.gyroYangle = 0.0 self.gyroZangle = 0.0 self.CFangleX = 0.0 self.CFangleY = 0.0 IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() #Initialise the accelerometer, gyroscope and compass self.atimenow = datetime.datetime.now() self.TOTAL_SAMPLES_TO_AVERAGE = 40.0 self.average_heading = 0.0 self.values_heading = [] thread = threading.Thread(target=self.run, args=()) thread.daemon = True # Daemonize thread thread.start()
def writeaccel(): global address_prefix,accelfile,accfile_lines_uploaded,accelfile_limit while True: if ( accelfile_lines >= accfile_limit): ltime=str(time.asctime(time.localtime(time.time()))) accelfile = 'accel/'+ltime+ 'accel.txt' accelfile_lines = 0 local = time.asctime(time.localtime(time.time())) # string = address_prefix+'accel.txt' #acc='/accel/acc%s'%local fa=open(address_prefix+accelfile,"a+") #collecting the value of accelerometer every .5 sec So 60 values in 30sec ACCx=IMU.readACCx() ACCy=IMU.readACCy() ACCz=IMU.readACCz() x=((ACCx*0.244)/100) y=((ACCy*0.244)/100) z=((ACCz*0.244)/100) print(("X = %f\t"%x),) print(("Y = %f\t"%y),c) print(("Z = %f\t"%z)) #ts=time.asctime(time.localtime(time.time())) coordinates = "X = %f\tY = %f\tZ=%f"%(x,y,z) time_str = str(local) s=("%s\t,\t%s\n"%(time_str,coordinates)) fa.write(s) fa.close() time.sleep(0.5)
def __init__(self): GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) self.GPIO_4 = 4 GPIO.setup(self.GPIO_4, GPIO.OUT) GPIO.output(self.GPIO_4, False) self.imu = IMU() self.servo = Servo() self.move_flag = 0x01 self.relax_flag = False self.pid = Incremental_PID(0.500, 0.00, 0.0025) self.flag = 0x00 self.timeout = 0 self.height = -25 self.body_point = [[137.1, 189.4, self.height], [225, 0, self.height], [137.1, -189.4, self.height], [-137.1, -189.4, self.height], [-225, 0, self.height], [-137.1, 189.4, self.height]] self.calibration_leg_point = self.readFromTxt('point') self.leg_point = [[140, 0, 0], [140, 0, 0], [140, 0, 0], [140, 0, 0], [140, 0, 0], [140, 0, 0]] self.calibration_angle = [[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]] self.angle = [[90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0], [90, 0, 0]] self.order = ['', '', '', '', '', ''] self.calibration() self.setLegAngle() self.Thread_conditiona = threading.Thread(target=self.condition)
def setup(): global PRINT, ID parser = argparse.ArgumentParser(description='data collection stuff') parser.add_argument('--print', type=int, default=0) parser.add_argument('--player', type=int, default=0) args = parser.parse_args() if args.print == 1: PRINT = 1 if not (args.player == 1 or args.player == 2): print("Please input \"--player 1\" OR \"--player 2") exit() ID = args.player IMU.detectIMU() #Detect if BerryIMU is connected. if (IMU.BerryIMUversion == 99): print(" No BerryIMU found... exiting ") sys.exit() IMU.initIMU() #Initialise the accelerometer, gyroscope and compass while len(_accX) < windowSize: ax, ay, az = _accel((IMU.readACCx(), IMU.readACCy(), IMU.readACCz())) gx, gy, gz = _gyro((IMU.readGYRx(), IMU.readGYRy(), IMU.readGYRz())) _accX.append(ax) _accY.append(ay) _accZ.append(az) _gyrX.append(gx) _gyrY.append(gy) _gyrZ.append(gz)
def calibrate(IMU): magXmin = 32767 magYmin = 32767 magZmin = 32767 magXmax = -32767 magYmax = -32767 magZmax = -32767 for i in range(1000): #Read magnetometer values MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() MAGz = IMU.readMAGz() if MAGx > magXmax: magXmax = MAGx if MAGy > magYmax: magYmax = MAGy if MAGz > magZmax: magZmax = MAGz if MAGx < magXmin: magXmin = MAGx if MAGy < magYmin: magYmin = MAGy if MAGz < magZmin: magZmin = MAGz return magXmin, magXmax, magYmin, magYmax, magZmin, magZmax
def initIMU(): IMU.detectIMU() IMU.initIMU() f = open("accel.csv", "w+") # delete and create a new file f.write("time," + "ACCx" + "," + "ACCy" + "," + "ACCz" + "," + "ACCnorm" + "\n") f.close() return 1
def readAcc(): ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() x = ACCx * 0.000833333 y = ACCy * 0.000833333 z = ACCz * 0.000833333 return x, y, z
def accel(): while True: #print(count) print(1) local = time.asctime(time.localtime(time.time())) string = '/home/pi/Documents/LOG/accel.txt' #acc='/accel/acc%s'%local fa = open(string, "a+") #collecting the value of accelerometer every .5 sec So 60 values in 30sec ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() x = ((ACCx * 0.244) / 100) y = ((ACCy * 0.244) / 100) z = ((ACCz * 0.244) / 100) print(("X = %f\t" % x), end=' ') print(("Y = %f\t" % y), end=' ') print(("Z = %f\t" % z)) #ts=time.asctime(time.localtime(time.time())) str2 = "X = %f\tY = %f\tZ=%f" % (x, y, z) str1 = str(local) s = ("%s\t\t%s\n" % (str2, str1)) fa.write("%s\t\t%s\n" % (str2, str1)) fa.close() time.sleep(0.5) print('Checking Internet') #writing data to file and sending it on firebase result = internet_on() if result: print('Internet is on') if os.path.exists("/home/pi/Documents/LOG/dataofaccel.txt"): print('It exists') f_noc = open("/home/pi/Documents/LOG/dataofaccel.txt", "a+") f_noc.seek(0, 0) lines = f_noc.readlines() if lines != '': for x in lines: action_thread = Thread(target=do_actions, args=( '\AccFile', x, )) action_thread.start() os.remove("/home/pi/Documents/LOG/dataofaccel.txt") s = ("%s\t\t%s\n" % (str2, str1)) action_thread_4 = Thread(target=do_actions, args=( '\AccFile', s, )) action_thread_4.start() else: print('Internet is not there') f_noc = open("/home/pi/Documents/LOG/dataofaccel.txt", "a+") f_noc.write(s) f_noc.close() time.sleep(2)
def __init__(self): if WORK_MODE: # online_mode self.imu_feeder = IMU.vn200_online_feeder(IMU_PKL) time.sleep(0.5) self.lidar_feeder = LIDAR.vlp16_online_feeder(LIDAR_PKL) else: # offline mode self.imu_feeder = IMU.vn200_offline_feeder(IMU_PKL) time.sleep(0.5) self.lidar_feeder = LIDAR.vlp16_offline_feeder(LIDAR_PKL)
def __init__(self,classifier : GestClassifier , window_length: int, overlap :int, sample_freq: int, username :str, debug = False): # debug status self.debug = debug # sampling paramaters self.window_length = window_length #number of Samples self.length_sample = 14 self.classifier = classifier self.data = np.array([0.0]*self.length_sample*window_length) self.reading = np.array([0.0]*self.length_sample*(window_length-overlap)) self.overlap = overlap self.samples_taken = 0 self.sample_freq = sample_freq self.sample_period = 1/sample_freq # Setup Server Link self.board = "ece180d/MEAT/general/gesture" self.user = "******" + username self.mqtt_server = mqtt.MQTTLink( self.board, self.user) self.designated_reciever = username self.last_classification = "" self.last_classification_time = datetime.datetime.now() #initialize sensor IMU.detectIMU() if(IMU.BerryIMUversion == 99): print(" No BerryIMU found...sick nasty") sys.exit() IMU.initIMU() # initialize all the relevant sensors # Sensor Reading Values self.gyroXangle = 0.0 self.gyroYangle = 0.0 self.gyroZangle = 0.0 self.CFangleX = 0.0 self.CFangleY = 0.0 self.CFangleXFiltered = 0.0 self.CFangleYFiltered = 0.0 self.kalmanX = 0.0 self.kalmanY = 0.0 self.oldXAccRawValue = 0 self.oldYAccRawValue = 0 self.oldZAccRawValue = 0 #Setup the tables for the median filter. Fill them all with '1' so we dont get devide by zero error self.acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE #timers self.a = datetime.datetime.now() self.b = datetime.datetime.now() self.c = datetime.datetime.now()
def getIMUmeasureG(): ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() Gx = (ACCx * 0.244) / 1000 Gy = (ACCy * 0.244) / 1000 Gz = (ACCz * 0.244) / 1000 return f"{Gx},{Gy},{Gz}"
def new(): # https://stackoverflow.com/a/735978/3339274 time = (datetime.datetime.now() - START_TIME).total_seconds() acc = [ IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF ] # Compensate for gravitational acceleration. # TODO: Do we want to do some angle calculations to figure out how much to subtract from x, y, and z? acc[0] += 9.81 gyro = [ IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF ] # TODO: Conversion needed? mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] baro_tuple = barometer.get_baro_values(i2c_bus) baro_p = baro_tuple[0] baro_temp = baro_tuple[1] return Timestep(time=time, acc=acc, gyro=gyro, mag=mag, baro_p=baro_p, baro_temp=baro_temp)
def height_control(): global man_under_thrust try: print('Height/Pitch Control') x_rot, y_rot = IMU.get_rotations() p = nodeRead.read_serial_data() print('Pressure String: ', p) p = int(p) offset = 440 p_UW = p - offset max_thrust = 400 desired_depth = 33 thrust_per_unit = max_thrust/desired_depth print('pressure: ', p) under_thrust = (desired_depth - p_UW)*thrust_per_unit if under_thrust >= 200: under_thrust = 200 print('under_thrust = ', under_thrust) rot_thrust = get_rot_thrust(x_rot) if rot_thrust >= 100: rot_thrust = 100 if rot_thrust <= -100: rot_thrust = -100 print('rot_thrust = ', rot_thrust) forward_thrust = int(1500 - under_thrust + rot_thrust) backward_thrust = int(1500 - under_thrust - rot_thrust) print(forward_thrust, backward_thrust) pi.set_servo_pulsewidth(pin_f, forward_thrust) pi.set_servo_pulsewidth(pin_b, backward_thrust) except: print('Exception') x_rot, y_rot = IMU.get_rotations() under_thrust = man_under_thrust print('under_thrust :', under_thrust) rot_thrust = get_rot_thrust(x_rot) if rot_thrust >= 100: rot_thrust = 100 if rot_thrust <= -100: rot_thrust = -100 print('rot_thrust = ', rot_thrust) forward_thrust = int(1500 - under_thrust + rot_thrust) backward_thrust = int(1500 - under_thrust - rot_thrust) print(forward_thrust, backward_thrust) pi.set_servo_pulsewidth(pin_f, forward_thrust) pi.set_servo_pulsewidth(pin_b, backward_thrust)
def __init__(self): self.gyroXangle = 0.0 self.gyroYangle = 0.0 self.gyroZangle = 0.0 self.CFangleX = 0.0 self.CFangleY = 0.0 IMU.detectIMU() # Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() # Initialise the accelerometer, gyroscope and compass self.lastReadTime = datetime.datetime.now()
def getSimpleHeading(self): # Get only the Heading MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() # Calculate heading heading = 180 * math.atan2(MAGy, MAGx) / M_PI # Only have our heading between 0 and 360 if heading < 0: heading += 360 return heading
def record(message): msg = message.payload.decode() lst = msg.replace("[", "").replace("]", "").replace("'", "") lst = lst.split(', ') action = lst[0] sensors = lst[1:] if action == "start": if 'IMU_acc' in sensors: IMU.record('IMU_acc') if 'IMU_vel' in sensors: IMU.record('IMU_vel') if 'Razor_acc' in sensors: Razor_IMU.record('Razor_acc') if 'Razor_vel' in sensors: Razor_IMU.record('Razor_vel') if 'Load_Cell' in sensors: LoadCell.record() if 'CAN' in sensors: CAN.record() elif action == "stop": if 'IMU_acc' in sensors: IMU.record_stop('IMU_acc') if 'IMU_vel' in sensors: IMU.record_stop('IMU_vel') if 'Razor_acc' in sensors: Razor_IMU.record_stop('Razor_acc') if 'Razor_vel' in sensors: Razor_IMU.record_stop('Razor_vel') if 'Load_Cell' in sensors: LoadCell.record_stop() if 'CAN' in sensors: CAN.record_stop() CSV.unifile(sensors)
def record(sid, data): action = data[0] sensors = data[1] if action == "start": if 'IMU_acc' in sensors: IMU.record('IMU_acc') if 'IMU_vel' in sensors: IMU.record('IMU_vel') if 'Razor_acc' in sensors: Razor_IMU.record('Razor_acc') if 'Razor_vel' in sensors: Razor_IMU.record('Razor_vel') if 'Load_Cell' in sensors: LoadCell.record() if 'CAN' in sensors: CAN.record() elif action == "stop": if 'IMU_acc' in sensors: IMU.record_stop('IMU_acc') if 'IMU_vel' in sensors: IMU.record_stop('IMU_vel') if 'Razor_acc' in sensors: Razor_IMU.record_stop('Razor_acc') if 'Razor_vel' in sensors: Razor_IMU.record_stop('Razor_vel') if 'Load_Cell' in sensors: LoadCell.record_stop() if 'CAN' in sensors: CAN.record_stop() CSV.unifile(sensors)
def readIMU(): # take in IMU data and analyze it somehow print("reading IMU") ACCx = (IMU.readACCx() * 0.244) / 1000 # math for converting raw data to g's ACCy = (IMU.readACCy() * 0.244) / 1000 ACCz = (IMU.readACCz() * 0.244) / 1000 ACC_norm = math.sqrt( math.pow(ACCx, 2) + math.pow(ACCy, 2) + math.pow(ACCz, 2)) t_stamp = (datetime.datetime.now() - i_time).microseconds / (1000000 * 1.0) f = open("accel.csv", "a+") # append to file f.write( str((t_stamp)) + "," + str(ACCx) + "," + str(ACCy) + "," + str(ACCz) + "," + str(ACC_norm) + "\n") f.close() return (ACCx, ACCy, ACCz, ACC_norm)
def read_data(self): ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() GYRx = IMU.readGYRx() GYRy = IMU.readGYRy() GYRz = IMU.readGYRz() MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() MAGz = IMU.readMAGz() return np.array([ACCx, ACCy, ACCz, GYRx, GYRy, GYRz, MAGx, MAGy, MAGz], dtype=float)
def __init__(self): rospy.init_node('IO_Interface', anonymous=False) # pub = rospy.Publisher('IO_Interface/', String, queue_size=10) #create motors self.motor_1 = Motors.Motor(motorID='t1', pin = 1, pwm_output = 0) self.motor_2 = Motors.Motor(motorID='t2', pin = 2, pwm_output = 0) self.motor_3 = Motors.Motor(motorID='t3', pin = 3, pwm_output = 0) self.motor_4 = Motors.Motor(motorID='t4', pin = 4, pwm_output = 0) #create inu sensors self.mpu = IMU.Inertia_Measurement_Unit('mpu') self.lsm = IMU.Inertia_Measurement_Unit('lsm') #create led output self.led = Led.Navio2_LED()
def set_motor_powers(): for i in range(len(motor_powers)): motor_powers[i] = 0 for i in range(len(motor_powers)): motor_powers[i] += acc_powers[i] motor_powers[i] += imu_powers[i] motor_powers[i] += pressure_powers[i] motor_powers[i] += move_powers[i] max_pow = 0 for i in range(len(motor_powers)): if abs(motor_powers[i]) > max_pow: max_pow = abs(motor_powers[i]) if abs(max_pow) > 100: for i in range(len(motor_powers)): motor_powers[i] = MotorMovement.remap(motor_powers[i], -abs(max_pow), abs(max_pow), -100, 100) motor_powers[3] = motor_powers[3] * MotorMovement.reverse MotorMovement.targets = motor_powers motors.__next__() if not IMU.check_calibration(): MotorMovement.GPIO.output(40, MotorMovement.GPIO.LOW)
def roll_control(): global A_motor_velocity,B_motor_velocity,C_motor_velocity,D_motor_velocity t_a=time.time() [ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz] = IMU.read() A_motor_velocity = P*GYRz + A_motor_velocity B_motor_velocity = P*GYRz + B_motor_velocity C_motor_velocity = P*GYRz + C_motor_velocity D_motor_velocity = P*GYRz + D_motor_velocity A_motor_dir() B_motor_dir() C_motor_dir() D_motor_dir() Apwm.write(A_motor_speed) Bpwm.write(B_motor_speed) Cpwm.write(C_motor_speed) Dpwm.write(D_motor_speed) t = time.time() - timestart Data.write('%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f,%5.3f\n' % (t,ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz,A_motor_speed)) print '2',ACCz return ACCz if ACCz <= -2: Adir.write(0) Bdir.write(0) Cdir.write(0) Ddir.write(0) Apwm.write(0) Bpwm.write(0) Cpwm.write(0) Dpwm.write(0) sys.exit()
def __init__(self): self.acc_medianTable1X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable1Z = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2X = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Y = [1] * ACC_MEDIANTABLESIZE self.acc_medianTable2Z = [1] * ACC_MEDIANTABLESIZE self.oldXAccRawValue = 0 self.oldYAccRawValue = 0 self.oldZAccRawValue = 0 IMU.detectIMU() #Detect if BerryIMU is connected. if (IMU.BerryIMUversion == 99): raise Exception("No BerryIMU found") IMU.initIMU() #Initialise the accelerometer, gyroscope and compass
def __init__(self, flight_state, time=None, acc=None, gyro=None, mag=None, baro=None): self.flight_state = flight_state if (time == None): if (IMUData.start_time == None): IMUData.start_time = datetime.datetime.now() self.time = (datetime.datetime.now() - IMUData.start_time).total_seconds() else: self.time = time if (acc == None): self.acc = [ IMU.readACCx() * IMU_ACC_COEFF, -IMU.readACCy() * IMU_ACC_COEFF, -IMU.readACCz() * IMU_ACC_COEFF ] # Compensate for gravitational acceleration. # TODO: Do we want to do some angle calculations to figure out how much to subtract from x, y, and z? self.acc[0] += 9.81 else: self.acc = acc if (gyro == None): self.gyro = [ IMU.readGYRx() * IMU_GYRO_COEFF, IMU.readGYRy() * IMU_GYRO_COEFF, IMU.readGYRz() * IMU_GYRO_COEFF ] else: self.gyro = gyro if (mag == None): # TODO: Conversion needed? self.mag = [IMU.readMAGx(), IMU.readMAGy(), IMU.readMAGz()] else: self.mag = mag if (baro == None): tuple = barometer.get_baro_values(i2c_bus) self.baro = [tuple[0], tuple[1]] else: self.baro = baro # Start events list off as empty. self.events = []
def read_sensor(): #Read the accelerometer,gyroscope and magnetometer values GYRx = IMU.readGYRx() GYRy = IMU.readGYRy() GYRz = IMU.readGYRz() ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() MAGx = IMU.readMAGx() MAGy = IMU.readMAGy() MAGz = IMU.readMAGz() return [ACCx, ACCy, ACCz, GYRx, GYRy, GYRz, MAGx, MAGy, MAGz]
def get_axes(): """ Get Axes Values """ return { 'ACCx': IMU.readACCx() * ACC_LSB, 'ACCy': IMU.readACCy() * ACC_LSB, 'ACCz': IMU.readACCz() * ACC_LSB, 'GYRx': IMU.readGYRx() * GYRO_GAIN, 'GYRy': IMU.readGYRy() * GYRO_GAIN, 'GYRz': IMU.readGYRz() * GYRO_GAIN, 'MAGx': IMU.readMAGx(), 'MAGy': IMU.readMAGy(), 'MAGz': IMU.readMAGz(), }
def gyroread(): c = 0 a = time.time() while c <= timer: [ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz] = IMU.read() print "GYRx: %3.2f, GYRy: %3.2f,GYRz: %3.2f" %(GYRx,GYRy,GYRz) b = time.time() c = b - a
def get_movement(a=a): #Read the accelerometer ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz() ##Calculate loop Period(LP). How long between Gyro Reads b = datetime.datetime.now() - a a = datetime.datetime.now() LP = b.microseconds / (1000000 * 1.0) # outputString = "Loop Time %5.2f " % ( LP ) accnorm = math.sqrt(ACCx * ACCx + ACCy * ACCy + ACCz * ACCz) # outputString += " Acceleration: "+str(accnorm) if abs(accnorm - 8500) >= 1000: return True return False
def gyroread(): c = 0 a = time.time() while c <= timer: [ACCx, ACCy, ACCz, GYRx, GYRy, GYRz, MAGx, MAGy, MAGz] = IMU.read() print "GYRx: %3.2f, GYRy: %3.2f,GYRz: %3.2f" % (GYRx, GYRy, GYRz) b = time.time() c = b - a
def roll_control(): t_a=time.time() [ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz] = IMU.read() A_motor_velocity = P*GYRz + A_motor_velocity B_motor_velocity = P*GYRz + B_motor_velocity C_motor_velocity = P*GYRz + C_motor_velocity D_motor_velocity = P*GYRz + D_motor_velocity A_motor_dir() B_motor_dir() C_motor_dir() D_motor_dir() Apwm.write(A_motor_speed) Bpwm.write(B_motor_speed) Cpwm.write(C_motor_speed) Dpwm.write(D_motor_speed) return [GYRz,A_motor_velocity,B_motor_velocity,C_motor_velocity,D_motor_velocity]
def roll_control(in_x_window,out_x_window,in_y_window,out_y_window,in_z_window,out_z_window): t_a=time.time() global A_motor_velocity,B_motor_velocity,C_motor_velocity,D_motor_velocity global ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz [ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz] = IMU.read() GYRz, out_z_window, in_z_window = floating_array_filter(b,a,in_z_window,out_z_window,GYRz) A_motor_velocity = P*GYRz + A_motor_velocity B_motor_velocity = P*GYRz + B_motor_velocity C_motor_velocity = P*GYRz + C_motor_velocity D_motor_velocity = P*GYRz + D_motor_velocity A_motor_dir() B_motor_dir() C_motor_dir() D_motor_dir() Apwm.write(A_motor_speed) Bpwm.write(B_motor_speed) Cpwm.write(C_motor_speed) Dpwm.write(D_motor_speed)
magZmax = 708 Dont use the above values, these are just an example. ''' gyroXangle = 0.0 gyroYangle = 0.0 gyroZangle = 0.0 CFangleX = 0.0 CFangleY = 0.0 IMU.detectIMU() #Detect if BerryIMUv1 or BerryIMUv2 is connected. IMU.initIMU() #Initialise the accelerometer, gyroscope and compass a = datetime.datetime.now() while True: #Read the accelerometer,gyroscope and magnetometer values ACCx = IMU.readACCx() ACCy = IMU.readACCy() ACCz = IMU.readACCz()
#import smbus import time import math import sys import IMU RAD_TO_DEG = 57.29578 M_PI = 3.14159265358979323846 while True: a = time.time() [ACCx,ACCy,ACCz,GYRx,GYRy,GYRz,MAGx,MAGy,MAGz] = IMU.read() AccXangle = (math.atan2(ACCy,ACCz)+M_PI)*RAD_TO_DEG AccYangle = (math.atan2(ACCz,ACCx)+M_PI/2)*RAD_TO_DEG #Calculate heading heading = 180 * math.atan2(MAGy,MAGx)/M_PI if heading < 0: heading += 360 b = time.time() LoopTime = b - a # print ("\033[1;34;40mAcceleration Values:") # print ("\033[1;34;40mACCX %5.4f, ACCy %5.4f, ACCz %5.4f" % (ACCx, ACCy, ACCz)) # print ("\033[1;31;40mGyro Values:") # print ("\033[1;31;40mGYRx %5.2f, GYRy %5.2f, GYRz %5.2f" % (GYRx, GYRy, GYRz)) # print ("\033[1;35;40mMagnetometer values:") # print ("\033[1;35;40mMAGx %5.2f, MAGy %5.2f, MAGz %5.2f" % (MAGx, MAGy, MAGz)) print ("\033[1;34;40mAcceleration angles:") print ("\033[1;34;40m AccXangle= %5.2f, AccYangle= %5.2f" % (AccXangle,AccYangle))
motorBlue.forward(10) sleep(.25) motorBlue.stop() motorGreen.forward(10) sleep(.25) motorGreen.stop() motorYellow.forward(10) sleep(.25) motorYellow.stop() sleep(5) imu = IMU(1) imu.zero() motorBlue.forward(10) sleep(.25) motorBlue.stop() motorGreen.forward(10) sleep(.25) motorGreen.stop() motorYellow.forward(10) sleep(.25) motorYellow.stop() sleep(5)
from termcolor import colored from time import sleep import math from IMU import * from Omnibot import * from PID import * imu = IMU(1) zeroAngle = math.radians(103) twitch = Omnibot() def getDriveAngle (x, y, zeroAngle): fallingAngle = math.atan2(x, y) if fallingAngle < 0: fallingAngle = fallingAngle + 2 * math.pi drivingAngle = fallingAngle + math.pi + zeroAngle if drivingAngle > 2 * math.pi: drivingAngle = drivingAngle - 2 * math.pi return drivingAngle def getDriveSpeed (x, y): drivingSpeed = 200 * (x ** 2 + y ** 2) ** (.5) return drivingSpeed pitch = [] roll = [] pitchPIDController = PIDController() rollPIDController = PIDController() count = 0 while True:
import datetime def handle_ctrl_c(signal, frame): print " " print "magXmin = ", magXmin print "magYmin = ", magYmin print "magZmin = ", magZmin print "magXmax = ", magXmax print "magYmax = ", magYmax print "magZmax = ", magZmax sys.exit(130) # 130 is standard exit code for ctrl-c IMU.detectIMU() IMU.initIMU() #This will capture exit when using Ctrl-C signal.signal(signal.SIGINT, handle_ctrl_c) a = datetime.datetime.now() #Preload the variables used to keep track of the minimum and maximum values magXmin = 32767 magYmin = 32767 magZmin = 32767 magXmax = -32767 magYmax = -32767