<commit_before>#!/usr/bin/python
from optparse import OptionParser
import os
import sys
import time
import math
import curses
import csv
import datetime

import motor
import adxl345
import hcsr04
import mpu6050


parser = OptionParser()
parser.add_option("-p", "--p", dest="p", help="")
(options, args) = parser.parse_args()


enable_motor = True
enable_mpu_dmp = False
enable_curse = True
RAD_TO_DEG = 57.3
DEG_TO_RAD = 1 / RAD_TO_DEG
ACCEL_SF = 0.004
base_sleep_time = 0.05


#motor0 : N / motor1 : S
print "Init motors...",
if enable_motor:
	motors = [motor.Motor(0, debug=False).init(), motor.Motor(1, debug=False).init()]
print "Done"

print "Init accels...",
accel = adxl345.ADXL345(bwrate=adxl345.ADXL345.BW_RATE_25HZ, range=adxl345.ADXL345.RANGE_2G)
dist  = hcsr04.HCSR04()
mpu   = mpu6050.MPU6050()
mpu.initialize()
mpu.setRate(39) # 1khz / (1 + 4) = 200 Hz [9 = 100 Hz]
#mpu.dmpInitialize()
#mpu.setDMPEnabled(True)
print "Done"

# Init gyro et angle
print "Init gyro offset...",
gyro_init = {}
for i in ['x', 'y', 'z']: gyro_init[i] = 0
count = 0
max_count = 2 / base_sleep_time
while count < max_count:
	data = mpu.readall()
	#print "gyro_init:[%.2f:%.2f:%.2f]" % (data['gyro_scaled']['x'], data['gyro_scaled']['y'], data['gyro_scaled']['z'])
	for i in ['x', 'y', 'z']: gyro_init[i] += data['gyro_scaled'][i]
	count += 1
	time.sleep(base_sleep_time)
for i in ['x', 'y', 'z']: gyro_init[i] /= count
print "Done. Iterations:[%d] gyro_init:[%.2f:%.2f:%.2f]" % (count, gyro_init['x'], gyro_init['y'], gyro_init['z'])
axis = accel.getAxes()
angle_rad = math.atan(axis['y']/axis['z']) if axis['z'] <> 0 else 0
#sys.exit(0)

try:
	lastt = 0
	previous_error = 0
	integral = 0
	derivative = 0
	diff_speed = 0
	max_speed = 0.20
	error = 0
	dt_ms = 0
	Kp = 0.001
	Ki = 0
	Kd = 0.002
	KpDiff = 0.0005
	KiDiff = 0.00001
	KdDiff = 0.00001
	target_deg = 0
	start_time = time.time()*1000000 # en us
	lines = []
	avg_speed = 3
	speed_percent = avg_speed
	pitch_offset = 0
	if enable_motor:
		print "Setup initial motor speed to:[%.2f]" % float(speed_percent),
		for m in motors:
			m.set_speed(speed_percent/100.0)
		time.sleep(2)
		print "Done"

	if enable_curse:
		stdscr = curses.initscr()
		stdscr.nodelay(1)

	ys = []
	#angle_rad = 0
	axis = {}
	while (True):
		init_time = time.time()
		
		distance = 0 #dist.measure()
		axis = accel.getAxes()
		axis['x'] = axis['x'] - 20;
		axis['y'] = axis['y'] - 20;
		data = mpu.readall()
		gyro = data['gyro_scaled']
		for i in ['x', 'y', 'z']: gyro[i] = gyro[i] - gyro_init[i] # Adjust gyro with initial offset
		mpu_accel = data['accel_scaled']
		mpu_accel['x'] = mpu_accel['x'] - 0.10

		#(axis['x'], axis['y'], axis['z'], fifocount) = mpu.getYPR()<commit_msg>add initial offsets for adxl and mpu<commit_after>#!/usr/bin/python
from optparse import OptionParser
import os
import sys
import time
import math
import curses
import csv
import datetime

import motor
import adxl345
import hcsr04
import mpu6050


