class Thread_COM(Thread):
def __init__(self, sleep_time, debug=False):
super(Thread_COM, self).__init__()
print "COM Init"
self.arduino = COMwithArduino(0x12, 10, debug=False)
self.transmission_time = 0.005
self.sleep_time = sleep_time - self.transmission_time
self.debug = debug
#Logging init
self.log = SaveData("./images/test/log")
COM_header = ["Battery", "Luminosity", "Remote Speed", "Remote Steering", "Remote Switch", "Motor Speed", "Steering", "Motor State"]
Motor_header = ["Motor speed", "Steering"]
#IMU_header = ["IMU Roll", "IMU Pitch", "IMU Yaw", "Gyro Roll", "Gyro Pitch", "Gyro Yaw", "Acc X", "Acc Y", "Acc Z"]
#GPS_header = ["GPS Longitude", "GPS Lattitude", "GPS altitude"]
Camera_header = ["Camera X", "Camera Y"]
header = ["Time"]
header.extend(COM_header)
header.extend(Motor_header)
#header.extend(IMU_header)
#header.extend(GPS_header)
header.extend(Camera_header)
self.log.write(header)
def run(self):
while not Done:
global COM_message, Mode
COM_message = self.arduino.Read() #Receive data from Arduino
if COM_message == False:
continue #skip the rest of this while loop and goes back to testing the expression
#if self.debug:
#print "COM: ", COM_message
battery_voltage = COM_message[0]
remote_speed = COM_message[2]-100
remote_steering = COM_message[3]-100
Mode = COM_message[4] #Mode = switch value
if battery_voltage > 70:
if Mode == MODE_REMOTE: #if Mode = 0, no processing, data are only logged
motor_speed = remote_speed
motor_steering = remote_steering
elif Mode == MODE_CAMERA: #Autonomous mode with camera
if BestBlob:
motor_steering = int( (BestBlob[0]-Camera_target[0]) ) #image width
motor_speed = int( (Camera_target[1] - BestBlob[1])/2 ) #image height
else:
motor_speed = 0
motor_steering = 0
else: #if Mode = 2, stop the car
motor_speed = 0
motor_steering = 0
else: # Low battery : stop the car
motor_speed = 0
motor_steering = 0
message2Arduino = [motor_speed+100, motor_steering+100, 0]
if self.debug:
print' BestBlob: ',BestBlob[0],' \t', motor_steering
self.arduino.Send(message2Arduino) #Send data to Arduino
#Log data
data = []
data.extend([time.time()])
data.extend(COM_message)
data.extend(message2Arduino)
data.extend(BestBlob)
#data.extend(IMU_position)
#data.extend(IMU_gyro)
#data.extend(IMU_accel)
#data.extend(GPS_position)
#data.extend(im_proc)
# if self.debug:
# print "Logging: ", data
self.log.write(data)
time.sleep(self.sleep_time)
self.log.close()
if self.debug:
print "Thread COM exiting"
