인텔 에디슨 RC카 만들기

인텔 에디슨 RC Car Arduino Sketch

/*
///////////////////////////////////////////////
// Description: Intel Edison WiFi Robot 
// Author: Motoduino 2015
// Website: http://www.motoduino.com
/////////////////////////////////////////////
 
 This example is written for a network using WPA2 encryption. 
 For WEP, change the Wifi.begin() call accordingly.
 
 Circuit:
 * L298P Motor Shield attached (10, 11, 12, 13);
 
 */
#include <WiFi.h>
#include "sha1.h"
#include "Base64.h"
 
#define DEBUG 1
 
char ssid[] = "XXXXXXX";      //  your network SSID (name) 
char pass[] = "XXXXXXXXXX";         // your network password
//char ssid[] = "techbang01";      //  Open mode  
 
//   可以修改此處
int MotorSpeedControl = 200;  //  馬達速度控制
int LeftMotorOffset = 10;     // MotorSpeedControl + LeftMotorOffset < 255
int RightMotorOffset = 50;   // MotorSpeedControl + RightMotorOffset < 255
int HTTP_SERVER_PORT = 8999;
//  需修改
char websocket_server[]=" webSocket = new WebSocket(\"ws://192.168.1.16:8999\"); ";
// 需修改
char IFrame_Tag[]="<iframe src=\"http://192.168.1.16:8080\" width=\"640\" height=\"480\" scrolling=\"no\" frameborder=\"0\"></iframe>";
 
int keyIndex = 0;  // your network key Index number (needed only for WEP)
 
int LED_PIN = 13;
// 馬達與motoduino的腳位對應
const int Motor_E1 = 10;  // 控制馬達1轉速 digital pin 10 of Arduino (PWM)  
const int Motor_E2 = 11; // 控制馬達2轉速 digital pin 11 of Arduino (PWM)    
const int Motor_M1 = 12;     // 控制馬達1正反轉 digital pin 12 of Arduino
const int Motor_M2 = 13;    // 控制馬達2正反轉 digital pin 13 of Arduino
 
int status = WL_IDLE_STATUS;
WiFiServer server(HTTP_SERVER_PORT);    
IPAddress robot_ip;
 
struct dataFrame {
    bool isMasked;
    bool isFinal;
    byte opcode;
    byte mask[4];
    byte length;
    char data[64];
} dataframe;
 
void setup() {
  Serial.begin(9600);      // initialize serial communication
  
   setPwmSwizzler(3, 5, 10, 11); //設定Edison Arduino D3, D5, D10, D11為PWM輸出
   
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  
  // 輸出入接腳初始設定
  pinMode(Motor_E1, OUTPUT);  //設定 Motor_E1為輸出腳位
  pinMode(Motor_E2, OUTPUT);  //設定 Motor_E2為輸出腳位
  pinMode(Motor_M1, OUTPUT);  //設定 Motor_M1為輸出腳位
  pinMode(Motor_M2, OUTPUT);  //設定 Motor_M2為輸出腳位
 
  // check for the presence of the shield:
  if (WiFi.status() == WL_NO_SHIELD) {
    Serial.println("WiFi shield not present"); 
    while(true);        // don't continue
  } 
 
  String fv = WiFi.firmwareVersion();
  if( fv != "1.1.0" )
    Serial.println("Please upgrade the firmware");
 
  // attempt to connect to Wifi network:
  while ( status != WL_CONNECTED) { 
    Serial.print("Attempting to connect to Network named: ");
    Serial.println(ssid);                   // print the network name (SSID);
 
    // Connect to WPA/WPA2 network. Change this line if using open or WEP network:    
    status = WiFi.begin(ssid, pass);
    // wait 10 seconds for connection:
//    status = WiFi.begin(ssid);  // network: open mode
 
    delay(10000);
  } 
  server.begin();                           // start the web server
  printWifiStatus();                        // you're connected now, so print out the status
  digitalWrite(LED_PIN, HIGH);
}
 
