def CheckBattery(new_voltage):
global filtered_voltage
coef = 0.01
filtered_voltage = (1-coef)*filtered_voltage + coef*new_voltage
if filtered_voltage < 9:
stopmotors=[0 for n in range(13)] ; stopmotors[-1]=69
robocaller("AllInOne","void",stopmotors)
print 'Motors stopped'
time.sleep(1)
exit()
def AllInOne(speed, rot_speed, direction):
global messagenumber
message_send = FloatToBytes(speed, rot_speed, direction)
message_send.append(messagenumber)
#print hex(message_send[13])
messagenumber = messagenumber + 1
return robocaller("AllInOne", "int", message_send)
def AllInOne(speed, rot_speed, direction):
global messagenumber
message_send = FloatToBytes(speed, rot_speed, direction)
message_send.append(messagenumber)
#print hex(message_send[13])
messagenumber=messagenumber+1
return robocaller("AllInOne","int",message_send)
def UARTSendandReceive(speed, rot_speed, orientation):
message_send = FloatToBytes(speed, rot_speed, orientation)
message_receive = robocaller("UARTSendandReceive", "int", message_send)
print "Receive", message_receive
if len(message_receive) >= 9:
for n in range(4): # convert the data to int
odom[n] = BytesToInt16(message_receive[2 * n], message_receive[2 * n + 1])
return odom
else:
return "No data"
def UARTSendandReceive(speed, rot_speed, orientation):
message_send = FloatToBytes(speed, rot_speed, orientation)
message_receive = robocaller("UARTSendandReceive", "int", message_send)
print "Receive", message_receive
if len(message_receive) >= 9:
for n in range(4): #convert the data to int
odom[n] = BytesToInt16(message_receive[2 * n],
message_receive[2 * n + 1])
return odom
else:
return 'No data'
def IMUInit():
#IMU -- See http://www.agottem.com/robovero_sensors for all the details
power = 1
Acc_x_on = 1
Acc_y_on = 1
Acc_z_on = 1
Acc_freq = 100
Acc_scale = 2
Mag_bias = 0
Mag_freq = 7500
Mag_scale = 19
Gyro_x_on = 1
Gyro_y_on = 1
Gyro_z_on = 1
Gyro_freq = 100
Gyro_scale = 250
print 'Configuration of the sensors\t\t',
robocaller("configAccel", "void", power, Acc_x_on, Acc_y_on, Acc_z_on,
Acc_freq, Acc_scale)
robocaller("configMag", "void", power, Mag_bias, Mag_freq, Mag_scale)
robocaller("configGyro", "void", power, Gyro_x_on, Gyro_y_on, Gyro_z_on,
Gyro_freq, Gyro_scale)
foo = robocaller("readMag", "int") #useless
#Compute the offset of the gyroscope
Gyro = []
global Gyro_offset
print 'Gyro calibration, please wait!\t\t'
for n in range(400):
gyro_raw = robocaller("readGyro", "int")
gyro_tmp = [((x - 32768.0) / (32768.0 / 250.0)) for x in gyro_raw]
Gyro.append(gyro_tmp)
Gyro_offset = [sum(row[n] for row in Gyro) / 400
for n in range(3)] #compute the mean of each axis
print 'Offset: [%0.3f\t%0.3f\t%0.3f]' % (Gyro_offset[0], Gyro_offset[1],
Gyro_offset[2])
Gyro = []
def IMUInit():
#IMU -- See http://www.agottem.com/robovero_sensors for all the details
power = 1
Acc_x_on = 1
Acc_y_on = 1
Acc_z_on = 1
Acc_freq = 100
Acc_scale = 2
Mag_bias = 0
Mag_freq = 7500
Mag_scale = 19
Gyro_x_on = 1
Gyro_y_on = 1
Gyro_z_on = 1
Gyro_freq = 100
Gyro_scale = 250
print 'Configuration of the sensors\t\t',
robocaller("configAccel","void",power,Acc_x_on,Acc_y_on,Acc_z_on,Acc_freq,Acc_scale)
robocaller("configMag", "void",power,Mag_bias,Mag_freq,Mag_scale)
robocaller("configGyro","void",power,Gyro_x_on,Gyro_y_on,Gyro_z_on,Gyro_freq,Gyro_scale)
foo=robocaller("readMag","int")#useless
#Compute the offset of the gyroscope
Gyro=[]
global Gyro_offset
print 'Gyro calibration, please wait!\t\t'
for n in range(400):
gyro_raw=robocaller("readGyro","int")
gyro_tmp=[((x-32768.0)/(32768.0/250.0)) for x in gyro_raw]
Gyro.append(gyro_tmp)
Gyro_offset = [sum(row[n] for row in Gyro)/400 for n in range(3)]#compute the mean of each axis
print 'Offset: [%0.3f\t%0.3f\t%0.3f]' % (Gyro_offset[0],Gyro_offset[1],Gyro_offset[2])
Gyro=[]
def IMURead():
Accel=robocaller("readAccel","int")
Gyro=robocaller("readGyro","int")
Mag=robocaller("readMag","int")
return IMUConvert(Accel+Gyro+Mag)
def IMURead():
Accel = robocaller("readAccel", "int")
Gyro = robocaller("readGyro", "int")
Mag = robocaller("readMag", "int")
return IMUConvert(Accel + Gyro + Mag)
