Main

/*
Robotics Via UDP over Wifi
Author: Adam Thomas
 */

//Dependencies
#include <SPI.h>         
#include <Servo.h> 
#include <WiFi.h>
#include <WiFiUdp.h>



//Set up WiFi
//String ssidString;     //  Default Wifi SSID <----------------------------------------------------------------------------------------
//String passString;  // Default Wifi password <----------------------------------------------------------------------------------------

//char ssid[] = "guest7";     //  your network SSID (name) 
//char pass[] = "intel4life";  // your network password

char ssid[] = "SSIDNAMEHERE";     //  your network SSID (name) 
char pass[] = "WIFIPASSWORDHERE";  // your network password

int status = WL_IDLE_STATUS;     // the Wifi radio's status
byte mac[] = {  
  0x08, 0x11, 0x96, 0xCF, 0x31, 0xFC };
IPAddress ip(192, 168,1,148);
unsigned int localPort = 8888;              // local port to listen on
char packetBuffer[255];  //buffer to hold incoming packet,
char  ReplyBuffer[] = "acknowledged";       // a string to send back

//Instantiate Objects
Servo HeadLeftRight; 
Servo HeadUpDown; 
Servo EyesLeftRight; 
Servo EyesUpDown; 
Servo MouthOpenShut; 
WiFiUDP Udp;

//Assign Pin Numbers
int led = 13;
int StatusBlue = 0;
int StatusGreen = 2;
int StatusRed = 1;


//Assign Variables
int Deadzone = 3;
boolean CenterServos = false;
int RXRate = 0;

void setup() {
    Serial.begin(9600);
//   while (!Serial) {
//    ; // wait for serial port to connect. 
//    }
    
    
  //Instantiate Pins
  pinMode(led, OUTPUT);
  pinMode(StatusBlue, OUTPUT);
  pinMode(StatusGreen, OUTPUT);
  pinMode(StatusRed, OUTPUT);
  
  StatusBlink("BLUE", 50, 10);

  
  HeadLeftRight.attach(6); 
  HeadUpDown.attach(9); 
  EyesLeftRight.attach(3); 
  EyesUpDown.attach(5); 
  MouthOpenShut.attach(10); 
  

   
   Status("RED");
   //--------------------------------------------------SET UP Wifi-----------------
  ConnectToWLAN();
  system("ifconfig eth0 192.168.1.148 netmask 255.255.255.0 up"); 
  Udp.begin(localPort);
  StatusBlink("BLUE", 50, 10);
  //debug setup
  Status("BLUE");

}

