Tag Archives: robot

#匯入相關函式庫
import BaseHTTPServer
import struct, fcntl, os
from pyfirmata import Arduino, util
from time import sleep  


board=Arduino('/dev/ttyS0') #定義Arduino連接埠
led=board.digital[13]       #定義LED腳位

EN1=board.get_pin('d:9:p')
IN1=board.digital[2]
IN2=board.digital[3]

EN2=board.get_pin('d:10:p')
IN3=board.digital[5]
IN4=board.digital[7]
GAIN=25.0   #輪速增益值,可視實際情況修改

def catch(recievedata):
  button=0;
  angle=0;
  num=0;
  try:
    num = recievedata.find(',');
    pass
  except:
    return

  button=int(recievedata[0:num]);  #判斷手機端按了哪個按鈕
  try:
      angle=int(recievedata[num+1:len(recievedata)]);
      pass
  except:
      return
  
  L_control=0;
  R_control=0;
   if angle>=0:
    L_control=(100-angle*GAIN)/100.0; #根據angle控制左輪速
    R_control=1;
  elif angle<0:
    L_control=1;
    R_control=(100+angle*GAIN)/100.0; #根據angle控制右輪速

  #控制上下限
  if L_control<0:
    L_control=0;
  elif L_control>1:
    L_control=1;
  if R_control<0:
    R_control=0;
  elif R_control>1:
    R_control=1;

  #顯示相關訊息
  print 'L'
  print L_control
  print 'R'
  print R_control

  if button==0:   #停止
    led.write(0);
    EN1.write(0);
    IN1.write(0);
    IN2.write(0);

    EN2.write(0);
    IN3.write(0);
    IN4.write(0);
    #pass
  if button==1:   #左轉
    led.write(1);
    EN1.write(L_control);
    IN1.write(0);
    IN2.write(1);

    EN2.write(R_control);
    IN3.write(0);
    IN4.write(1);
    #pass
  if button==2:   #右轉
    EN1.write(L_control);
    IN1.write(1);
    IN2.write(0);

    EN2.write(R_control);
    IN3.write(1);
    IN4.write(0);
    #pass
  pass

#處理網路通訊
class ServerHandler(BaseHTTPServer.BaseHTTPRequestHandler):
    def do_HEAD(self):
        self.send_response(200)
        self.send_header("Connection","Close")
        self.end_headers()
    def do_GET(self):
        print ("======= GET STARTED =======")
        print (self.headers)

    def do_POST(self):
        self.send_response(200)
        self.send_header("Connection","Close")
        self.end_headers()
        print ("======= POST STARTED =======")
        print (self.headers)
        print ("======= POST VALUES =======")
        length = int(self.headers['content-length'])
        item = self.rfile.read(length)
        catch(item)
        print (item)

class WebServer(BaseHTTPServer.HTTPServer):
    def __init__(self, *args, **kwargs):
        BaseHTTPServer.HTTPServer.__init__(self, *args, **kwargs)
        # Set FD_CLOEXEC flag
        flags = fcntl.fcntl(self.socket.fileno(), fcntl.F_GETFD)
        flags |= fcntl.FD_CLOEXEC
        fcntl.fcntl(self.socket.fileno(), fcntl.F_SETFD, flags)


server_class = WebServer

#29876 是指開出來的http port
httpd = server_class(("", 29876), ServerHandler)

httpd.serve_forever()