def _Motor_TurnLeft_():
#print ' M2-L REV;M1-R FOR; '
XR.GPIOSet(XR.ENA)
XR.GPIOSet(XR.ENB)
XR.GPIOSet(XR.IN1)
XR.GPIOClr(XR.IN2)
XR.GPIOClr(XR.IN3)
XR.GPIOSet(XR.IN4)
XR.GPIOClr(XR.LED1)
XR.GPIOSet(XR.LED2)
def _Motor_TurnRight_():
#print ' M2-L FOR;M1-R REV; '
XR.GPIOSet(XR.ENA)
XR.GPIOSet(XR.ENB)
XR.GPIOClr(XR.IN1)
XR.GPIOSet(XR.IN2)
XR.GPIOSet(XR.IN3)
XR.GPIOClr(XR.IN4)
XR.GPIOClr(XR.LED1)
XR.GPIOSet(XR.LED2)
def _Motor_Backward_():
#print ' M2-L REV;M1-R REV; '
XR.GPIOSet(XR.ENA)
XR.GPIOSet(XR.ENB)
XR.GPIOClr(XR.IN1)
XR.GPIOSet(XR.IN2)
XR.GPIOClr(XR.IN3)
XR.GPIOSet(XR.IN4)
XR.GPIOSet(XR.LED1)
XR.GPIOClr(XR.LED2)
def _Motor_Forward_():
#print ' M2-L FOR;M1-R FOR; '
XR.GPIOSet(XR.ENA)
XR.GPIOSet(XR.ENB)
XR.GPIOSet(XR.IN1)
XR.GPIOClr(XR.IN2)
XR.GPIOSet(XR.IN3)
XR.GPIOClr(XR.IN4)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
def _Get_Distence_():
time_count = 0
time.sleep(0.01)
XR.GPIOSet(XR.TRIG)
time.sleep(0.000015)
XR.GPIOClr(XR.TRIG)
while not XR.DigitalRead(XR.ECHO):
pass
t1 = time.time()
while XR.DigitalRead(XR.ECHO):
if (time_count < 0xfff):
time_count = time_count + 1
time.sleep(0.000001)
pass
else:
print 'NO ECHO receive! Please check connection '
break
t2 = time.time()
Distence = (t2 - t1) * 340 / 2 * 100
print 'Distence is %d' % Distence
if (Distence < 500):
print 'Distence is %d' % Distence
return Distence
else:
print 'Distence is 0'
return 0
def _Motor_Stop_():
#print ' M2-L STOP;M1-R STOP; '
XR.GPIOClr(XR.ENA)
XR.GPIOClr(XR.ENB)
XR.GPIOClr(XR.IN1)
XR.GPIOClr(XR.IN2)
XR.GPIOClr(XR.IN3)
XR.GPIOClr(XR.IN4)
XR.GPIOSet(XR.LED1)
XR.GPIOClr(XR.LED2)
def _FLOW_LED_(): #流水灯
print(" Flow led start...")
XR.GPIOClr(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.2)
for i in range(0, 10):
XR.GPIOSet(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.2)
XR.GPIOClr(XR.LED0)
XR.GPIOSet(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.2)
XR.GPIOClr(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOSet(XR.LED2)
time.sleep(0.2)
XR.GPIOClr(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.2)
print(" Flow led over...")
def _Communication_Decode_(buffer):
#print 'Communication_decoding...'
if buffer[0]=='00':
if buffer[1]=='01': #前进
go.forward()
elif buffer[1]=='02': #后退
go.back()
elif buffer[1]=='03': #左转
go.left()
elif buffer[1]=='04': #右转
go.right()
elif buffer[1]=='00': #停止
go.stop()
else:
go.stop()
elif buffer[0]=='02':
if buffer[1]=='01':#M1_R速度
speed=hex(eval('0x'+buffer[2]))
speed=int(speed,16)
go.M2_Speed(speed)
elif buffer[1]=='02':#M2_L侧速度
speed=hex(eval('0x'+buffer[2]))
speed=int(speed,16)
go.M1_Speed(speed)
elif buffer[0]=='01':
ServoNum = eval('0x'+buffer[1])
angle = _Angle_cal_(buffer[2])
Servo.XiaoRGEEK_SetServoAngle(ServoNum,angle)
if (angle%2):
XR.GPIOSet(XR.LED1)
XR.GPIOSet(XR.LED2)
else:
XR.GPIOClr(XR.LED2)
XR.GPIOClr(XR.LED1)
elif buffer[0]=='13':
if buffer[1]=='01':
G_val.Cruising_Flag = 1#进入红外跟随模式
print 'Cruising_Flag红外跟随模式 1 '
elif buffer[1]=='02':#进入红外巡线模式
G_val.Cruising_Flag = 2
print 'Cruising_Flag红外巡线模式 %d '%G_val.Cruising_Flag
elif buffer[1]=='03':#进入红外避障模式
G_val.Cruising_Flag = 3
print 'Cruising_Flag红外避障模式 %d '%G_val.Cruising_Flag
elif buffer[1]=='04':#进入超声波避障模式
G_val.Cruising_Flag = 4
print 'Cruising_Flag超声波避障 %d '%G_val.Cruising_Flag
elif buffer[1]=='05':#进入超声波距离PC显示
G_val.Cruising_Flag = 5
print 'Cruising_Flag超声波距离PC显示 %d '%G_val.Cruising_Flag
elif buffer[1]=='06':
G_val.Cruising_Flag = 6
print 'Cruising_Flag超声波摇头避障 %d '%G_val.Cruising_Flag
elif buffer[1]=='07':
_Socket_sendbuf_(['\xFF','\xA8','\x00','\x00','\xFF'])
G_val.Cruising_Flag = 7
elif buffer[1]=='08':
if buffer[2]=='00':#Path_Dect 调试模式
G_val.Path_Dect_on = 0
G_val.Cruising_Flag = 8
print 'Cruising_Flag Path_Dect调试模式 8'
elif buffer[2]=='01':#Path_Dect 循迹模式
path_sh = 'sh '+ os.path.split(os.path.abspath(__file__))[0] + '/stop_mjpg_streamer.sh &'
call("%s"%path_sh,shell=True)
time.sleep(2)
G_val.Path_Dect_on = 1
G_val.Cruising_Flag = 9
print 'Cruising_Flag Path_Dect循迹模式 9 '
elif buffer[1]=='00':
G_val.RevStatus=0
G_val.Cruising_Flag = 0
print 'Cruising_Flag正常模式 %d '%G_val.Cruising_Flag
elif buffer[0]=='a0':
Tangle=hex(eval('0x'+buffer[1]))
Tangle=int(Tangle,16)
G_val.TurnAngle=Tangle
Golen=hex(eval('0x'+buffer[2]))
Golen=int(Golen,16)
G_val.Golength=Golen
G_val.RevStatus=2
elif buffer[0]=='a1':
Tangle=hex(eval('0x'+buffer[1]))
Tangle=int(Tangle,16)
G_val.TurnAngle=Tangle
Golen=hex(eval('0x'+buffer[2]))
Golen=int(Golen,16)
G_val.Golength=Golen
G_val.RevStatus=1
elif buffer[0]=='40':
temp=hex(eval('0x'+buffer[1]))
temp=int(temp,16)
print 'mode_flag====== %d '%temp
G_val.motor_flag = temp
elif buffer[0]=='32': #存储角度
Servo.XiaoRGEEK_SaveServo()
XR.GPIOSet(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.01)
XR.GPIOSet(XR.LED2)
XR.GPIOClr(XR.LED1)
elif buffer[0]=='33': #读取角度
Servo.XiaoRGEEK_ReSetServo()
XR.GPIOSet(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.01)
XR.GPIOSet(XR.LED2)
XR.GPIOClr(XR.LED1)
elif buffer[0]=='04': #开关灯模式 FF040000FF开灯 FF040100FF关灯
if buffer[1]=='00':
XRLED.Open_Light()
elif buffer[1]=='01':
XRLED.Close_Light()
else:
print 'error1 command!'
elif buffer == ['ef','ef','ee'] :
print 'Heartbeat Packet!'
elif buffer[0]=='fc':#FFFC0000FF shutdown
XR.GPIOClr(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.1)
XR.GPIOSet(XR.LED0)
XR.GPIOSet(XR.LED1)
XR.GPIOSet(XR.LED2)
time.sleep(0.1)
XR.GPIOClr(XR.LED0)
XR.GPIOClr(XR.LED1)
XR.GPIOClr(XR.LED2)
time.sleep(0.1)
XR.GPIOSet(XR.LED0)
XR.GPIOSet(XR.LED1)
XR.GPIOSet(XR.LED2)
os.system("sudo shutdown -h now")
else:
print 'error4 command!'
def _Close_Light_(): #关大灯
XR.GPIOSet(XR.LED0) #大灯正极接5V 负极接IO口
time.sleep(0.5)