void loop() {
    char temp[128];
    char key[80];
    char bite;
    
    bool hasUpgrade = false;
    bool hasConnection = false;
    bool isSupportedVersion = false;
    bool hasHost = false;
    bool hasKey = false;  
 
   WiFiClient client = server.available();   // listen for incoming clients
    byte counter = 0;
 
  if (client) {                             // if you get a client,
    Serial.println("new client");           // print a message out the serial port
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected()) {            // loop while the client's connected
      if (client.available()) {             // if there's bytes to read from the client,
       
        char bite = client.read();             // read a byte, then
        Serial.write(bite);
        temp[counter++] = bite;
        
        if(bite == '\n')
        {
           if (counter <= 2){   // check end of header "\r\n"                  
                Serial.println(" End of Header!");
                break;
           }
        }
           
         if (counter > 2 && (bite == '\n' || counter >= 127)) { // EOL got, or too long header. temp should now contain a header string
            temp[counter - 2] = 0; // Terminate string before CRLF
            
            // Ignore case when comparing and allow 0-n whitespace after ':'. See the spec:
            // http://www.w3.org/Protocols/rfc2616/rfc2616-sec4.html
            if (!hasUpgrade && strstr_P(temp, PSTR("Upgrade: "))) {
                // OK, it's a websockets handshake for sure
                hasUpgrade = true;    
            } else if (!hasConnection && strstr_P(temp, PSTR("Connection: "))) {
                hasConnection = true;
            } else if (!hasHost && strstr_P(temp, PSTR("Host: "))) {
                hasHost = true;
            } else if (!hasKey && strstr_P(temp, PSTR("Sec-WebSocket-Key: "))) {
                hasKey = true;
                strtok(temp, " ");
                strcpy(key, strtok(NULL, " "));
            } else if (!isSupportedVersion && strstr_P(temp, PSTR("Sec-WebSocket-Version: ")) && strstr_P(temp, PSTR("13"))) {
                isSupportedVersion = true;
            }
            
            counter = 0; // Start saving new header string
        }
      }
    }
      
        // Assert that we have all headers that are needed. If so, go ahead and
        // send response headers.
        if (hasUpgrade && hasConnection && isSupportedVersion && hasHost && hasKey) {
          Serial.println("websocket request!");
            strcat_P(key, PSTR("258EAFA5-E914-47DA-95CA-C5AB0DC85B11")); // Add the omni-valid GUID
            Sha1.init();
            Sha1.print(key);
            uint8_t *hash = Sha1.result();
            base64_encode(temp, (char*)hash, 20);
            char buf[132];
            client.println("HTTP/1.1 101 Switching Protocol");
            client.println("Upgrade: websocket");
            client.println("Connection: Upgrade");
            client.print("Sec-WebSocket-Accept: ");
            client.println(temp);
             client.println();
      
            Serial.println("wesocket client connected....");
              //
              wesocket_data_session(&client);    // data session
 
        } else {
               Serial.println("HTTP request!");
            // Nope, failed handshake. Disconnect
                  client.println("HTTP/1.1 200 OK");
                  client.println("Content-type:text/html");
                  client.println("Connection: close");  // the connection will be closed after completion of the response
                  client.println();
                   mainpage(&client);
 //                LED_test(&client);
                 client.println();
                 client.stop();
                 Serial.println("client disonnected");
      }
  }
}
 
bool wesocket_data_session(WiFiClient *myClient)
{
       while(true)
       {
           if (myClient->available()) {             // if there's bytes to read from the client,
 
                Serial.println("getFrame!");
                  if(getFrame(myClient))
                  {
                     websocket_send(myClient, "Received Data!", 14);
                    // break;
                  }
                  else
                  {
                    myClient->stop();
                    break;
                  }
              }
           }
    return true;
}
 
void printWifiStatus() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());
 
  // print your WiFi shield's IP address:
  robot_ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(robot_ip);
  
  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");
  // print where to go in a browser:
  Serial.print("To see this page in action, open a browser to http://");
  Serial.println(robot_ip);
}
 
void LED_test(WiFiClient *myClient)
{
            // the content of the HTTP response follows the header:
            myClient->print("Click <a href=\"/H\">here</a> turn the LED on pin 3 on<br>");
            myClient->print("Click <a href=\"/L\">here</a> turn the LED on pin 3 off<br>");
            // The HTTP response ends with another blank line:
}
 
boolean mainpage(WiFiClient *myClient)
{
   Serial.println("mainpage responsed!");
            myClient->println("<!DOCTYPE HTML>");
            myClient->println("<html><head>");
   
            websocket_js(myClient);    // Javascript: websocket process 
            
            myClient->println("</head>");
          myClient->println("<body>");
 
          myClient->print(IFrame_Tag);    // video streaming
 
          myClient->println("<table border= 0>");
          myClient->print("<tr>");
          myClient->print("<th>  </th>");
          myClient->print("<th>");
          myClient->print("<form method=\"get\" >");
          myClient->print("<input type=hidden name=V value=F /><br />");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" onclick=\"sendF()\" >Forward</button>");
