Author Topic: Oaks, websockets, robots, wifi, it's all too much!  (Read 6597 times)

digi_guy

  • Jr. Member
  • **
  • Posts: 87
Oaks, websockets, robots, wifi, it's all too much!
« on: May 06, 2016, 08:06:03 pm »
Last year I managed to get a websocket working on the Digispark pro, and today I manged to get working on the Oak. But wait, there's more! I also set it up to control my little Rover 5 from a webpage!

Keep in mind that I have no idea what I'm doing, so if anyone wants to offer up some suggestions they would be greatly appreciated. Here's my code for the Oak:

Code: [Select]
#include <ESP8266WiFi.h>


WiFiServer server(80);
WiFiClient client;

boolean CONNECTED = false;

String msg = "AAAAAAAAAAAAAAAAAAAAAA";
#define SHA1_K0 0x5a827999
#define SHA1_K20 0x6ed9eba1
#define SHA1_K40 0x8f1bbcdc
#define SHA1_K60 0xca62c1d6
#define w6 0x32353845
#define w7 0x41464135
#define w8 0x2d453931
#define w9 0x342d3437
#define w10 0x44412d39
#define w11 0x3543412d
#define w12 0x43354142
#define w13 0x30444338
#define w14 0x35423131
#define w15 0x80000000



void setup() {
  server.begin();
  Particle.publish("Status", "starting");
  pinMode(7, OUTPUT); //pwm speed control
  pinMode(8, OUTPUT); //forward/back
  analogWrite(7,0);
  digitalWrite(8,1);

  pinMode(9, OUTPUT); //pwm speed control
  pinMode(10, OUTPUT); //forward/back
  analogWrite(9,0);
  digitalWrite(10,1);

 
}

void loop() {
  client = server.available();
  if (client) {Particle.publish("Status", "connected");}
  while (client.connected()) {

    while (CONNECTED) {
       decodeMessage();
    }
   
    if(serverRequest()){
      String path = getRequestPath();
     
      if(path == F("/WS")){
        client.find("Key: ");
        msg = client.readStringUntil('==');
        client.flush();
        client.print(F("HTTP/1.1 101 Switching"));
        client.print(F(" Protocols\r\nUpgrade: "));
        client.print(F("Websocket\r\nConnection: "));
        client.print(F("Upgrade\r\nSec-WebSocket-Accept: "));
        generateResponse();     
        client.print(F("=\r\n\r\n"));
        CONNECTED = true;
      }
      else{
      client.flush();
      }
    }
  }
}

bool serverRequest(){
  if(client.available()>4){
    return client.find("GET ");
  }
  return false;
}

String getRequestPath(){
    String path = client.readStringUntil(' ');
    return path;
}
void sendResponse(String response){
    sendResponseStart();
    sendResponseChunk(response);
    sendResponseEnd();
}

void sendResponseStart(){
    //sends a chunked response
    client.println(F("HTTP/1.1 200 OK"));
    client.println(F("Content-Type: text/html"));
    client.println(F("Connection: close"));
    client.println(F("Transfer-Encoding: chunked"));
    client.println();
}

void sendResponseChunk(String response){
    client.println(response.length()+2,HEX);
    client.println(response);
    client.println();
}
void sendResponseEnd(){
    client.println(F("0"));
    client.println();
}

void generateResponse() {
  // this main function has the initial hash variables, fills the w array, then makes two passes through the sha-1 function
    uint32_t H[] = {0x67452301, 0xefcdab89, 0x98badcfe, 0x10325476, 0xc3d2e1f0}; // Initial H values
    uint32_t H1[5] = {0,0,0,0,0}; 
    uint32_t w[16];

    getMessage(w);
    getHash(w, H, H1);
     
    for (int w_cntr = 0; w_cntr<16; w_cntr++) { w[w_cntr] = 0; }
     w[15]=480 & 0xffff;
     getHash(w, H1, H1);

    convertBase64(H1);
}



void getMessage(uint32_t* w_out){ // takes the websocket key (msg) and fills up the w array
  uint8_t temp;
  int k=0;
  for (int j=0; j<6; j++){
      w_out[j]=0x00;
      for (int i=0; i<4; i++) {
        temp = msg[k++] ;
        if (k>22) temp=0x3d; //this adds thetwo '=' to the key
        w_out[j] = (w_out[j] <<8) | (temp);
      }
  } 
  w_out[6] = w6;  // the last 10 w's are constant
  w_out[7] = w7;
  w_out[8] = w8;
  w_out[9] = w9;
  w_out[10] = w10;
  w_out[11] = w11;
  w_out[12] = w12;
  w_out[13] = w13;
  w_out[14] = w14;
  w_out[15] = w15;
}


uint32_t ROTL(uint32_t number, uint8_t bits) {
  return ((number << bits) | (number >> (32-bits)));
}