void loop() {
  // if there's data available, read a packet+
  
  
  delay(RXRate);
  //printCurrentNet();
  
  int packetSize = Udp.parsePacket();
  if(packetSize)
  {
    digitalWrite(led, HIGH); 
    Udp.read(packetBuffer,255);
    //Serial.println("Contents:");
   
   
    //Serial.println(packetBuffer); //<print recieved data for debug
   
   
   // Serial.println(String(packetBuffer).indexOf("Buttons5:ON"));
      //RX rate 
      //digitalWrite(led, HIGH);  
      //delay(10);
     //digitalWrite(led, LOW); 
    //interpret commands
    
   //---------------------------------------------------------------RB
    if(String(packetBuffer).indexOf("Buttons5:ON") > -1) //RB
    {
      
         digitalWrite(led, HIGH); 
    }
    if(String(packetBuffer).indexOf("Buttons5:OFF") > -1)
    {
      //Serial.println("Turning LED off.");
       digitalWrite(led, LOW); 
    }
    
    
    //---------------------------------------------------------------A
    if(String(packetBuffer).indexOf("Buttons0:ON") > -1) 
    {
         Status("GREEN"); 
    }
    if(String(packetBuffer).indexOf("Buttons0:OFF") > -1)
    {
        Status("BLUE"); 
    }

    
    
   //---------------------------------------------------------------B
    if(String(packetBuffer).indexOf("Buttons1:ON") > -1)
    {
        Status("RED"); 
    }
    if(String(packetBuffer).indexOf("Buttons1:OFF") > -1)
    {
        Status("BLUE"); 
    }
    
       //---------------------------------------------------------------Y
    if(String(packetBuffer).indexOf("Buttons3:ON") > -1)
    {
      Status("YELLOW"); 
    }
    if(String(packetBuffer).indexOf("Buttons3:OFF") > -1)
    {
      Status("BLUE"); 
    }
    
       //---------------------------------------------------------------X
    if(String(packetBuffer).indexOf("Buttons2:ON") > -1)
    {
       Status("VIOLET"); 
    }
    if(String(packetBuffer).indexOf("Buttons2:OFF") > -1)
    {
      Status("BLUE"); 
    }
    
    
           //---------------------------------------------------------------Start (Speaker)
    if(String(packetBuffer).indexOf("Buttons7:ON") > -1)
    {
    }
    if(String(packetBuffer).indexOf("Buttons7:OFF") > -1)
    {
    }
    
           //---------------------------------------------------------------Back (Mic)
    if(String(packetBuffer).indexOf("Buttons6:ON") > -1)
    {
    }
    if(String(packetBuffer).indexOf("Buttons6:OFF") > -1)
    {
    }
    
   //---------------------------------------------------------------LB
   if(String(packetBuffer).indexOf("Buttons4:ON") > -1) 
    {
      CenterServos = true;
 
      
    }
   if(String(packetBuffer).indexOf("Buttons4:OFF") > -1)
   {
      CenterServos = false;
   }
    
        
   //---------------------------------------------------------------Left Joystick, Right/Left (X Axis)
    if(String(packetBuffer).indexOf("X:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, true);
      EyesLeftRight.write(SD); 
    }
    
    //---------------------------------------------------------------Left Joystick, Up/Down (Y Axis)
    if(String(packetBuffer).indexOf("Y:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, true);
      EyesUpDown.write(SD); 
    }
    
       //---------------------------------------------------------------Right Joystick, Right/Left (RotationX Axis)
    if(String(packetBuffer).indexOf("RotationX:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, true);
      HeadLeftRight.write(SD); 
    }
    
    //---------------------------------------------------------------Right Joystick, Up/Down (RotationY Axis)
    if(String(packetBuffer).indexOf("RotationY:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, true);
      HeadUpDown.write(SD); 
    }
    
    //---------------------------------------------------------------Left Trigger
    if(String(packetBuffer).indexOf("Z:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, false);
       MouthOpenShut.write(SD); 
    }
    
    //---------------------------------------------------------------Right Trigger
//        if(String(packetBuffer).indexOf("Sliders1:") == 0)
//    {
//      //Serial.println("Turning LED off.");
//      int SD = ServoDegree(packetBuffer, false);
//      analogWrite(Bled,SD); 
//    }

    //---------------------------------------------------------------Audio
    if(String(packetBuffer).indexOf("Audio:") == 0)
    {
      //Serial.println("Turning LED off.");
      int SD = ServoDegree(packetBuffer, false);
      MouthOpenShut.write(SD);
    }
  
      // send a reply, to the IP address and port that sent us the packet we received
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
    Udp.write(ReplyBuffer);
    Udp.endPacket();
    
    digitalWrite(led, LOW); 
  }
  

}

void StatusBlink(char* Color, int Frequency, int Qty)
{
  for (int i = 0; i < Qty; i++) 
  {
    Status(Color);
    delay(Frequency);
    Status("OFF");
    delay(Frequency);
  }
}

void Status(char* Color)
{
  
      if(Color == "RED")
      {
        digitalWrite(StatusRed, LOW); 
        digitalWrite(StatusGreen, HIGH); 
        digitalWrite(StatusBlue, HIGH); 
        Serial.println("RED:");
      }
      
       if(Color ==  "GREEN")
     {
        digitalWrite(StatusRed, HIGH); 
        digitalWrite(StatusGreen, LOW); 
        digitalWrite(StatusBlue, HIGH); 
        Serial.println("GREEN");
      }
      
      if(Color ==  "BLUE")
      {
        digitalWrite(StatusRed, HIGH); 
        digitalWrite(StatusGreen, HIGH); 
        digitalWrite(StatusBlue, LOW); 
        Serial.println("BLUE");
      }
      
      if(Color ==  "VIOLET")
      {
        digitalWrite(StatusRed, LOW); 
        digitalWrite(StatusGreen, HIGH); 
        digitalWrite(StatusBlue, LOW); 
      }
      
      if(Color ==  "YELLOW")
      {
        digitalWrite(StatusRed, LOW); 
        digitalWrite(StatusGreen, LOW); 
        digitalWrite(StatusBlue, HIGH); 
       }
          
      if(Color ==  "BLUEGREEN")
      {
        digitalWrite(StatusRed, HIGH); 
        digitalWrite(StatusGreen, LOW); 
        digitalWrite(StatusBlue, LOW); 
       }
      
      if(Color ==  "OFF")
      {
        digitalWrite(StatusRed, HIGH); 
        digitalWrite(StatusGreen, HIGH); 
        digitalWrite(StatusBlue, HIGH); 
        Serial.println("OFF");
       }
      
      if(Color ==  "ALL")
      {
        digitalWrite(StatusRed, LOW); 
        digitalWrite(StatusGreen, LOW); 
        digitalWrite(StatusBlue, LOW); 
       }
       
      if(Color ==  "WHITE")
      {
        digitalWrite(StatusRed, LOW); 
        digitalWrite(StatusGreen, LOW); 
        digitalWrite(StatusBlue, LOW); 
       }

     

  }



String getValue(String data, char separator, int index)
{
  int found = 0;
  int strIndex[] = {0, -1};
  int maxIndex = data.length()-1;

  for(int i=0; i<=maxIndex && found<=index; i++){
    if(data.charAt(i)==separator || i==maxIndex){
        found++;
        strIndex[0] = strIndex[1]+1;
        strIndex[1] = (i == maxIndex) ? i+1 : i;
    }
  }
  return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}

int ServoDegree(String Command, boolean Invert)
{
  //Get first command in string 
  String btnval = getValue(Command, '~', 0);
  //get number off of btnval
  String inChar = getValue(btnval,':',1);
  
  int deg = inChar.toInt();
  //Serial.println("was" + String(deg));
  if(Invert){
    int newdeg = (deg * -1) + 180;
    deg = newdeg;
    //Serial.println("now" + String(deg));
  }
  

  if(CenterServos)
  {
    deg = 90;
  }
  
  return deg;
}


/*
Networking Finctions:
//*/
void ConnectToWLAN(){
  
  // check for the presence of the shield:
  if (WiFi.status() == WL_NO_SHIELD) {
    Serial.println("WiFi shield not present"); 
    // don't continue:
    while(true);
  } 

  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 WPA SSID: ");
    Serial.println(ssid);
    // Connect to WPA/WPA2 network:    
    status = WiFi.begin(ssid, pass);

    // wait 10 seconds for connection:
    delay(10000);
  }
   
  // you're connected now, so print out the data:
  Serial.print("You're connected to the network");
  printCurrentNet();
  printWifiData();
}

//void PickNetwork() {
//  // scan for nearby networks:
//  Status("YELLOW");
//  Serial.println("** Scan Networks **");
//  int numSsid = WiFi.scanNetworks();
//  if (numSsid == -1)
//  { 
//    Serial.println("Couldn't get a wifi connection");
//    while(true)
//    {
//      Status("RED"); 
//      delay(1000);
//      Status("OFF");
//      delay(1000);
//    }
//  } 
//
//  // print the list of networks seen:
//  Serial.print("number of available networks:");
//  Serial.println(numSsid);
//  boolean foundssid = false;
//
//  // print the network number and name for each network found:
//  for (int thisNet = 0; thisNet<numSsid; thisNet++) {
//    Serial.print(thisNet);
//    Serial.print(") ");
//    Serial.print(WiFi.SSID(thisNet));
//    Serial.print("\tSignal: ");
//    Serial.print(WiFi.RSSI(thisNet));
//    Serial.print(" dBm");
//    Serial.print("\tEncryption: ");
//    //printEncryptionType(WiFi.encryptionType(thisNet));
//    
//    
//    if(String(WiFi.SSID(thisNet)) == "guest5") //Office
//    {
//      ssidString = "";    
//      passString = ""; 
//      StatusBlink("GREEN", 200, 10);
//      foundssid = true;
//      break;
//    }
//    else if(String(WiFi.SSID(thisNet)) == "RAWifi") //Home
//    {
//      ssidString = "";    
//      passString = ""; 
//      StatusBlink("VIOLET", 100, 20);
//      foundssid = true;
//      break;
//    }
//    else if(String(WiFi.SSID(thisNet)) == "thomasair") //Moms
//    {
//      ssidString = "";    
//      passString = ""; 
//      StatusBlink("GREEN", 200, 10);
//      foundssid = true;
//      break;
//    }
//    else if(String(WiFi.SSID(thisNet)) == "idunno") //Perrys
//    {
//      ssidString = "";    
//      passString = ""; 
//      StatusBlink("VIOLET", 100, 20);
//      foundssid = true;
//      break;
//    }
//    else //Not on list
//    {
//      char ssid[] = "";    
//      char pass[] = ""; 
//    }
//  }
//  
//  if(!foundssid)
//  {
//      while(true)
//      {
//        Status("YELLOW"); 
//        delay(1000);
//        Status("OFF");
//        delay(1000);
//      }
//  }
//}

void printCurrentNet() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print the MAC address of the router you're attached to:
  byte bssid[6];
  WiFi.BSSID(bssid);    
  Serial.print("BSSID: ");
  Serial.print(bssid[5],HEX);
  Serial.print(":");
  Serial.print(bssid[4],HEX);
  Serial.print(":");
  Serial.print(bssid[3],HEX);
  Serial.print(":");
  Serial.print(bssid[2],HEX);
  Serial.print(":");
  Serial.print(bssid[1],HEX);
  Serial.print(":");
  Serial.println(bssid[0],HEX);

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.println(rssi);

  // print the encryption type:
  byte encryption = WiFi.encryptionType();
  Serial.print("Encryption Type:");
  Serial.println(encryption,HEX);
  Serial.println();
}


void printWifiData() {
  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
    Serial.print("IP Address: ");
  Serial.println(ip);
  Serial.println(ip);
  
  // print your MAC address:
  byte mac[6];  
  WiFi.macAddress(mac);
  Serial.print("MAC address: ");
  Serial.print(mac[5],HEX);
  Serial.print(":");
  Serial.print(mac[4],HEX);
  Serial.print(":");
  Serial.print(mac[3],HEX);
  Serial.print(":");
  Serial.print(mac[2],HEX);
  Serial.print(":");
  Serial.print(mac[1],HEX);
  Serial.print(":");
  Serial.println(mac[0],HEX);
 
}