/*
///////////////////////////////////////////////
// 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!");
