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