//          myClient->print("<input type=submit style='height: 55px; width: 200px' value=FORWARD>");
            myClient->print("</form>");
              myClient->print("</th><th> </th>");
          myClient->print("</tr>");
          myClient->print("<tr><th>");
          myClient->print("<form method=\"get\" >");
          myClient->print("<input type=hidden name=V value=L /><br />");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" onclick=\"sendL()\" >LEFT</button>");
//          myClient->print("<input type=submit style='height: 55px; width: 200px' value=LEFT>"); 
          myClient->print("</form>");
          myClient->print("</th><th>");
          myClient->print("<form method=\"get\" >");
          myClient->print("<input type=hidden name=V value=S /><br />");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" onclick=\"sendS()\" >STOP</button>");
//          myClient->print("<input type=submit style='height: 55px; width: 200px' value=STOP>"); 
          myClient->print("</form>");
         myClient->print("</th><th> ");
          myClient->print("<form method=\"get\" >");
          myClient->print("<input type=hidden name=V value=R /><br />");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" onclick=\"sendR()\" >RIGHT</button>");
//          myClient->print("<input type=submit style='height: 55px; width: 200px' value=RIGHT>"); 
          myClient->print("</form>");
          myClient->print("</th></tr> <tr> <th>  </th> <th> ");
          myClient->print("<form method=\"get\" >");
          myClient->print("<input type=hidden name=V value=B /><br />");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" onclick=\"sendB()\" >BACKWARD</button>");
//          myClient->print("<input type=submit style='height: 55px; width: 200px' value=BACKWARD>"); 
          myClient->print("</form>");
          myClient->print(" </th> <th> </th> </tr>");
          myClient->print("</table>");
          myClient->print("<br/>");
          myClient->print("<font color=#888888 size=1>2015 Motoduino.</font><font size=3>");
//            myClient->print("<br /><font size=3>http://www.motoduino.com</font><br />");
          myClient->print("<br /><a href=\"http://www.motoduino.com\"></font><font size=3>http://www.motoduino.com</font></a><br />");      
     //     myClient->print("<a href=\"http://www.motoduino.com\">http://www.motoduino.com</a>
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" id=\"connectbutton\" onclick=\"openSocket()\" >CONNECT</button>");
          myClient->println("<button style='height: 55px; width: 200px' type=\"button\" id=\"disconnectbutton\" onclick=\"closeSocket()\" >DISCONNECT</button>");
          
          myClient->print("</body>");
          myClient->print("</html>");
    
          return true; 
}
 
void websocket_js(WiFiClient *myClient)
{
//  myClient->println("</div>");
  myClient->println("<div id=\"messages\"></div>");
  myClient->println("<script type=\"text/javascript\"> ");
  myClient->println(" var webSocket; ");
  myClient->println(" var ip_address;");
  myClient->println("var messages = document.getElementById(\"messages\"); ");
  myClient->println(" function openSocket() { ");
  myClient->println(" if(webSocket !== undefined && webSocket.readyState !== WebSocket.CLOSED){ ");
  myClient->println("  writeResponse(\"WebSocket is already opened.\"); return; }");
  myClient->println(websocket_server);
  myClient->println(" webSocket.onopen = function(event){ ");
  myClient->println("document.getElementById(\"connectbutton\").style.background='#00FF00'; ");
  myClient->println("document.getElementById(\"disconnectbutton\").style.background='#FFFFFF'; ");
 
 myClient->println(" if(event.data === undefined) return; writeResponse(event.data); }; ");
 myClient->println("webSocket.onmessage = function(event){ writeResponse(event.data); };");
 myClient->println("webSocket.onclose = function(event){ writeResponse(\"Connection closed\"); }; } ");
 
 myClient->println("function sendF(){ webSocket.send(\"F\"); } ");
 myClient->println("function sendB(){ webSocket.send(\"B\"); } ");
 myClient->println("function sendS(){ webSocket.send(\"S\"); } ");
 myClient->println("function sendL(){ webSocket.send(\"L\"); } ");
 myClient->println("function sendR(){ webSocket.send(\"R\"); } "); 
 myClient->println("function closeSocket(){ webSocket.close(); ");
 myClient->println("document.getElementById(\"connectbutton\").style.background='#FFFFFF'; ");
  myClient->println("document.getElementById(\"disconnectbutton\").style.background='#FF0000'; } ");
 // myClient->println("function writeResponse(text){ messages.innerHTML += \"<br/>\" + text; }  ");
 myClient->println("function writeResponse(text){ }  ");
 
 myClient->println("</script>  ");
  
}
 
