def init(device): #device number is set using SmcCenter.exe tool (0-6)
log.store('Initializing Motor: ' + str(device))
device_hex = struct.pack(
"B", int(device)) #packs device number into serial value
ser.write('\xAA') #if using auto baud rate, tells controller the speed
ser.write(device_hex) #device command
ser.write('\x03') #0 power on a scale of -127 to 127
def IMU_get_data():
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
try:
floatdata = [float(data[x]) for x in range(0,len(data)) ] #grab the first 3 values (magnetometer)
log.store(floatdata)
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
def IMU_get_data():
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
try:
floatdata = [float(data[x]) for x in range(0, len(data))
] #grab the first 3 values (magnetometer)
log.store(floatdata)
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
def IMU_get_accel(logging):
global data_err
global magdata
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
global acceldata
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x])/1632 for x in [3,4,5]] #grab the second 3 value (Accelerometers)
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
if (logging is True): log.store(acceldata)
return acceldata
def move_cl(axis, distance):
log.store('Move Distance '+str(degrees))
sel_axis = select_axis(axis)[0]
motora = select_axis(axis)[1]
motorb = select_axis(axis)[2]
dist = sensor.IMU_get_theta()[sel_axis]
while(dist<=distance):
mout = (distance-dist)*kp
motor.power(motora, mout, True)
motor.power(motorb, mout, True)
dist = sensor.IMU_get_theta()[axis]
motor.power(motora, 0, True)
motor.power(motorb, 0, True)
def power(device, power, logging): #device: 0-6 power: -100 to 100
if logging is True:
log.store('Motor '+str(device)+' Power = '+ str(power))
ser.write('\xFF') #set miniSSC protocol
device_hex = struct.pack("B", int(device)) #pack int value to hex value
ser.write(device_hex) #set device number
# input scaling
if (power > 100):
power = 100
if(power < -100):
power = -100
power = (power + 100) * 254/200 #shift and scale input to the motor controller range
speed_hex = struct.pack("B", int(power)) #pack int value to hex value
ser.write(speed_hex) #set speed
def rotate_cl(axis, degrees):
log.store('Rotate '+str(degrees)+' along '+str(axis))
sel_axis = select_axis(axis)[0]
motora = select_axis(axis)[1]
motorb = select_axis(axis)[2]
motor.power(motora, 40, True)
motor.power(motorb,-40, True)
theta_0 = sensor.IMU_get_theta()[sel_axis]
theta = sensor.IMU_get_theta()[sel_axis]-theta_0
while theta < degrees:
theta = sensor.IMU_get_theta()[sel_axis]-theta_0
log.store('theta = ' + str(theta))
motor.power(motora, 0, True)
motor.power(motorb, 0, True)
def select_axis(axis):
if (axis is 'y'):
axis1 = 0
motora = 1
motorb = 2
elif (axis is 'x'):
axis1 = 1
motora = 3
motorb = 4
elif (axis is 'z'):
axis1 = 2
motora = 5
motorb = 6
else:
log.store('not an axis')
return axis1,motora, motorb
def IMU_get_theta():
global data_err
global gyrodata
global gyro_old
global theta
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
try:
gyrodata = [float(data[x]) for x in [3,4,5]] #grab the first 3 values (magnetometer)
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
theta = [theta[x] + (gyro_old[x]-gyrodata[x]) for x in range(0,3)]
gyro_old = gyrodata
log.store(theta)
def move_ol(axis, distance):
log.store('Move Distance '+str(degrees))
sel_axis = select_axis(axis)[0]
motora = select_axis(axis)[1]
motorb = select_axis(axis)[2]
total_time = distance/move_time[sel_axis]
start_time = time.time()
current_time = time.time()-start_time
while(current_time<total_time):
motor.power(motora, 100, True)
motor.power(motorb, 100, True)
print '100'
current_time = time.time()-start_time
motor.power(1,0, True)
motor.power(2,0, True)
def IMU_get_accel(logging):
global data_err
global magdata
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
global acceldata
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x]) / 1632 for x in [3, 4, 5]
] #grab the second 3 value (Accelerometers)
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
if (logging is True): log.store(acceldata)
return acceldata
def power(device, power, logging): #device: 0-6 power: -100 to 100
if logging is True:
log.store('Motor ' + str(device) + ' Power = ' + str(power))
ser.write('\xFF') #set miniSSC protocol
device_hex = struct.pack("B", int(device)) #pack int value to hex value
ser.write(device_hex) #set device number
# input scaling
if (power > 100):
power = 100
if (power < -100):
power = -100
power = (
power +
100) * 254 / 200 #shift and scale input to the motor controller range
speed_hex = struct.pack("B", int(power)) #pack int value to hex value
ser.write(speed_hex) #set speed
def rotate_ol(axis, degrees):
log.store('Rotate(ol) '+str(degrees)+' along '+str(axis))
sel_axis = select_axis(axis)[0]
motora = select_axis(axis)[1]
motorb = select_axis(axis)[2]
motor.power(motora, 100, True)
motor.power(motorb,-100, True)
total_time = degrees/rot_time[sel_axis]
start_time = time.time()
current_time = time.time()-start_time
while(current_time<total_time):
motor.power(motora, 100, True)
motor.power(motorb, -100, True)
current_time = time.time()-start_time
motor.power(motora, 0, True)
motor.power(motorb, 0, True)
def IMU_get_position(logging):
global data_err
global accel_total
global accel_old
global accel_delta
global acceldata
global vel
global vel_old
global vel_delta
global position
global time_delta
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x])/1632 for x in [3,4,5]] #grab the second 3 value (Accelerometers)
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
if (logging is True): log.store(acceldata)
'''
accel_delta[0] = accel_old[0]-acceldata[0]
accel_old[0] = acceldata[0]
vel[0] = vel[0] + accel_delta[0]
vel_delta[0] = vel_old[0]-vel[0]
vel_old[0] = vel[0]
position[0] = position[0] + vel_delta[0]
accel_delta = [accel_old[x]-acceldata[x] for x in range(0,3)]
accel_old = acceldata
vel = [vel[x] + accel_delta[x] for x in range(0,3)]
vel_delta = [vel_old[x]-vel[x] for x in range(0,3)]
vel_old = vel
position = [vel[x] + vel_delta[x] for x in range(0,3)]
'''
return position
def pressuretest():
log.store('Starting Pressure Test')
robot.motor.power(3, 100, True)
robot.motor.power(4, 100, True)
init_time = time.time()
current_time = time.time()-init_time
while(current_time<=10):
robot.sensor.IMU_get_pressure()
current_time = time.time()-init_time
init_time = time.time()
current_time = time.time()-init_time
robot.motor.power(3, -50, True)
robot.motor.power(4, -50, True)
while(current_time<=10):
robot.sensor.IMU_get_pressure()
current_time = time.time()-init_time
robot.motor.power(3, 0, True)
robot.motor.power(4, 0, True)
def IMU_get_theta():
global data_err
global gyrodata
global gyro_old
global theta
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
try:
gyrodata = [float(data[x]) for x in [3, 4, 5]
] #grab the first 3 values (magnetometer)
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
theta = [theta[x] + (gyro_old[x] - gyrodata[x]) for x in range(0, 3)]
gyro_old = gyrodata
log.store(theta)
def IMU_get_position(logging):
global data_err
global accel_total
global accel_old
global accel_delta
global acceldata
global vel
global vel_old
global vel_delta
global position
global time_delta
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x]) / 1632 for x in [3, 4, 5]
] #grab the second 3 value (Accelerometers)
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
if (logging is True): log.store(acceldata)
'''
accel_delta[0] = accel_old[0]-acceldata[0]
accel_old[0] = acceldata[0]
vel[0] = vel[0] + accel_delta[0]
vel_delta[0] = vel_old[0]-vel[0]
vel_old[0] = vel[0]
position[0] = position[0] + vel_delta[0]
accel_delta = [accel_old[x]-acceldata[x] for x in range(0,3)]
accel_old = acceldata
vel = [vel[x] + accel_delta[x] for x in range(0,3)]
vel_delta = [vel_old[x]-vel[x] for x in range(0,3)]
vel_old = vel
position = [vel[x] + vel_delta[x] for x in range(0,3)]
'''
return position
def IMU_init(): # sda-->a4 scl-->a5
log.store('initializing IMU')
rawdata = ser2.readline()
#check to see if data coming in is numerical
trooth = True
while trooth:
rawdata = ser2.readline().strip('\r\n').split(':') #reads in data, delimit around ':'
log.store(rawdata[0])
if rawdata[0].strip('-').isdigit(): #check to see if data is numerical
trooth = False
log.store('its a number!')
def IMU_init(): # sda-->a4 scl-->a5
log.store('initializing IMU')
rawdata = ser2.readline()
#check to see if data coming in is numerical
trooth = True
while trooth:
rawdata = ser2.readline().strip('\r\n').split(
':') #reads in data, delimit around ':'
log.store(rawdata[0])
if rawdata[0].strip('-').isdigit(): #check to see if data is numerical
trooth = False
log.store('its a number!')
def IMU_get_pressure():
global data_err
global pressuredata
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
try:
pressuredata = 5./1024.*float(data[9]) #grab the first 3 values (magnetometer)
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
except(IndexError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
log.store('pressure = '+str(pressuredata))
return pressuredata
def IMU_get_pressure():
global data_err
global pressuredata
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
try:
pressuredata = 5. / 1024. * float(
data[9]) #grab the first 3 values (magnetometer)
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
except (IndexError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
log.store('pressure = ' + str(pressuredata))
return pressuredata
def integrate(logging):
global timer
global acceldata
global timer_old
global accel_old
global accel_delta
global vel
global vel_old
global position
# global timer_delta
global data_err
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(
':') #reads in data, delimits around ':'
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x]) / 1632 for x in [6, 7, 8]
] #grab the third 3 value (Accelerometers)
# divide by 1632
except (ValueError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
except (IndexError):
data_err = data_err + 1
log.store('Error Count = ' + str(data_err))
if (logging is True): log.store(acceldata)
timer = time.time()
timer_delta = timer - timer_old
#acceldata[2] =acceldata[2]-9.806
vel = [vel[x] + (acceldata[x] * timer_delta) for x in range(0, 3)]
position = [position[x] + (vel[x] * timer_delta) for x in range(0, 3)]
timer_old = timer
return acceldata, vel, position, timer_delta
def integrate(logging):
global timer
global acceldata
global timer_old
global accel_old
global accel_delta
global vel
global vel_old
global position
# global timer_delta
global data_err
rawdata = ser2.readline()
data = rawdata.strip('\r\n').split(':') #reads in data, delimits around ':'
try:
#1632 is the conversion from miles/sec^2 to meters/sec^2
acceldata = [float(data[x])/1632 for x in [6,7,8]] #grab the third 3 value (Accelerometers)
# divide by 1632
except(ValueError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
except(IndexError):
data_err = data_err+1
log.store('Error Count = '+str(data_err))
if (logging is True): log.store(acceldata)
timer = time.time()
timer_delta = timer-timer_old
#acceldata[2] =acceldata[2]-9.806
vel = [vel[x] + (acceldata[x] * timer_delta) for x in range(0,3)]
position = [position[x] + (vel[x] * timer_delta) for x in range(0,3)]
timer_old = timer
return acceldata, vel, position, timer_delta
def serial_init():
global ser
mcdevice = '/dev/ttyUSB0'
ser = serial.Serial(mcdevice, 115200, timeout=1)
log.store('initializing motor serial device:'+ str(mcdevice))
def init(device): #device number is set using SmcCenter.exe tool (0-6)
log.store('Initializing Motor: '+str(device))
device_hex = struct.pack("B", int(device)) #packs device number into serial value
ser.write('\xAA') #if using auto baud rate, tells controller the speed
ser.write(device_hex) #device command
ser.write('\x03') #0 power on a scale of -127 to 127
def downthrust():
log.store('Starting downthrust test:')
robot.motor.power(3,100,logging)
robot.motor.power(4,100,logging)
robot.time.sleep(5)
accel = robot.sensor.IMU_get_accel(logging)
import log
num_plate__cascade = cv2.CascadeClassifier(
'haarcascade_russian_plate_number.xml')
cap = cv2.VideoCapture('Car.mp4')
time.sleep(2)
k = 0
while 1:
ret, img = cap.read()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
faces = num_plate__cascade.detectMultiScale(gray, 1.3, 5)
for (x, y, w, h) in faces:
cv2.rectangle(img, (x, y), (x + w, y + h), (255, 0, 0), 2)
crp = img[y:y + h, x:x + w]
cv2.imwrite("img4.jpg", crp)
from PIL import Image
import pytesseract
i = Image.open("img4.jpg")
j = pytesseract.image_to_string(i)
print j
log.store(j, k)
k += 1
cv2.imshow('img', img)
k = cv2.waitKey(30) & 0xff
if k == 27:
break
cap.release()
cv2.destroyAllWindows()
def serial_init():
global ser
mcdevice = '/dev/ttyUSB0'
ser = serial.Serial(mcdevice, 115200, timeout=1)
log.store('initializing motor serial device:' + str(mcdevice))