char b64(int in) {
  //I used this instead of the huge 64 byte string
  // char b64[64]="ABCDEFGHIJKLMNOP...
  if (in > 62) {return '/';}
  else if (in > 61) {return '+';}
  else if (in > 51) {return (char)(in + -4 );}
  else if (in > 25) {return (char)(in + 71 );}
  else {return (char)(in + 65);} 
}
void convertBase64(uint32_t input_msg[]) {
  //This takes the final hash digest and prints out the base64 conversion
  int j = 0;
  for (int i=26; i>0; i-=6) {
    client.print(b64(input_msg[j]>>i&63));
  }
  client.print(b64( (input_msg[j]<<4 | input_msg[j+1]>>28)&63));
  j=1;
  for (int i=22; i>0; i-=6) {
    client.print(b64(input_msg[j]>>i&63));
  }
  client.print(b64( (input_msg[j]<<2 | input_msg[j+1]>>30)&63));
  j=2;
  for (int i=24; i>=0; i-=6) {
    client.print(b64(input_msg[j]>>i&63));
  }
  j=3;

  for (int i=26; i>0; i-=6) {
    client.print(b64(input_msg[j]>>i&63));
  }
  client.print(b64( (input_msg[j]<<4 | input_msg[j+1]>>28)&63));
  j=4;
  for (int i=22; i>=0; i-=6) {
    client.print(b64(input_msg[j]>>i&63));
  }
  client.print(b64( (input_msg[j]<<2)&63));
// technically it still needs to add '=' to the end, but that is done after the getresponse() call
}


void getHash(uint32_t* W, uint32_t* hash, uint32_t* hash1) {
  // this is the main sha-1 conversion takes in the 16 word array 'w' along with the initial hash and returns the new hash
  // it is called twice, first with the initial hash constants and the full w, then with the generated hash values
  // and the mostly empty w
  uint32_t a = hash[0];
  uint32_t b = hash[1];
  uint32_t c = hash[2];
  uint32_t d = hash[3];
  uint32_t e = hash[4];
  uint32_t temp = 0;
  uint32_t k = 0;
  uint32_t t;   


  for (uint32_t i=0; i<80; i++) {  //I don't htink I needs to be 32, probably just int or 8
    if (i>=16) {
      t = W[(i+13)&15] ^ W[(i+8)&15] ^ W[(i+2)&15] ^ W[i&15];
      W[i&15] = ROTL(t,1);
    }
    if (i<20) {
      t = (d ^ (b & (c ^ d))) + SHA1_K0;
    } else if (i<40) {
      t = (b ^ c ^ d) + SHA1_K20;
    } else if (i<60) {
      t = ((b & c) | (d & (b | c))) + SHA1_K40;
    } else {
      t = (b ^ c ^ d) + SHA1_K60;
    }
    t+=ROTL(a,5) + e + W[i&15];
    e=d;
    d=c;
    c=ROTL(b,30);
   
    b=a;
    a=t;
  }
  hash1[0] = hash[0] + a;
  hash1[1] = hash[1] + b;
  hash1[2] = hash[2] + c;
  hash1[3] = hash[3] + d;
  hash1[4] = hash[4] + e;
}


void decodeMessage() {
      if (client.available()>6){ 
        if (client.peek() == 129) {
          while (true) {
            client.read();
            int L = client.read() & 127;
 
            char msg_out[L+2];
            msg_out[0]= 0x81;
            msg_out[1]= L;
           
            uint8_t mask[4];
            for (int i=0; i<4; i++) {
              mask[i] = client.read();
            }
            char power_in[L];
            for (int i=0; i<L; i++) {
              uint8_t character = mask[i%4] ^ client.read();
              msg_out[2+i] = (char)character;
              power_in[i]= (char)character;
            }
 
            char toSend[] = {0x81, 1, 65};
            client.print(toSend);
             driveMotors(power_in);
            if (client.peek() == -1) {break;}
           
          }
        } else {
          CONNECTED = false;
          Particle.publish("status", "disconnected");
        }
//        while(client.read() != -1);
        // clear read buffer
      }
}

void driveMotors(char* input) {

          char* command = strtok(input, "&");
          int left_power = atoi(command);
         
          command = strtok(0, "&");
          int right_power = atoi(command);
             
          int left_side = (left_power < 0) ? 0:1;
          left_power = abs(left_power);

          int right_side = (right_power <0) ? 0:1;
          right_power = abs(right_power);
         
          analogWrite(7,left_power);
          digitalWrite(8,left_side);
          analogWrite(9,right_power);
          digitalWrite(10,right_side);
 
}

Most of that is just dealing with the websocket security, then a tiny portion reads the message from the webpage, and then adjusts the 4 pins.

Now here's the code for the webpage. I currently save it on my desktop as websocket.html because it was too much for the Digispark Pro to handle. Over the next couple of days I might look to trim it down a bit and see if it will fit on the Oak.

Code: [Select]
<!DOCTYPE html>

<meta charset="utf-8" />

<title>WebSocket Test</title>

<script language="javascript" type="text/javascript">