bool getFrame(WiFiClient *myClient) {
    byte bite;
    
    // Get opcode
    bite = myClient->read();
        
    dataframe.opcode = bite & 0xf; // Opcode
    dataframe.isFinal = bite & 0x80; // Final frame?
    // Determine length (only accept <= 64 for now)
    bite = myClient->read();
    dataframe.length = bite & 0x7f; // Length of payload
    if (dataframe.length > 64) {
#ifdef DEBUG
        Serial.print(F("Too big frame to handle. Length: "));
        Serial.println(dataframe.length);
#endif
        myClient->write((uint8_t) 0x08);
        myClient->write((uint8_t) 0x02);
        myClient->write((uint8_t) 0x03);
        myClient->write((uint8_t) 0xf1);
        return false;
    }
    // Client should always send mask, but check just to be sure
    dataframe.isMasked = bite & 0x80;
    if (dataframe.isMasked) {
        dataframe.mask[0] = myClient->read();
        dataframe.mask[1] = myClient->read();
        dataframe.mask[2] = myClient->read();
        dataframe.mask[3] = myClient->read();
    }
 
    // Clear any frame data that may have come previously
    memset(dataframe.data, 0, sizeof(dataframe.data)/sizeof(char));
    
    // Get message bytes and unmask them if necessary
    for (int i = 0; i < dataframe.length; i++) {
        if (dataframe.isMasked) {
            dataframe.data[i] = myClient->read() ^ dataframe.mask[i % 4];
        } else {
            dataframe.data[i] = myClient->read();
        }
    }
    // Frame complete!
    
    if (!dataframe.isFinal) {
        // We don't handle fragments! Close and disconnect.
#ifdef DEBUG
        Serial.println(F("Non-final dataframe, doesn't handle that."));
#endif
        myClient->print((uint8_t) 0x08);
        myClient->write((uint8_t) 0x02);
        myClient->write((uint8_t) 0x03);
        myClient->write((uint8_t) 0xf1);
        return false;
    }
 
    switch (dataframe.opcode) {
        case 0x01: // Txt frame
              Serial.println(dataframe.data);
              
              // 
              switch(dataframe.data[0])
              {
                case 'F':
                          forward(0, MotorSpeedControl);
                          break;
                case 'B':
                          backward(0, MotorSpeedControl);
                          break;
                case 'S':
                          motorstop(0, 0);
                          break;
                case 'L':
                          turnleft(0, MotorSpeedControl);
                           break;
               case 'R':
                           turnright(0, MotorSpeedControl);
                          break;
               default:
                             break;
              };
              
            break;
            
        case 0x08:
            // Close frame. Answer with close and terminate tcp connection
            // TODO: Receive all bytes the client might send before closing? No?
#ifdef DEBUG
            Serial.println(F("Close frame received. Closing in answer."));
#endif
            myClient->write((uint8_t) 0x08);
            return false;
            break;
            
        default:
            // Unexpected. Ignore. Probably should blow up entire universe here, but who cares.
#ifdef DEBUG
            Serial.println(F("Unhandled frame ignored."));
#endif
            return false;
            break;
    }
    return true;
}
 
bool websocket_send(WiFiClient *myClient, char *data, byte length)
{
 
    myClient->write((uint8_t) 0x81); // Txt frame opcode
    myClient->write((uint8_t) length); // Length of data
    myClient->write( (const uint8_t *)data, length );
    return true;
}
 
//// Motor Control Section
void motorstop(byte flag, byte motorspeed)
{
  Serial.println("stop!");
  
  digitalWrite( Motor_M1, LOW);
  digitalWrite( Motor_M2, LOW);
  analogWrite( Motor_E1, 0);
  analogWrite( Motor_E2, 0);
}
 
void forward(byte flag, byte motorspeed)
{
  Serial.println("forward!");
    
  digitalWrite( Motor_M1, HIGH);
  digitalWrite( Motor_M2, HIGH);
  analogWrite( Motor_E1, motorspeed+RightMotorOffset);
  analogWrite( Motor_E2, motorspeed+LeftMotorOffset);
}
 
void backward(byte flag, byte motorspeed)
{
  Serial.println("back!");
  
  digitalWrite( Motor_M1, LOW);
  digitalWrite( Motor_M2, LOW);
  analogWrite( Motor_E1, motorspeed+RightMotorOffset);
  analogWrite( Motor_E2, motorspeed+LeftMotorOffset);
}
 
void turnright(byte flag, byte motorspeed)
{
  Serial.println("right!");  
  

댓글

댓글 본문
버전 관리
Kim John
현재 버전
선택 버전
graphittie 자세히 보기