def detectAndDisply(img,cascade,mode='human'):
max_size = -1
index = 0
if mode == 'human':
detector = cascade.detectMultiScale(img)
if(len(detector) != 0):
for (x,y,w,h) in detector:
if w*h > max_size:
max_size = w*h
max_pos = index
index += 1
max_rect = detector[max_pos]
(max_x,max_y,max_w,max_h) = detector[max_pos]
cv2.rectangle(img,(max_x,max_y),(max_x + max_w,max_y + max_h),(0,255,0),2)
move(detector[max_pos])
elif mode == 'qr':
barcodes = pyzbar.decode(img)
if(len(barcodes) != 0):
for barcode in barcodes:
(x,y,w,h) = barcode.rect
cv2.rectangle(img,(x,y),(x+w,y+h),(0,255,0),2)
move((x,y,w,h))
else:
m.stop()
def quit():
motor.stop()
state = device_state.loads()
state['motor_running'] = False
# hue.set_light(False, bri=0)
calculated_bri.reset()
return render_json(state)
def init(): # モータ停止 motor.stop() sound.STARTUP() led.STARTUP() # 顔を正面に向ける servo.drive_servo(servo.FRONT)
def main(): wm=wimote.connect(Led) while True: button=wimote.whichbutton(wm) time.sleep(0.05) wm.rumble=False #Moving Forwards if button==3: distance_front = usonic.reading(Trigger_front,Echo_front) print distance_front if distance_front < Collision: wm.rumble=True #led.off(Led) motor.stop() elif distance_front >= Collision: #led.on(Led) motor.forward() #Reverse if button==4: motor.reverse() if button==7: motor.cleanup() led.cleanup() usonic.cleanup() sys.exit() if button==None: led.off(Led) motor.stop()
def go_Forward(speed, running_time):
global forward0, forward1
motor.setSpeed(speed)
motor.motor0(forward0)
motor.motor1(forward1)
time.sleep(running_time)
motor.stop()
def run(self):
while not self.stopped:
if objectDetected():
print("Object detected")
sleep(3)
stop()
run(self.updateImageSignal, self.mode, self.setLabelSignal)
def go_Backward(speed, running_time):
global backward0, backward1
motor.setSpeed(speed)
motor.motor0(backward0)
motor.motor1(backward1)
time.sleep(running_time)
motor.stop()
def right_forward():
turn.home()
print("turn right")
turn.turn_right()
motor.forwardWithSpeed(spd=100)
time.sleep(1)
motor.stop()
turn.home()
def set_actuators_idle(self):
self.actuators['throttle'].get_value_out(0) #- positive fwd
self.actuators['steering'].get_value_out(0) #-positve right
rospy.loginfo('Setting Actuators to idle')
#-- Puplish the message using a function
self.send_servo_msg()
motor.stop()
def left_forward():
print("turn left")
turn.home()
turn.turn_left()
motor.forwardWithSpeed(spd=100)
time.sleep(1)
motor.stop()
turn.home()
def stopthread(state):
# print state.moving
while not state.quit :
#print "time: ", time.time(), " moved:", movetime
# print moving
if time.time() > state.movetime + 0.3 and state.moving :
motor.stop()
state.moving = False
time.sleep(0.1)
def rotate(angle):
stderr.write("rotate by " + str(angle) + '\n')
if angle > 0:
motor.pivot(motor.LEFT, 100)
elif angle < 0:
motor.pivot(motor.RIGHT, 100)
time.sleep(WAIT_FOR_90 * abs(angle) / 90.0)
motor.stop()
return
def stopthread(state):
# print state.moving
while not state.quit:
#print "time: ", time.time(), " moved:", movetime
# print moving
if time.time() > state.movetime + 0.3 and state.moving:
motor.stop()
state.moving = False
time.sleep(0.1)
def start(self):
if param.isOn:
param.isOn = False
tempControl.off()
motor.stop()
self.button_start_manuell.setText("ON")
else:
param.isOn = True
motor.restart()
self.button_start_manuell.setText("OFF")
def stuck():
"""
This function is called when the rover has not moved 0.5 meters from
its last position
"""
motor.move_backward(100)
time.sleep(2)
motor.stop()
motor.stationary_turn(100, 1)
time.sleep(2)
motor.stop()
return
def loop():
global offset_x, offset_y, offset, forward0, forward1
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
data = tcpCliSock.recv(
BUFSIZ) # Receive data sent from the client.
# Analyze the command received and control the car accordingly.
if not data:
break
#--------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
#---------------------------------
#-------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
else:
print 'cmd error !'
def command(cmd=None):
if cmd == STOP:
motor.stop()
elif cmd == FORWARD:
motor.forward()
elif cmd == BACKWARD:
motor.backward()
elif cmd == LEFT:
motor.left()
else:
motor.right()
response = "Moving {}".format(cmd.capitalize())
return response, 200, {'Content-Type': 'text/plain'}
def loop(): global offset_x, offset_y, offset, forward0, forward1 while True: print 'Waiting for connection...' # Waiting for connection. Once receiving a connection, the function accept() returns a separate # client socket for the subsequent communication. By default, the function accept() is a blocking # one, which means it is suspended before the connection comes. tcpCliSock, addr = tcpSerSock.accept() print '...connected from :', addr # Print the IP address of the client connected with the server. while True: data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client. # Analyze the command received and control the car accordingly. if not data: break #--------Motor calibration---------- if data == 'motor_run': print 'motor moving forward' motor.setSpeed(50) motor.motor0(forward0) motor.motor1(forward1) elif data[0:9] == 'leftmotor': forward0 = data[9:] motor.motor0(forward0) elif data[0:10] == 'rightmotor': forward1 = data[10:] motor.motor1(forward1) elif data == 'motor_stop': print 'motor stop' motor.stop() #--------------------------------- #-------Turing calibration------ elif data[0:7] == 'offset=': offset = int(data[7:]) car_dir.calibrate(offset) #-------------------------------- #----------Mount calibration--------- elif data[0:8] == 'offsetx=': offset_x = int(data[8:]) print 'Mount offset x', offset_x video_dir.calibrate(offset_x, offset_y) elif data[0:8] == 'offsety=': offset_y = int(data[8:]) print 'Mount offset y', offset_y video_dir.calibrate(offset_x, offset_y) #---------------------------------------- else: print 'cmd error !'
def move(rect):
(x,y,w,h) = rect
center = (320,240)
rect_center = (x+w//2, y+h//2)
cv2.circle(img, center, 1, (0, 0, 255), 2);
cv2.circle(img, rect_center, 1, (0, 0, 255), 2);
go_back = -1
right_left = -1
if w*h > 45000 or y < 50:
go_back = 2
elif w*h < 35000 or y > 430:
go_back = 1
else:
go_back = 0
if rect_center[0] > center[0]+100:
right_left = 2
elif rect_center[0] < center[0]-100:
right_left = 1
else:
right_left = 0
location = (('stop','go','back'),
('left','go_left','back_left'),
('right','go_right','back_right'))
loc = location[right_left][go_back]
if loc == 'stop':
m.stop()
elif loc == 'go':
m.go()
elif loc == 'back':
m.back()
elif loc == 'left':
m.left()
elif loc == 'right':
m.right()
elif loc == 'go_left':
m.go_left()
elif loc == 'go_right':
m.go_right()
elif loc == 'back_left':
m.back_left()
elif loc == 'back_right':
m.back_right()
print(loc)
def goto_next_marker():
# DONE_TODO: goto new location
logging.info("Inside goto_next_marker")
motor.start_fw()
retval = None
while retval is None:
sleep(10)
retval = mk.marker_solve(mk.get_frame(cap))
if ping.get_distance() < config.MIN_DIST:
while ping.get_distance() < config.MIN_DIST:
motor.stop()
logging.info("Ping distance < %s", config.MIN_DIST)
sleep(2000)
return retval
def attention_level(attention_value):
neuropy.setCallBack("attention",attention_level)
av = attention_value
print "\nAttention level = ", attention_value
if attention_value > 50: #check if attention is above threshold
print "Moving forward"
motor.forward() #forward motion of wheelchair
time.sleep(1)
ultrasonic.ultra() #check for obbstacle distance
else :
print("Stopped Low Attention")
motor.stop()
return
def detectAndDisply(img):
max_size = -1
index = 0
barcodes = pyzbar.decode(img)
if (len(barcodes) != 0):
for barcode in barcodes:
(x, y, w, h) = barcode.rect
cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
move((x, y, w, h))
else:
m.stop()
cv2.imshow('img', img)
def speedcontrol_test():
'''
#thread_encoder = threading.Thread(target = encoder.start_encoder,args = ())
thread_motor = threading.Thread(target = motor.test_motor,args = ())
thread_standstill = threading.Thread(target = standstill,args = ())
#thread_encoder.start()
thread_motor.start()
thread_standstill.start()
'''
SPEED = 0
DIRECTION = 0
motor.init_motor(500)
motor.stop()
standstill()
def change(changePin):
changePin = int(changePin)
if changePin == 1:
motor.turnLeft()
elif changePin == 2:
motor.forward()
elif changePin == 3:
motor.turnRight()
elif changePin == 4:
motor.backward()
elif changePin == 5:
motor.stop()
else:
print("Wrong command")
return render_template("index.html")
def advance_robot(self):
if self.robot_state == START:
motor.setup()
self.robot_state = STOPPED
elif self.robot_state == STOPPED:
motor.cw()
self.robot_state = CW
elif self.robot_state == CW:
motor.ccw()
self.robot_state = CCW
elif self.robot_state == CCW:
motor.stop()
self.robot_state = STOPPED
def command(cmd=None):
if cmd == STOP:
led.led_stop()
motor.stop()
elif cmd == FORWARD:
led.led_off()
motor.forward()
elif cmd == BACKWARD:
led.led_off()
motor.backward()
elif cmd == LEFT:
led.led_left()
motor.left()
elif cmd == RIGHT:
led.led_right()
motor.right()
return "Success", 200, {'Content-Type': 'text/plain'}
def control_stop():
try:
desired = float(request.args.get('desired'))
current = float(request.args.get('current'))
weight = float(request.args.get('weight'))
desired = calculated_bri.write(desired, weight)
diff = desired - current
print '>> current: %d, desired: %d, diff: %d' % (current, desired, diff)
log_data(current, desired)
except Exception as e:
print e
state = device_state.loads()
state['motor_running'] = False
motor.stop()
return render_json(state)
def speed_control():
global speed_right, speed_left, speed_forward, speed_reverse
while True:
# Stop if both triggers pressed
if speed_forward > 0 and speed_reverse > 0:
motor.stop()
else:
# Set forward speed based on triggers and thumbstick
speed_right = -val + (speed_forward - speed_reverse)
speed_left = val + (speed_forward - speed_reverse)
# Saturate speed
if abs(speed_right) > 255:
speed_right = math.copysign(255, speed_right)
if abs(speed_left) > 255:
speed_left = math.copysign(255, speed_left)
#print('Fwd: {} Rev: {}'.format(speed_forward, speed_reverse))
print('L: {} R: {}'.format(speed_left, speed_right))
set_speed(speed_right / 255, speed_left / 255)
async def hello(websocket, path):
while True:
msg = await websocket.recv()
print("< {}".format(msg))
if msg == 'forward':
motor.forward()
if msg == 'stop':
motor.stop()
if msg == 'right':
motor.right()
if msg == 'left':
motor.left()
if msg == 'backward':
motor.backward()
def ultra():
t1 = 0
t2 = 0
GPIO.output(TRIG,1)
time.sleep(0.0001)
GPIO.output(TRIG,0)
while GPIO.input(ECHO)== 0:
t1 = time.time() #initial time
while GPIO.input(ECHO)== 1:
t2 = time.time() #final time
distance = (t2-t1) * 17150 #speed of sound is 343m/s
if (distance < 20):
motor.stop() #stop motors of obstacle too close
print("Stopped Obstacle Too Close")
time.sleep(2)
else:
print("Moving Forward Distance = {}".format(distance)) #Print distance of nearest obstacle
blink.main() #call blinking code
state = robostate()
print "Starting"
thread.start_new_thread(stopthread, (state, ))
print "Started"
while not state.quit:
key = keyhandler.getch()
print key
keyNum = ord(key)
print keyNum
if keyNum == 65:
print "up"
move(motor.forward, state)
elif keyNum == 66:
print "down"
move(motor.back, state)
elif keyNum == 67:
print "right"
move(motor.right, state)
elif keyNum == 68:
print "left"
move(motor.left, state)
elif key == 'q':
state.quit = True
else:
print "press q to quit"
time.sleep(0.1)
motor.stop()
def control():
try:
desired = float(request.args.get('desired'))
current = float(request.args.get('current'))
weight = float(request.args.get('weight'))
except Exception as e:
print e
return u'Invalid parameters'
state = device_state.loads()
print '-' * 80
print state
print '-' * 80
desired = calculated_bri.write(desired, weight)
diff = desired - current
print '>> current: %d, desired: %d, diff: %d' % (current, desired, diff)
log_data(current, desired)
# if is_night():
if 0:
print '>> nighttime'
# curtain is still open
if current > THRESHOLD and not curtain.is_closed():
# hue.set_light(False, bri=0)
print '>> curtain is still open'
motor.run_ccw()
print '>> night: motor down'
state['motor_running'] = True
state['motor_dir'] = 'down'
state['curtain'] = 'closed'
curtain.set_closed()
# if current < CURTAIN_MIN:
# motor.stop()
# curtain.set_closed()
# state['curtain'] = 'closed'
# state['motor_running'] = False
# state['motor_dir'] = ''
# curtain is closed
else:
print '>> curtain is closed'
light_state = hue.get_light()
if light_state['on']:
current_light = light_state['bri']
print '>> bri: %d' % (current_light)
bri = hue.change(diff, current_light)
else:
bri = MAX_LIGHT / 4
hue.set_light(True, bri=bri)
state['light'] = True
state['bri'] = bri
return render_json(state)
else:
print '>> daytime'
# if curtain is open, but still needs light
if curtain.is_open():
print '>> fully open'
light_state = hue.get_light()
# if light is on
if light_state['on']:
current_light = light_state['bri']
bri = hue.change(diff, current_light)
# closes curtain
if bri == 0:
print '>> bri: 0, turn off light'
hue.set_light(False, bri=bri)
state['light'] = False
state['bri'] = bri
motor.run_ccw()
state['motor_running'] = True
state['motor_dir'] = 'down'
state['curtain'] = ''
curtain.clear_open()
return render_json(state)
else:
bri = MAX_LIGHT / 2
hue.set_light(True, bri=bri)
state['light'] = True
state['bri'] = bri
print '>> bri: ', bri
return render_json(state)
# if curtain is not fully open
else:
print '>> not fully open'
hue.set_light(False, bri=0)
state['light'] = False
state['bri'] = 0
# if needs motor up (more light): desired > current
if diff > 0 and abs(diff) > THRESHOLD:
current = int(current)
# previous_max = curtain.log_max(current)
# # if brightness does not increase, we assume the curtain fully open
# if previous_max - current_int < THRESHOLD:
# curtain.set_open()
# if curtain is opening
if state['motor_dir'] != 'down' and current > CURTAIN_MAX:
curtain.set_open()
motor.stop()
state['curtain'] = 'open'
state['motor_running'] = False
state['motor_dir'] = ''
else:
motor.run_cw()
print '>> motor up'
state['motor_dir'] = 'up'
state['motor_running'] = True
state['curtain'] = ''
curtain.clear_closed()
return render_json(state)
# if needs motor down
if diff < 0 and abs(diff) > THRESHOLD:
print '>> motor down'
motor.run_ccw()
state['motor_dir'] = 'down'
state['motor_running'] = True
curtain.clear_open()
state['curtain'] = ''
# if no light, set curtain state as closed
if not state['light'] and current < CURTAIN_MIN:
motor.stop()
curtain.set_closed()
state['curtain'] = 'closed'
state['motor_running'] = False
state['motor_dir'] = ''
return render_json(state)
# stops the motor
else:
motor.stop()
state['motor_running'] = False
state['motor_dir'] = ''
if current < CURTAIN_MIN:
state['curtain'] = 'closed'
return render_json(state)
def stop():
motor.stop()
return
def straight():
stderr.write("forwards\n")
motor.straight(100)
time.sleep(WAIT_FOR_CELL)
motor.stop()
return
def stop_all(self):
steer_motor.home()
drive_motor.stop()
raw_accX = sensor.read_raw_accel_x()
raw_accY = sensor.read_raw_accel_y()
raw_accZ = sensor.read_raw_accel_z()
# Accelerometer value Degree Per Second / Scalled Data
rate_accX = sensor.read_scaled_accel_x()
rate_accY = sensor.read_scaled_accel_y()
rate_accZ = sensor.read_scaled_accel_z()
# http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG
CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX
# http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 )
# Followed the Second example because it gives resonable pid reading
pid = int(p.update(CFangleX1))
speed = pid * 30
if(pid > 0):
MOTOR.forward(speed)
elif(pid < 0):
MOTOR.backward( abs(speed) )
else:
MOTOR.stop()
print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f} | {7:.2f} ".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid, speed)
#print "{0:.2f} {1:.2f}".format( sensor.read_pitch(), sensor.read_roll())
state = robostate()
print "Starting"
thread.start_new_thread(stopthread, (state,))
print "Started"
while not state.quit:
key = keyhandler.getch()
print key
keyNum = ord(key)
print keyNum
if keyNum == 65:
print "up"
move(motor.forward, state)
elif keyNum == 66:
print "down"
move(motor.back, state)
elif keyNum == 67:
print "right"
move(motor.right, state)
elif keyNum == 68:
print "left"
move(motor.left, state)
elif key == 'q':
state.quit = True
else :
print "press q to quit"
time.sleep(0.1)
motor.stop()
def stop(): motor.stop(1)
def stop_clicked(self):
print("Stop clicked")
stop()
self.thread.kill()
def loop():
global offset_x, offset_y, offset, forward0, forward1
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
# Analyze the command received and control the car accordingly.
if not data:
break
#--------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
# -------------Added--------------
elif data == 'leftreverse':
if forward0 == "True":
forward0 = "False"
else:
forward0 = "True"
print "left motor reversed to", forward0
motor.motor0(forward0)
elif data == 'rightreverse':
if forward1 == "True":
forward1 = "False"
else:
forward1 = "True"
print "right motor reversed to", forward1
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
#---------------------------------
#-------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
#-------Turing calibration 2------
elif data[0:7] == 'offset+':
offset = offset + int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
elif data[0:7] == 'offset-':
offset = offset - int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration 2---------
elif data[0:8] == 'offsetx+':
offset_x = offset_x + int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsetx-':
offset_x = offset_x - int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety+':
offset_y = offset_y + int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety-':
offset_y = offset_y - int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
#----------Confirm--------------------
elif data == 'confirm':
config = 'offset_x = %s\noffset_y = %s\noffset = %s\nforward0 = %s\nforward1 = %s\n ' % (offset_x, offset_y, offset, forward0, forward1)
print ''
print '*********************************'
print ' You are setting config file to:'
print '*********************************'
print config
print '*********************************'
print ''
fd = open('config', 'w')
fd.write(config)
fd.close()
motor.stop()
tcpCliSock.close()
quit()
else:
print 'Command Error! Cannot recognize command: ' + data
def calibrate_motor_stop(request):
motor.stop()
return HttpResponse('Motors stop')
def loop():
global offset_x, offset_y, offset, forward0, forward1
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
data = tcpCliSock.recv(
BUFSIZ) # Receive data sent from the client.
# Analyze the command received and control the car accordingly.
if not data:
break
# --------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
# -------------Added--------------
elif data == 'leftreverse':
if forward0 == "True":
forward0 = "False"
else:
forward0 = "True"
print "left motor reversed to", forward0
motor.motor0(forward0)
elif data == 'rightreverse':
if forward1 == "True":
forward1 = "False"
else:
forward1 = "True"
print "right motor reversed to", forward1
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
# ---------------------------------
# -------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
# --------------------------------
# ----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
# ----------------------------------------
# -------Turing calibration 2------
elif data[0:7] == 'offset+':
offset = offset + int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
elif data[0:7] == 'offset-':
offset = offset - int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
# --------------------------------
# ----------Mount calibration 2---------
elif data[0:8] == 'offsetx+':
offset_x = offset_x + int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsetx-':
offset_x = offset_x - int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety+':
offset_y = offset_y + int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety-':
offset_y = offset_y - int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
# ----------------------------------------
# ----------Confirm--------------------
elif data == 'confirm':
config = 'offset_x = %s\noffset_y = %s\noffset = %s\nforward0 = %s\nforward1 = %s\n ' % (
offset_x, offset_y, offset, forward0, forward1)
print ''
print '*********************************'
print ' You are setting config file to:'
print '*********************************'
print config
print '*********************************'
print ''
fd = open('config', 'w')
fd.write(config)
fd.close()
motor.stop()
tcpCliSock.close()
quit()
else:
print 'Command Error! Cannot recognize command: ' + data