parser = OptionParser()
parser.add_option("-p", "--p", dest="p", help="")
(options, args) = parser.parse_args()


enable_motor = True
enable_mpu_dmp = False
enable_curse = True
RAD_TO_DEG = 57.3
DEG_TO_RAD = 1 / RAD_TO_DEG
ACCEL_SF = 0.004
base_sleep_time = 0.05


#motor0 : N / motor1 : S
print "Init motors...",
if enable_motor:
	motors = [motor.Motor(0, debug=False).init(), motor.Motor(1, debug=False).init()]
print "Done"

print "Init accels...",
accel = adxl345.ADXL345(bwrate=adxl345.ADXL345.BW_RATE_25HZ, range=adxl345.ADXL345.RANGE_2G)
accel_init = {'x':20, 'y':20, 'z':0}
dist  = hcsr04.HCSR04()
mpu   = mpu6050.MPU6050()
mpu.initialize()
mpu.setRate(39) # 1khz / (1 + 4) = 200 Hz [9 = 100 Hz]
mpu_init = {'x':0.10, 'y':0, 'z':0}
#mpu.dmpInitialize()
#mpu.setDMPEnabled(True)
print "Done"

# Init gyro et angle
print "Init gyro offset...",
gyro_init = {}
for i in ['x', 'y', 'z']: gyro_init[i] = 0
count = 0
max_count = 2 / base_sleep_time
while count < max_count:
	data = mpu.readall()
	#print "gyro_init:[%.2f:%.2f:%.2f]" % (data['gyro_scaled']['x'], data['gyro_scaled']['y'], data['gyro_scaled']['z'])
	for i in ['x', 'y', 'z']: gyro_init[i] += data['gyro_scaled'][i]
	count += 1
	time.sleep(base_sleep_time)
for i in ['x', 'y', 'z']: gyro_init[i] /= count
print "Done. Iterations:[%d] gyro_init:[%.2f:%.2f:%.2f]" % (count, gyro_init['x'], gyro_init['y'], gyro_init['z'])
axis = accel.getAxes()
angle_rad = math.atan(axis['y']/axis['z']) if axis['z'] <> 0 else 0
#sys.exit(0)

try:
	lastt = 0
	previous_error = 0
	integral = 0
	derivative = 0
	diff_speed = 0
	max_speed = 0.20
	error = 0
	dt_ms = 0
	Kp = 0.001
	Ki = 0
	Kd = 0.002
	KpDiff = 0.0005
	KiDiff = 0.00001
	KdDiff = 0.00001
	target_deg = 0
	start_time = time.time()*1000000 # en us
	lines = []
	avg_speed = 3
	speed_percent = avg_speed
	pitch_offset = 0
	if enable_motor:
		print "Setup initial motor speed to:[%.2f]" % float(speed_percent),
		for m in motors:
			m.set_speed(speed_percent/100.0)
		time.sleep(1)
		print "Done"

	if enable_curse:
		stdscr = curses.initscr()
		stdscr.nodelay(1)

	ys = []
	#angle_rad = 0
	axis = {}
	while (True):
		init_time = time.time()
		
		distance = 0 #dist.measure()
		axis = accel.getAxes()
		for i in ['x', 'y', 'z']: axis[i] -= accel_init[i]
		data = mpu.readall()
		gyro = data['gyro_scaled']
		for i in ['x', 'y', 'z']: gyro[i] -= gyro_init[i] # Adjust gyro with initial offset
		mpu_accel = data['accel_scaled']
		for i in ['x', 'y', 'z']: mpu_accel[i] -= mpu_init[i]

		#(axis['x'], axis['y'], axis['z'], fifocount) = mpu.getYPR()
{'fraction_non_alphanumeric': 0.18287584075622615, 'fraction_numerical': 0.052899472823123066, 'mean_word_length': 4.2250712250712255, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1952439', 'n_tokens_mistral': 2673, 'n_tokens_neox': 2364, 'n_words': 561}