var startY_L = 0;
var endY_L = 0;
var Left_power=0;
var startY_R = 0;
var endY_R = 0;
var Right_power=0;
var tic=Date.now();
var toc=Date.now();
var touch_time = Date.now();

function init()
  {
document.myform.url.value = "ws://10.0.0.56:80/WS"
document.myform.inputtext.value = "0&0"
document.myform.disconnectButton.disabled = true;
document.getElementById('mycanvas').width = window.innerWidth;
// document.getElementById('mycanvas').height = window.innerHeight;


  }


function endPoint(e) {
e.preventDefault();

if (e.touches.length == 2) {
if (e.touches[0].pageX < e.touches[1].pageX) {
endY_L = (e.touches[0].pageY);
endY_R = (e.touches[1].pageY);
} else {
endY_L = (e.touches[1].pageY);
endY_R = (e.touches[0].pageY);
}

if ((Date.now()-touch_time)>250) {  //this timer needs to be adjusted based on your connection speed, the touchmove function is insanely fast
Left_power = Math.floor(1020*(-endY_L + startY_L)/300);
Right_power = Math.floor(1020*(-endY_R + startY_R)/300);

console.log(Left_power+" : "+Right_power);
websocket.send(Left_power+"&"+Right_power);  // This is the message that drives the motors
touch_time = Date.now();
}
}
}

function startPoint(e) {
e.preventDefault();

if (e.touches.length == 2) {
if (e.touches[0].pageX < e.touches[1].pageX) {
startY_L = (e.touches[0].pageY);
startY_R = (e.touches[1].pageY);
} else {
startY_L = (e.touches[1].pageY);
startY_R = (e.touches[0].pageY);
}
}
}

function sendStop(e) {
e.preventDefault();
console.log("Stopped");
websocket.send("0&0");  // stops the motors
}




  function doConnect()
  {
    writeToScreen("connecting\n");
    websocket = new WebSocket(document.myform.url.value);
    websocket.onopen = function(evt) { onOpen(evt) };
    websocket.onclose = function(evt) { onClose(evt) };
    websocket.onmessage = function(evt) { onMessage(evt) };
    websocket.onerror = function(evt) { onError(evt) };
  }

  function onOpen(evt)
  {
    writeToScreen("connected\n");
document.myform.connectButton.disabled = true;
document.myform.disconnectButton.disabled = false;
  }

  function onClose(evt)
  {
       writeToScreen("disconnected\n");
document.myform.connectButton.disabled = false;
document.myform.disconnectButton.disabled = true;
  }

  function onMessage(evt)
  {
    writeToScreen("response: " + evt.data + '\n');
  }

  function onError(evt)
  {
    writeToScreen('error: ' + evt.data + '\n');

websocket.close();

document.myform.connectButton.disabled = false;
document.myform.disconnectButton.disabled = true;

  }

  function doSend(message)
  {
    writeToScreen("sent: " + message + '\n');
    websocket.send(message);
  }

  function writeToScreen(message)
  {
tic = Date.now()- toc;
toc = Date.now();
message = tic+": "+message;
document.myform.outputtext.value += message;
document.myform.outputtext.scrollTop = document.myform.outputtext.scrollHeight;

  }

  window.addEventListener("load", init, false);
  window.addEventListener("touchstart", startPoint, false);
  window.addEventListener("touchmove", endPoint, false);
  window.addEventListener("touchend", sendStop, false);





   function sendText() {
doSend( document.myform.inputtext.value );
   }

  function clearText() {
document.myform.outputtext.value = "";
   }

   function doDisconnect() {
websocket.close();
   }


</script>

<div id="output"></div>

<form name="myform">
<p>
<textarea name="outputtext" rows="20" cols="50"></textarea>
</p>
<p>
<textarea name="inputtext" cols="50"></textarea>
</p>
<p>
<textarea name="url" cols="50"></textarea>
</p>
<p>
<input type="button" name=sendButton value="Send" onClick="sendText();">
<input type="button" name=clearButton value="Clear" onClick="clearText();">
<input type="button" name=disconnectButton value="Disconnect" onClick="doDisconnect();">
<input type="button" name=connectButton value="Connect" onClick="doConnect();">
</p>
</form>

<canvas id="mycanvas" width="50" height="400" style="border-style: solid; float: left;">
      Canvas element not supported.
</canvas>


</html>

Did I mention the best part? It's touch based! The idea is to hold your touch-enabled device with both hands, and then slide your thumbs to increase/decrease/reverse the track speed.

It's all still a mess, but it's working and I couldn't wait to share.

Right now it's setup to work within my home network where both my laptop and Oak are connected to the same router. If I get time this weekend I'm going to set it up on my server and see if I can get it working from my phone over 4G.

driffster

  • Newbie
  • *
  • Posts: 42
Re: Oaks, websockets, robots, wifi, it's all too much!
« Reply #1 on: May 07, 2016, 10:12:52 am »
That looks very interesting, when I get some free time I will look it up, trying to do more or less the same thing.