screen /dev/tty.usbmodem621 9600

void ptb(char c)
{
  //prints to SpiSerial (fills up the buffer).
  SpiSerial.print(c, BYTE);
}
void fb()
{
  //FLUSH BUFFER (sends buffer contents to host).
  //a carriage return character (hex 0xd) flushes the WIFLYs buffer and sends the msg
  //where FBC is defined as: char FBC = 0xd;  //carriage return.
  SpiSerial.print(FBC, BYTE);
}

SpiSerial.begin();

udp = new UDP( this, listenPort );  //port 6000
udp.listen( true );

void receive( byte[] data ) {
  //void receive( byte[] data, String ip, int port ) {   // <-- extended handler
  char[] mrTdata = new char[data.length]; //FOR CONVERTING BYTE TO CHAR. here is stored information coming from the arduino.
  String incomingDataString = ""; //makes a whole string out of data presently being received...
  String firstChar = "";
  String currentChar = "";
 
 
  for (int i=0; i < data.length; i++) {
    mrTdata[i] = char(data[i]);   //BYTE TO CHAR.
    incomingDataString = incomingDataString + mrTdata[i];  //added to whole string used for reference.
    firstChar = "" + mrTdata[0];   //store the first char for reference... "" appears to be necessary to force conversion from CHAR to STRING
    currentChar = "" + mrTdata[i];
 
    if(!currentChar.equals("1") && !currentChar.equals("2") && !currentChar.equals("3") && !currentChar.equals("4") && !currentChar.equals("5") && !currentChar.equals("6") && !currentChar.equals("7") && !currentChar.equals("8") && !currentChar.equals("9") && !currentChar.equals("0") && !currentChar.equals(".")) { 
      //the character is not a number, not a value to go along with a command,
      //so it is probably a command.
      if(instructionDataString != "") {
        aCommandVal = Integer.parseInt(instructionDataString);
      }
      if(aCommand != "") {
        delegate();
      }
      aCommand = currentChar;
      instructionDataString = "";
    } else {
      //in this case, we're probably receiving a command value.
      //store it
      instructionDataString = instructionDataString + currentChar;
    }
 
  }
}

void mapPS3mac()
{
  controlDevice = controll.getDevice("PLAYSTATION(R)3 Controller");
  printDeviceItems();   //prints a list of available controls.
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelMovementMin = 0.001;  //wheel has to be moved this much in order to calculate new value.
  wheelStickDeadZone = 0.01;    //stick neutral deadzone. If stick reading is within + or - this value, it's as good as in the centre.
  wheelHardValue = 0.93;  //if the stick is moved to the right or left this amplitude then we consider it moved left or right 100%
  wheelPOW = 1.0;  //I haven't developed this fully, tinker with caution. It is intended to add progressive steering.
  throttleStickDeadzone = 0.05; //throttle neutral deadzone.
  pedalDeadZone = 0.004;  //if the pedal reading is under this value then we consider it not-activated.
  pedalMovementMin = 0.004;  //minimim distance pedal must be moved to calculate a new value
  pedalHardValue = 0.95;    //if pedal is depressed more than this amount then we consider it fully activated...
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.07;
  cameraHardValue = 0.90;
  cameraMaxStep = 7;
  cameraMinStep = 4; 
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = false;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = false;
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //one stick, on the left, signifies both directions...
  hasReversePedal = false; //this controller does not have a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick. (as opposed to a wheel, like the G27)
  throttleIsStick = true; //make sure you use stick deadzones.
  hasCameraStick = false;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers, though I can't get them to work yet for the PS3 or Xbox360 controller.
  maxRumbleIntensity = 1.0;  //max rumble intensity...
  throttleBase = 2;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
  reverseBase = 0;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(2);
  sliderThrottle = controlDevice.getSlider(1);
  //sliderCameraX = controlDevice.getSlider(1);
  //buttons
  buttonStart = controlDevice.getButton(3);      //START BUTTON
  buttonESC = controlDevice.getButton(0);     //SELECT BUTTON
  buttonSendSettings = controlDevice.getButton(3);      //START BUTTON, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(0);      //SELECT BUTTON (clutch must be in for this one to work).
 
  buttonGEARDOWN = controlDevice.getButton(10);       //LTB
  buttonGEARUP = controlDevice.getButton(11);        //RTB
  buttonCLUTCH = controlDevice.getButton(2);    //RJD
 
  buttonCentreCamera = controlDevice.getButton(1);    //LJD (depress to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton(14);    //X. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(8);       //LEFT PADDLE
  buttonCameraMoveRight = controlDevice.getButton(9);        //RIGHT PADDLE
 
  buttonCalibrateSensors = controlDevice.getButton(12);        //TRIANGLE  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(12);        //TRIANGLE (when clutch is NOT in).
 
  //cooliehat
  //broken into buttons for this controller.
  //(left arrow pad)
  coolieUP = controlDevice.getButton(4);    //cooliehat UP
  coolieDOWN = controlDevice.getButton(6);    //cooliehat DOWN
  coolieLEFT = controlDevice.getButton(7);    //cooliehat LEFT
  coolieRIGHT = controlDevice.getButton(5);    //cooliehat RIGHT 
}

//Arduino WIFLY Mini
//now with FPV
//Two-Way Communications
//Direct SPI-UART manipulation.
//
//ADDED ACCELEROMETER FEEDBACK, REMOVED pot reading/logging, DUE TO MEMORY ISSUES.
//removed complicated braking functions.
//simplified communications method, now transmissions to (and from) the wifly are bundled, to more resemble a PPM stream
float AWMV = 1.110;   //arduino wifly mini version (AWMV).
 
#include "WiFly.h" // We use this for the preinstantiated SpiSerial object.
#include <Servo.h>
 
//WIFLY SETTINGS:
boolean performSetup = false; //should we set these settings? or skip them?
boolean requireFirstContact = false; // do we need to make first contact with AP before entering main loop?
//String hostIP = "192.168.1.4";  //mbp, wireless, Hitch
String hostIP = "192.168.1.6";  //Blair WIRED, Hitch
String ssid = "Hitch";            //WRT54GL
String phrase = "TwentyTwo";
String listenPort = "8888";
String sendPort = "6000";
 
//ARDUINO SETTINGS AND VARIABLES:
//PIN DEFINITIONS
const int potPin = A0;    //potentiometer input pin.
const int pinAccel1X = A1;    //accelerometer #1 Xplane output.
const int pinAccel1Y = A2;    //accelerometer #1 Yplane output.
const int pinAccel1Z = A3;    //accelerometer #1 Zplane output.
const int pinInfoLED = 8; //pin on which information LED is found.
const int pinOpto = 4; //opto-isolator pin, for turning on ESC.
const int pinThrottle = 5; //throttle/drive output
const int pinSteer = 6; //the pin that writes location to steering servo.
//SERVOS
Servo theCamera;  //this variable references the steering servo.
Servo theWheel;  //this variable references the steering servo.
Servo theGas; //the variable for referencing the ESC.
//SERVO-AT-REST SETTINGS
int cameraCentre = 90; //default centre position of camera servo.
int wheelCentre = 92; //default wheel center. Official value comes from processing.
//SERVO RANGE SETTINGS (Movement, Allowances)
//steering
int steeringRange = 8; //default steering range.
int middleRange = 4;   //how far on either side of wheelCentre will the arduino correct for a constant feedback from the steering servo pot?
int autoCameraRange = 50; //default movement degrees either side of center for camera servo.
//throttle
int throttleNeutral = 50; //no throttle. SAFETY VALUE.
int throttleFullReverse = 30;  //be careful with this.
int throttleSetting = throttleNeutral; //this is for Arduino's reference when error handling, etc.
//camera
//int cMMdelay = 3300; //cameraMoveManualDelay, delay in milliseconds before arduino reverts back to auto camera movement, if set.
//unsigned long cMMtimeout = millis();
//
//SERVO Bearing/Setting Memory and Variables
//steering
//int cBearing; //this is the currently-set steering servo bearing.
//int sBearing; //this is the setting of the servo guessed at by the arduino; a rough estimate.
//unsigned long sStime;   //steering servo time; if millis is beyond this, then tick the servo one degree toward cBearing, then reset.
//unsigned long oldBearing;   //when this is up, then the bearing is considered old. move back to strict pot threshold.
//FORCE FEEDBACK: Settings, Values, Ranges, Allowances, Variables
unsigned long accelCheckTime;
int accelCheckDelay = 33; //milliseconds between accel checks.
int accel1counter = 0;  //keeps track of where we are in the accel1 average arrays, used for determining what the average of the last 100 readings have been.
const int accelAVGarraySize = 12;
int accel1avgXarray[accelAVGarraySize];
int accel1avgYarray[accelAVGarraySize];
int accel1avgZarray[accelAVGarraySize];
boolean FFaccelsCalibrated = false; //are the accelerometers calibrated?
boolean FFsendACCELDATA = false;   //should the arduino check the ACCELEROMETER(s) and send back information to the driver?
int minAccelFFamplitudeX = 27; //default sent by host at calibrate sensors (or send thresholds).
int minAccelFFamplitudeY = 5; //default... sent again by host at calibrate sensors.(or send thresholds).
int minAccelFFamplitudeZ = 27; //default... sent again by host at calibrate sensors.(or send thresholds).
int lastAXdifference = 0;  //what was the last accelerometer difference?
int lastAYdifference = 0;  //what was the last accelerometer difference?
int lastAZdifference = 0;  //what was the last accelerometer difference?
int lastAXFFresult = 0; //what was the last aff result?
int lastAYFFresult = 0; //what was the last aff result?
int lastAZFFresult = 0; //what was the last aff result?
//WIFLY COMMUNICATION WITH ARDUINO
//heartbeat
int heartRate = 888; //heartbeat delay in milliseconds.
unsigned long deadBeat; //this is theTime plus the heartRate delay. If no HB signal is received before deadBeat time is up, consider connection lost and go into errorHandling.
unsigned long flatlinedAt;
unsigned long timeToWiflyReset;
int flatlineWiflyResetDelay = 27000;
boolean resetAfterFlatline = false;
//register availability (applies to specific functions, to stop overflowing the buffer). 
boolean rCRV = false; // this determines if certain functions are allowed to send data to the SPI register.
unsigned long cSW; //time until another variable can be sent to the register.
int cSD = 8; //the delay, in milliseconds, before another character can be sent to the SPI register.
//states
boolean flatline = false;
int wFFC = 0; //wait for first connection, counter.
boolean icM = false; //Initial Connection Made? Have we connected to an AP?
boolean commandMode = false;   //is the Wifly presently in command mode?
boolean started = false; //did we receive the start command, and are now started? (will change if connection is lost or started is received again, or if there's an error.).
boolean escOn = false; //same as started, but for ESC. Is it on?
boolean autoCamera = false; //does the camera movement follow the steering?
boolean settingsReceived = false;
boolean autoCameraProportional = false;
//data
int sCS = 0; //settings checksum.
char newInsCharArray[20]; //new instruction char array... stores the instruction.
int iC = 0; //instruction counter, for the newInsCharArray (new instruction char array).
String newInstruction;
int cIs = 0; //is the new command an integer, a no value command, or a float? (0,1,2 respectively).
//command counter variables
//I use these to count certain instructions I send more than once to activate.
//I do this when certain functions get set accidently by extraneous data coming
//from the SPI uart.
int CCraf = 0;   //command counter, reset after flatline.
//TIME
unsigned long theTime = millis(); //this will be equated to millis();
 
 
 
 
 
 
 
 
/////////////////////////////////////////////
//                                         //
//          Arduino Setup                  //
//                                         //
/////////////////////////////////////////////
//Setup Routine:
void setup() {
 Serial.begin(9600);
 //Serial.println(" ");
 Serial.print("WIFLY Arduino Mini v");
 Serial.print(AWMV);
 Serial.println(" ");
 Serial.println("--------------------------------------");  
 Serial.println("Initialize SpiSerial.");
 SpiSerial.begin();
 Serial.println("Connected to SPI UART.");
 Serial.println();
 
  //pinMode(pinOpto1, OUTPUT); //SERVO opto-isolator pin set to OUTPUT
  pinMode(pinOpto, OUTPUT); //ESC opto-isolator pin set to OUTPUT
  //digitalWrite(pinOpto1, LOW); //turn off SERVOS
  digitalWrite(pinOpto, LOW); //turn off ESC
 
  pinMode(pinInfoLED, OUTPUT); //information LED pin set to OUTPUT.
  digitalWrite(pinInfoLED, LOW); //information LED off.
 
  theCamera.attach(3);  //associates theCamera with indicated pin.
  theGas.attach(5);  //associates theGas with indicated pin.
  theWheel.attach(6);  //associates theWheel with indicated pin.
 
  theCamera.write(cameraCentre);
  theWheel.write(wheelCentre);
  theGas.write(throttleNeutral);
  //sBearing = wheelCentre; //initialize sBearing
 
  if(requireFirstContact == true) {
    icM = false;
  } else {
    icM = true;
  }
 
  if(performSetup == true) {
    //configure wifly
    configureWifly();
  } else {
    //do nothing
  }
 
  //Serial.println("DONE ARDUINO SETUP");   
}
 
 
 
 
 
 
 
 
 
 
 
 
 
 
//////////////////////////////////////
//                                  //
//          ERROR HANDLING          //
//                                  //
//////////////////////////////////////
 
void errorHandle(String theError)
{
  //do nothing for the time being.
  if(theError == "flatline") {
    if(throttleSetting > (throttleNeutral + 10)) {
      throttle(throttleFullReverse);
      delay(777);
    }
    neutral();
    if(escOn == true) {
      ESC();
    }
    if(started == true) {
      start();
      flatline = true;
      flatlinedAt = millis();
      timeToWiflyReset = flatlinedAt + flatlineWiflyResetDelay;
      Serial.println("Flatlined!");
    }
 
  }
}
 
 
 
 
 
 
 
 
 
////////////////////////////////////////////////////
//                                                //
//     Update Time Sensitive Functions (uTSF)     //
//     (Sensor checks, etc)                       //
//                                                //
////////////////////////////////////////////////////
void checkACCEL1() {
  theTime = millis();
  if(theTime > accelCheckTime) {
     int accelX = analogRead(pinAccel1X);    // read accel's X value
     int accelY = analogRead(pinAccel1Y);    // read accel's Y value
     int accelZ = analogRead(pinAccel1Z);    // read accel's Z value
     //minAccelFFamplitude
     int accelXaverage = 0;
     int accelYaverage = 0;
     int accelZaverage = 0;
     accel1avgXarray[accel1counter] = accelX; //put X reading into array
     accel1avgYarray[accel1counter] = accelY; //put Y reading into array
     accel1avgZarray[accel1counter] = accelZ; //put Z reading into array
     accel1counter++;
     if(accel1counter > accelAVGarraySize) {
       accel1counter = 0;
     }
     for(int a = 0; a<accelAVGarraySize; a++) {
         //add together, to update average...
         accelXaverage = accelXaverage + accel1avgXarray[a];
         accelYaverage = accelYaverage + accel1avgYarray[a];
         accelZaverage = accelZaverage + accel1avgZarray[a];
     }
     accelXaverage = accelXaverage / accelAVGarraySize;
     accelYaverage = accelYaverage / accelAVGarraySize;
     accelZaverage = accelZaverage / accelAVGarraySize;
 
     //now find out if there's enough of a disturbance to report...
     int aXdifference = 0;
     int aYdifference = 0;
     int aZdifference = 0;
     //determine if X reading is out of range
     int accelUnder = accelXaverage - minAccelFFamplitudeX;
     int accelOver = accelXaverage + minAccelFFamplitudeX;
     if(accelX <= accelUnder) {
       aXdifference = accelXaverage - accelX;
     } else if (accelX >= accelOver) {
       aXdifference = accelX - accelXaverage;
     }
     //determine if Y reading is out of range
     accelUnder = accelYaverage - minAccelFFamplitudeY;
     accelOver = accelYaverage + minAccelFFamplitudeY;
     if(accelY <= accelUnder) {
       aYdifference = accelYaverage - accelY;
     } else if (accelY >= accelOver) {
       aYdifference = accelY - accelYaverage;
     }
     //determine if Z reading is out of range
     accelUnder = accelZaverage - minAccelFFamplitudeZ;
     accelOver = accelZaverage + minAccelFFamplitudeZ;
     if(accelZ <= accelUnder) {
       aZdifference = accelZaverage - accelZ;
     } else if (accelZ >= accelOver) {
       aZdifference = accelZ - accelZaverage;
     }
 
     //Serial.print("accelZ: ");
     //Serial.println(accelZ);
     //delay(500);
 
     boolean bufferLoaded = false;
     //X
     if(aXdifference != lastAXdifference) {
       //the affXresult has changed, send to host.
       ptb(0x3C); //print "<" to spi uart, for X
       wSPIintToChar(aXdifference);
       bufferLoaded = true;
       lastAXdifference = aXdifference;
     }
     //Y
     if(aYdifference != lastAYdifference) {
       //the affZresult has changed, send to host.
       ptb(0x3E); //print ">", for Y
       wSPIintToChar(aYdifference);
       bufferLoaded = true;
       lastAYdifference = aYdifference;
     }
     //Z
     if(aZdifference != lastAZdifference) {
       //the affZresult has changed, send to host.
       ptb(0x5E); //print "^", for Z.
       wSPIintToChar(aZdifference);
       bufferLoaded = true;
       //Serial.println(aZdifference);
       lastAZdifference = aZdifference;
     }
     if(bufferLoaded) {
       fb(); //flush buffer
     }
     accelCheckTime = theTime + accelCheckDelay;
  }
}
void heartbeat()
{
  theTime = millis();
  deadBeat = theTime + heartRate;
  //Serial.println("BEAT");
}
void checkHeart() 
{
  if(started == true) {
    theTime = millis();
    if(theTime > deadBeat) {
      errorHandle("flatline");  
    }
  } else {
    //started is not equal to true.
    //did we flatline?
    if(flatline == true) {
        theTime = millis();
        if((theTime > timeToWiflyReset) && resetAfterFlatline) {
          //in development...
          Serial.println("Attempting to resuscitate connection...");
          wiflyReconnectToAP();
          //wiflySuddenReboot();
        }
    }
  }
}
void uTSF()
{
  checkHeart();
  if(FFsendACCELDATA && FFaccelsCalibrated) {
    checkACCEL1();
  }
}
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
/////////////////////////////////////////////
//                                         //
//            DATA MANIPULATION            //
//                                         //
/////////////////////////////////////////////
 
//delegate received command.
void delegate(String wtd, char* tVc) {  //tVc = the float char
  float tVf = 0.0;  //the value, float
  int tV = 0;     //integer
 
  if(cIs == 0) {
    //the value needs ot be an integer. most cases.
    tV = atoi(tVc);  //turn char array into an integer.
  } else if (cIs == 1) {
    //there is no accompanying value
    //do nothing.
  } else if (cIs == 2) {
    //the value must be a float
    tVf = atof(tVc); //turn char array into a float.
  }
  //most important first:
  if(wtd == "B") {
      //first tier.
      heartbeat();
      //Serial.println("Heartbeat!");
  } else {  //so, not a heartbeat.
    //second tier.
    if(wtd == "W") {
      //steer
      wheel(tV);
    } else if (wtd == "T" && escOn == true) {
      //throttle
      throttle(tV);
    } else if(wtd == "L") {
      //if(autoCamera) {
        //theTime = millis();
        //cMMtimeout = theTime + cMMdelay; //camera manual move timeout, theTime offset by camera manual move delay.
      //}
      if(!autoCamera) {
        camera(tV);
      }
    } else { 
      //third tier.
      if (wtd == "A") {
        ESC();
      } else if(wtd == "S") {
        start();
      } else if (wtd == "R") {
        //set the steering range.
        //Serial.println("SET STEERING RANGE");
        steeringRange = tV;
        sCS++;
        //Serial.print(steeringRange);
        //Serial.println("");
      } else if (wtd == "V") {
        //set the wheelCentre
        //Serial.println("SET WHEEL CENTRE");
        wheelCentre = tV;
        sCS++;
        //Serial.print(wheelCentre);
        //Serial.println("");
      } else if (wtd == "h") {
        Serial.print("autoCamera: ");
        if(!autoCamera && settingsReceived) {
          autoCamera = true;
          ptb(0x68);  //h
          ptb(0x31);  //indicate on.
          fb();
          Serial.println("ON");
        } else {
          autoCamera = false;
          ptb(0x68);  //h
          ptb(0x30);  //indicate off.
          fb();
          Serial.println("OFF");
        }
      } else if (wtd == "a") {
        Serial.println("SET autoCameraRange: ");
        autoCameraRange = tV;
        sCS++;
        Serial.println(cameraCentre);
      } else if (wtd == "l") {
        Serial.println("SET Camera Centre: ");
        sCS++;
        cameraCentre = tV;
        Serial.println(cameraCentre);
      } else if (wtd == "x") {
        //Serial.println("SET ACCELEROMETER MINIMUM FF AMPLITUDE THRESHOLD X");
        minAccelFFamplitudeX = tV;
        sCS++;
        //Serial.println(minAccelFFamplitudeX);
      } else if (wtd == "y") {
        //Serial.println("SET ACCELEROMETER MINIMUM FF AMPLITUDE THRESHOLD Y");
        minAccelFFamplitudeY = tV;
        sCS++;
        //Serial.println(minAccelFFamplitudeY);
      } else if (wtd == "z") {
        //Serial.println("SET ACCELEROMETER MINIMUM FF AMPLITUDE THRESHOLD Z");
        minAccelFFamplitudeZ = tV;
        sCS++;
        //Serial.println(minAccelFFamplitudeZ);
      } else if (wtd == "~" && started == false) {
        //calibrate accelerometer rest ranges
        if(!started) {
          Serial.println("Calibrating Accelerometer");
          calibrateAccelerometer();
        }
      } else if (wtd == "M") {
        //we want pots data.
        Serial.println("Settings checksum...");
        if(tV == 0) {
          Serial.println("Cleared.");
          sCS = tV;
        } else if (tV > 0) {
          Serial.print("Comparing, sending to HOST. sCS = ");
          Serial.println(tV);
          ptb(0x4D);  //M
          wSPIintToChar(sCS);
          fb();
        }
      } else if (wtd == "m") {
        //settings checksum checked out.
        Serial.println("Checksum Confirmed. Settings Received.");
        settingsReceived = true;
      } else if (wtd == "X") {
        //we want acceleration data.
        if(FFsendACCELDATA && FFaccelsCalibrated) {
          FFsendACCELDATA = false;
          //send ACK
          ptb(0x58);  //X
          ptb(0x30);  //indicate off.
          fb();
        } else if (FFaccelsCalibrated) {
          Serial.println("SEND FF ACCELERATION DATA");
          FFsendACCELDATA = true;
          //send ACK
          ptb(0x58);  //X
          ptb(0x31); //indicate on.
          fb();
        }
      } else if (wtd == "Z" && started == false) {
        //set pot defaults.
        if(CCraf < 2) {
          CCraf++;
        } else {
          CCraf = 0;
          if(resetAfterFlatline) {
            resetAfterFlatline = false;
            ptb(0x5A); //Z
            ptb(0x30); //indicate off.
            fb();
          } else if (!resetAfterFlatline) {
            resetAfterFlatline = true;
            ptb(0x5A); //Z
            ptb(0x31); //indicate on.
            fb();
          }
        }
      } else {
        //clear counted commands to avoid unexpected results
        CCraf = 0; 
        //CCpotvals = 0;
      }
    }
  }  //end of not a heartbeat if
 
  //Serial.print(wtd);
  //Serial.println(tV);
}
//interpret received data
void interpret(char c)
{
  String iD = (String)c; //incoming data equated to c.
  //Serial.println(iD);
  if((iD != "0") && (iD != "1") && (iD != "2") && (iD != "3") && (iD != "4") && (iD != "5") && (iD != "6") && (iD != "7") && (iD != "8") && (iD != "9") && (iD != ".")) {
    if(newInstruction != "") {
      delegate(newInstruction, newInsCharArray);    //send instruction to be delegated.
      //Serial.println(newInstruction);
    }
    newInstruction = iD;
    cIs = 1;  
    iC = 0;                         //set instruction counter to zero.
    memset(newInsCharArray,0,sizeof(newInsCharArray)); //clear char array.
  } else if (newInstruction != "") {
    //accumulate contents/details of new instruction.
    newInsCharArray[iC] = c;
    iC++;
    //there are accompanying values, set cls to 0
    if(cIs == 1) {
      cIs = 0;
    }
    if(cIs == 1 && iD == ".") {
      //accompanying value will be a float
      cIs = 2;
    }
    //Serial.print(c);
  } else {
    //something's happened that shouldn't have.
    Serial.println("ComSeqErr");
    errorHandle("ComSeqErr");
  }
 
}
void wSPIintToChar(int tpd) {
  if(rCRV == true) {
    char tpdc[12];  //was six
    itoa(tpd, tpdc, 10);  //turn the integer into an ascii string/char array.
    int tpdcl = strlen(tpdc);  //how long is the string?
    for(int ttt = 0; ttt < tpdcl; ttt++) {
      //Serial.println(tpdc[ttt]);
      ptb(tpdc[ttt]);
    }
  }
}
 
 
 
 
 
 
 
 
 
/////////////////////////////////////////////////////////////
//                                                         //
//                    COMMAND   FUNCTIONS                  //
//                                                         //
/////////////////////////////////////////////////////////////
void neutral()
{
  throttle(throttleNeutral);
}
void ESC() {
    //turn on the ESC or turn it off.
    if(escOn == false && started == true) {
      ptb(0x41);
      digitalWrite(pinOpto, HIGH); //ESC ON
      escOn = true;
      Serial.println("ESC is on");
    } else if(escOn == true) {
      ptb(0x61);
      digitalWrite(pinOpto, LOW); //ESC OFF
      escOn = false;
      Serial.println("ESC is off");
    }
    fb(); //flush buffer
}
void camera(int dir)
{
  theCamera.write(dir);
}
void wheel(int bearing)
{
  theWheel.write(bearing);
 
  //cBearing = bearing;
  if(autoCamera) {
    //theTime = millis();
    //if(theTime > cMMtimeout) {
      float cameraPoint = 0.0;
      int cameraPointSet = 0;
      float percentToMove = 0.0;
      if(bearing < wheelCentre) {
        //turning left
        percentToMove = ((float)wheelCentre - (float)bearing) / (float)steeringRange;
        if(autoCameraProportional) {
          percentToMove = pow(percentToMove,2);
        } else {
          //do nothing.
        }
        cameraPoint = (float)cameraCentre + ((float)autoCameraRange * percentToMove);
      } else if (bearing > wheelCentre) {
        percentToMove = ((float)bearing - (float)wheelCentre) / (float)steeringRange;
        if(autoCameraProportional) {
          percentToMove = pow(percentToMove,3);
        } else {
          //do nothing.
        }
        cameraPoint = (float)cameraCentre - ((float)autoCameraRange * percentToMove);
      } else {
        //camera centred.
        cameraPoint = (float)cameraCentre;
      }
      cameraPointSet = cameraPoint; //float to int.
      //Serial.print("cameraPointSet: ");
      //Serial.println(cameraPointSet);
      camera(cameraPointSet);
    //}
  }
 
}
void throttle(int power)
{
  throttleSetting = power;
  theGas.write(power);
}
void start() {
    if(started == false) {
      if(settingsReceived) {
        //start up, send ACK.
        started = true;
        ptb(0x53);
        Serial.println("STARTED");
        heartbeat(); //start the heart.
        flatline = false;
      } else {
        //request user send settings
        ptb(0x70);  //ask for settings ("p");
        fb();       //flush buffer
      }
    } else if (started == true) {
      //shut down, send ACK.
      started = false;
      ptb(0x73);
      digitalWrite(pinOpto, LOW); //ESC OFF
      escOn = false;
      Serial.println("STOPPED");
    }
    fb();
}
void calibrateAccelerometer() {
  //set accelerometer at-rest values...
  int theXaverage = 0;
  int theYaverage = 0;
  int theZaverage = 0;
  for(int a = 0; a < accelAVGarraySize; a++) {
    //initialize array
    accel1avgXarray[a] = analogRead(pinAccel1X);
    theXaverage = theXaverage + accel1avgXarray[a];
    accel1avgYarray[a] = analogRead(pinAccel1Y);
    theYaverage = theYaverage + accel1avgYarray[a];
    accel1avgZarray[a] = analogRead(pinAccel1Z);
    theZaverage = theZaverage + accel1avgZarray[a];
 
    boolean showme = false;
    if(showme) {
      Serial.print("accel1avgXarray[");
      Serial.print(a);
      Serial.print("] = ");
      Serial.print(accel1avgXarray[a]);
 
      Serial.print(", accel1avgYarray[");
      Serial.print(a);
      Serial.print("] = ");
      Serial.print(accel1avgYarray[a]);
 
      Serial.print(", accel1avgZarray[");
      Serial.print(a);
      Serial.print("] = ");
      Serial.println(accel1avgZarray[a]);
    }
 
    delay(15);
  }
  Serial.println("BEFORE DIVISION:");
  Serial.print("theXaverage = ");
  Serial.print(theXaverage);
  Serial.print(", theYaverage = ");
  Serial.print(theYaverage);
  Serial.print(", theZaverage = ");
  Serial.println(theZaverage);
 
 
  theXaverage = theXaverage / accelAVGarraySize;
  theYaverage = theYaverage / accelAVGarraySize;
  theZaverage = theZaverage / accelAVGarraySize;
 
  Serial.println("AFTER DIVISION:");
  Serial.print("theXaverage = ");
  Serial.print(theXaverage);
  Serial.print(", theYaverage = ");
  Serial.print(theYaverage);
  Serial.print(", theZaverage = ");
  Serial.println(theZaverage);
 
  ptb(0x47);  //tell homebase calibration is complete.
  fb();       //flush buffer
 
  FFaccelsCalibrated = true;
}
 
 
 
 
 
 
 
 
/////////////////////////////////////////////
//                                         //
//        WIFLY  GENERAL COMMANDS          //
//                                         //
/////////////////////////////////////////////
//general command definitions:
char EC = 0x0d;  //(carriage return for entering commands)
char ED = 0x24;  //$
char FBC = 0xd;  //flush buffer command, carriage return.
//general command functions:
void exCom() {
  //execute command on WIFLY when in Command Mode.
 SpiSerial.print(EC, BYTE);  //execute command 
}
void fb()
{
  //FLUSH BUFFER (sends buffer contents to host).
  //a carriage return character (hex 0xd) flushes the WIFLYs buffer and sends the msg
  theTime = millis();
  if(theTime > cSW) {
    //it's ok to send now.
    SpiSerial.print(FBC, BYTE);
    rCRV = false; //register can no longer receive values.
    //set wait period!
    cSW = theTime + cSD;
  }
}
void stSPI(String s) {
  SpiSerial.print(s); //print string s to SPI serial
}
void enterCommandMode()
{ 
  for(int i = 0; i <= 2; i++) {
     SpiSerial.print(ED, BYTE);
  }
  rpSPI();
  commandMode = true;
}
void quitCommandMode()
{
  stSPI("exit"); //Exit Command Mode
  exCom();  //execute command
  delay(333);
  rpSPI();
  commandMode = false;
}
void rebootWIFLY() {
  Serial.println("Rebooting...");
  delay(1333);
  //this is performed when already in command mode
  stSPI("reboot"); //Reboot command
  exCom();  //execute command 
  delay(2000);
  commandMode = false;
}
void wiflySuddenReboot()
{
  //performed if not already in command mode.
  enterCommandMode();
  delay(666);
  exCom();  //execute command, in case there's anything there...
  rebootWIFLY();
  rpSPI();
  delay(666);
}
void wiflyReconnectToAP()
{
  //performed if not already in command mode.
  enterCommandMode();
  delay(500);
  stSPI(" ");
  exCom();  //execute command, in case there's anything there...
  delay(333);
  rpSPI();
  stSPI("leave");
  exCom();  //execute command
  delay(3500);
  rpSPI();
  stSPI("join");
  delay(3500);
  rpSPI();
  delay(3500);
  rpSPI();
  quitCommandMode();
  quitCommandMode();
  delay(666);
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  rpSPI();
  theTime = millis();
  timeToWiflyReset = theTime + flatlineWiflyResetDelay;
}
 
 
 
/////////////////////////////////////////////
//                                         //
//         WIFLY SETUP and STATUS          //
//                                         //
/////////////////////////////////////////////
//wifly status manipulation/checking
void waitForFirstConnection()
{
  if(commandMode) {
    String cm = "show connection";
    SpiSerial.print(cm); //show connection status
    exCom();
    delay(666);
    int responseLength = 3; //number of digits of the response minus one, since 0 is counted.
    int i = 0;
    int ii = 0;
    int iStart = cm.length() + 3;
    int iEnd = iStart + 3;
    //Serial.println(iStart);
    //Serial.println(iEnd);
    char c;
    char w[8];
    while(SpiSerial.available() > 0) {
      c = SpiSerial.read();
      if((i>=iStart) && (i<=iEnd)) {
        w[ii] = c;
        ii++;
      }
      i++;
    }
    w[4] = 0x0; //nullify the last character in the string so the comp knows where the end of the string is.
    String iString1 = w;
    if(iString1 == "8130") {
     Serial.println("First AP Association Established. Quitting command mode...");
     quitCommandMode();
     Serial.println(" ");
     Serial.println("Entering Main Loop.");
     icM = true;
    }
    rpSPI();
    delay(1111);
  } else {
    enterCommandMode();
    delay(1111);
    rpSPI();
  }
}
void configureWifly()
{
    delay(333);
    Serial.println("Configuring Wifly...");
    //SET UP WIFLY
 
    //enter command mode (prints $$$).
    enterCommandMode();
 
    rpSPI();
 
    stSPI(" ");   //purposely sabotage the command line, then
    exCom();      //execute command
                  //that would happen if the arduino was rebooted before it had finished issuing commands to WIFLY
 
    //set dhcp mode
    stSPI("set ip dchp 1"); //set DHCP mode
    exCom();      //execute command
 
    rpSPI();
 
    //set the SSID
    stSPI("set wlan ssid "); //set ssid
    stSPI(ssid); //set ssid
    exCom();      //execute command
 
    rpSPI();
 
    stSPI("set wlan phrase "); //set password
    stSPI(phrase); //set ssid
    exCom();      //execute command
 
    rpSPI();
 
    stSPI("set ip host "); //set the host IP address.
    stSPI(hostIP);
    exCom();      //execute command
 
    rpSPI();
 
    stSPI("set ip localport "); //set the listen port
    stSPI(listenPort);
    exCom();      //execute command
 
    rpSPI();
 
    stSPI("set ip remote "); //set the send port
    stSPI(sendPort);
    exCom();      //execute command
 
    rpSPI();
 
    stSPI("save"); //save settings
    exCom();      //execute command
 
    rpSPI();
 
    Serial.println(" ");
    Serial.println("Configured, Saved, Now Reboot:");
    Serial.println(" ");
 
    //reboot the wifly. this function has a delay set within.
    rebootWIFLY();
    rpSPI();
    Serial.println("Wifly is Ready");
}
 
 
 
 
 
///////////////////////////////////////////
//                                       //
//       WIFLY SPI COMMUNICATION         //
//                                       //
//                                       //
///////////////////////////////////////////
void rpSPI()
{
 delay(500);
 while(SpiSerial.available() > 0) {
    Serial.print(SpiSerial.read(), BYTE);
  }
  Serial.println(" ");
}
void ptb(char c)
{
  //prints to SpiSerial (fills up the buffer).
  theTime = millis();
  if(theTime > cSW) {
    if((c == 0x3C) || (c == 0x3E) || (c == 0x5E) || (c==0x4D) || (c==0x4E) || (c==0x68) || (c==0x46)) {
      //we have received a command that requires a value to be sent.
      //therefore, it is ok for the register to receive values.
      //it's MEANT FOR spiIntToChar function... so we don't issue values to the register if there's no representing character.
      rCRV = true;
    }
    //0x4b = K. 0x41 = A. 0x61 = a. 0x53 = S. 0x73 = s.
    //char c = 0x4b; 
    SpiSerial.print(c, BYTE);
  }
}
void listenForCommands()
{
  //read by arduino when receiving commands.
  while(SpiSerial.available() > 0) {
    char c = SpiSerial.read();
    //Serial.print(c);
    interpret(c);  //send char to the interpret function.
  }
}
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
///////////////
//   LOOP ////
//////////////
 
void loop() 
{ 
  if(icM) {
    listenForCommands(); //Listen for commands from Host received by Wifly.
    uTSF(); //run functions that update time-sensitive variables...
  } else {
    waitForFirstConnection();
  }
}

//WIFLY Mini Multicontroller Processing Sketch
//VERSION 1.27
//Controls WIFLY Mini with Playstation 3 Controller (Mac OS X), XBOX 360 Controller (Mac OS X & Windows 7),
//and Logitech G27 Steering Wheel (Windows 7), Logitech F510 Rumblepad (Windows, both Dinput and Xinput modes).
 
//IMPORT STATEMENTS
import hypermedia.net.*;
import procontroll.*;
import java.io.*;
 
//VARIABLES
//CONTROLLER IN USE:
int usingController = 1;  //0 = PS3 for Mac, 
                          //1 = Xbox360 for Windows 7, 2 = Xbox360 controller for MacOS
                          //3 = G27 Steering Wheel for Windows 7
                          //4 = Logitech F510 rumblepad, xInput, 5=Same controller, directInput
//STEERING
int wheelCentreDefault = 96; //was 96
float wheelMaxTravelDefault = 34.0; //was 36. 42 is the max so far. is a float value because it is multiplied by a float value.
int wheelCentre = wheelCentreDefault;
float wheelMaxTravel = wheelMaxTravelDefault;
int steeringRange = (int)wheelMaxTravel;  //just wheelmaxtravel in integer form.
int steeringLevel = 0; //indicates steering range/tier... wide/narrow.
int wheelSetting = wheelCentreDefault;
int lastWheelSetting = wheelCentreDefault;
float wheelLastReading = 0;
float wheelCurrentReading = 0;
float wheelSettingFloat;
float wheelMovementMin; //the minimum change the wheel must experience to recalculate new instruction. SET IN CONTROLLER MAP
float wheelHardValue;  //Set in controllmap. if under this (when reading is negative) or over this (when reading is positive) value, go hard left or right.
float wheelStickDeadZone;    //if using a stick, this (negative and postive) are boundaries for wheelCentre to activate. set in controller map
float wheelDeadZone;    // deadzone for wheel. set in controller map
float wheelPOW;  //proportional steering setting of wheel... sometimes useful with stick steering. set in controller maps.
//THROTTLE AND BRAKE
float throttleMax = 120;
float numberOfGears = 6;
float throttleForMaxDefault = (throttleMax / 6);   //120 max.
float throttleRevMaxDefault = 20;  //20 max.
float throttleMaxForward = throttleForMaxDefault;
float throttleMaxReverse = throttleRevMaxDefault / 2;
int throttleNeutral = 50;  //don't touch! (set at 50!)
float throttleCurrentReading = 0;
float throttleLastReading = 0;
float throttleSettingFloat = 0;
int throttleSetting = throttleNeutral;
int lastThrottleSetting = 0;
float throttleStickDeadzone; //set in controllermaps. what's the deadzone when the throttle is a stick?
float reverseCurrentReading = 0;
float pedalDeadZone; //if reading is less than this, pedal is considered not engaged. set in controller map
float pedalMovementMin; //set in control maps
float pedalHardValue; //set in control maps.
//CLUTCH
float clutchCurrentReading;
float clutchThrow; //set in controller map. percentage sliding clutch must be depressed to activate.
//CAMERA
boolean flipCameraStickX = true;  //do you want to flip the x reading of the camera control stick, if there is one?
int cameraCentre = 97;  //in this case higher is more to the left...
float cameraMaxLeft = 176.0;
float cameraMaxRight = 4.0;
int cameraSetting = cameraCentre;
float cameraLastReading = 0;
float cameraCurrentReading = 0;
float cameraSettingFloat;
float cameraLeftRange = (float)cameraCentre - cameraMaxLeft;
float cameraRightRange = cameraMaxRight - (float)cameraCentre;
int cameraStep; //the movement step size the camera's servo will take.
long cameraMoveTime = 0;
int cameraMoveInterval; //number of milliseconds between each camera step. set in controller map.
float cameraMovementMin; //set in controller map. if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
float cameraHardValue; //set in controllermap
float cameraStickDeadZone;  //set in controller map.
float cameraMaxStep; //set in controller map.
float cameraMinStep; //set in controller map.
int autoCameraRange = 27; //how far the camera will travel when autocamera is on. (follows steering stick).
boolean autocameraOn = false;
//FORCE FEEDBACK AND OTHER SENSORS
boolean rumbling = false; //this needs to be made better... are any of the rumblers going?
boolean rumbling0 = false; //this needs to be made better... represents rumbler number one. Is it rumbling or not?
boolean rumbling1 = false; //this needs to be made better... represents rumbler number one. Is it rumbling or not?
boolean rumbling2 = false; //this needs to be made better... represents rumbler number one. Is it rumbling or not?
float rumble0val = 0.0;
float rumble1val = 0.0;
float rumble2val = 0.0;
long timeFFreduce;
int timeFFdelay = 16; //time in ms till the force feedback is reduced.
float feedbackreceived = 0.0;
float feedbacksetting = 0.0;
int minAccelFFamplitudeX = 99; //minimum fluctuation to cause accel feedback X to send from wifly.
int minAccelFFamplitudeY = 20; //minimum fluctuation to cause accel feedback Y to send from wifly.
int minAccelFFamplitudeZ = 30; //was 50 //minimum fluctuation to cause accel feedback Z to send from wifly.
int aDivider = 123; //was 127. this is the divider used by arduino to determine amplitude of FF from Accelerometer... it is arbitrary.
int hasRumblers;        //how many rumblers does the controller have? set in controlmap.
float maxRumbleIntensity;  //set in controllmap.
//timeFFreduce
//TIME
long theTime; //this will be equated to millis();
//Heartbeat Timing
int heartRate = 111;
long beatTimer; //will be equated to theTime plus heartRate;
//Command Timing
int repeatDelay = 23; //number of milliseconds between each command send.
//COMMUNICATIONS
//Address, Ports.
//String ip       = "10.0.1.33"; // The Mini's IP Address
  //String ip       = "192.168.0.33"; // ON DLINK
  String ip       = "192.168.1.3"; // ON HITCH
  //String ip       = "207.164.79.91"; //iPhone, experimental.
  //String ip = "24.141.134.157"; //home base. experimental.
  //String ip = "172.20.10.5"; //experimental
int sendPort        = 8888;        // Destination Port, usually 8888
int listenPort  = 6000;          //port to listen on. usually 6000.
//Command Handling
String comMsg = ""; //the command message sent at regular intervals...
String delayedCommands = ""; //for commands that would otherwise be discarded due to the nature of the processing loop...
int sCS = 0;  //settings checksum
String instruction;  //instruction to send
int commandCounter = 0;            //how many commands have been sent since a heartbeat was last sent?
int commandsSentToHeartbeat = 10;  //how many commands need to be sent before a heartbeat is sent?
//boolean eocReceived = false;;
String instructionDataString = "";
String aCommand = "";  //the command received from Arduino.
int aCommandVal = 0;  //value attached to command, if any.
//Commands (to send to ARDUINO).
boolean comDelayRequired = false;
String CsetCamera = "L";  //camera setting
String CsetCameraCentre = "l";
String CautoCamera = "h";
String CsetAutoCameraRange = "a";
String CsetWheel = "W";  //wheel setting
String CsetThrottle = "T";  //throttle setting
String Cstart = "S";  //Start Command.
String CswitchESC = "A";  //ESC setting.
String CheartBeat = "B";  //heartbeat
String CsetSteeringRange = "R";  //SteeringRange (wheelMaxTravelDefault);
String CsetWheelCentre = "V";  //wheel centre default.
String CcalibrateSensors = "~";  //Calibrate Sensors (Steering Pot, Accelerometer, etc).  (tilde).
String CresetOnFlatline = "Z";  //if the arduino receives this, switches on/off: reset if the heartbeat is lost.
  String CaAminX = "x"; //X axis of accelerometer amplitude minimum command.
  String CaAminY = "y"; //Y axis of accelerometer amplitude minimum command.
  String CaAminZ = "z"; //Z axis of accelerometer amplitude minimum command.
  //String CsPTM = "k"; //steering pot threshold maximum command
  //String CoBD = "l"; //old bearing delay time
  String CsendFFaccels = "X";  //send accel forcefeedback info
String CSCS = "M";  //tell arduino to reset settings checksum. Also used by arduino to indicate its settings checksum for verification.
String CsCSgood = "m"; //command tells arduino the settings checksum was good.
//String endOfCommand = "Q";  //close command
//Vehicle Operation States
boolean started = false; //has the start button been pressed and we received a response?
boolean escOn = false;  //according to this, is the esc on or off?
boolean reversing = false; //is a reverse-power instruction being sent to the ESC?
boolean clutchIn = false; //is the clutch being depressed?
boolean settingsSent = false; //have the settings been successfully sent to the arduino?
int direction = 0; //0 = neutral, 1 = forward, 2 = reverse
int gear = 1; //we start in first gear. Goes up to 6.
//NETWORK DECLARATION
UDP udp;
//UNIVERSAL CONTROLLER DECLARATIONS
ControllIO controll;
ControllDevice controlDevice;  //equated to the device of control, xbox 360/PS3/G27 etc.
//variables (set in controller maps).
//details
boolean hasCoolieHat;   //does the controller have a cooliehat?
boolean coolieIsCoolie; //is the cooliehat treated as a cooliehat, or is it broken out into buttons?
boolean clutchForGears; //do we need to press a clutch in to go up a gear or down?
boolean hasSlidingClutch; //does the controller have a sliding clutch or a button?
boolean hasForRevGear;  //is there a forward/reverse gear method? used to determine if we need to pay attention to "direction".
boolean hasReversePedal; //the controller has a dedicated reverse pedal. set in controllermaps.
boolean wheelIsStick;   //is the steering controlled by a stick? (affects steering curve).
boolean throttleIsStick; //is throttle a stick or a pedal? affects deadzones used.
boolean hasCameraStick;  //is the camera control on this controller a slider?
boolean hasCameraButtons; //does this controller have buttons that control the camera?
boolean hasCameraCentreButton;  //does this button have a dedicated button for centering a controller?
boolean oneSliderForThrottle; //does the controller have only one slider for throttle (like xbox 360 on windows or g27 with only one pedal?).
//sliders, sticks
ControllSlider sliderWheel;    //equated to the slider (stick, wheel, etc) designated for steering. fairly consistent.
ControllSlider sliderThrottle; //slider for forward acceleration
int throttleBase; //set in controll map. 0 = regular (-1 to 1, rests at 0), 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
ControllSlider sliderReverse;    //slider for backwards acceleration
int reverseBase; //set in controll map. 0 = regular (-1 to 1, rests at 0), 1 = -1 to 1, rests at -1.
ControllSlider sliderClutch;    //slider for clutch.
int clutchBase; //set in controll map. 0 = regular (-1 to 1, rests at 0), 1 = -1 to 1, rests at -1.
ControllSlider sliderCameraX;  //slider for controlling camera movement, X plane.
//cooliehat (note that the drivers I'm using for xbox 360 controller on mac break the cooliehat out into buttons)
ControllCoolieHat cooliehat;
//Buttons
//cooliehat buttons
boolean cooliehatPressed = false;
boolean cooliePressedOnce = false; //for functions controlled by the cooliehat that need to execute once with each press.
ControllButton coolieUP;
boolean buttonCOOLIEUPpressed = false;
ControllButton coolieDOWN;
boolean buttonCOOLIEDOWNpressed = false;
ControllButton coolieLEFT;
boolean buttonCOOLIELEFTpressed = false;
ControllButton coolieRIGHT;
boolean buttonCOOLIERIGHTpressed = false;
//general buttons
ControllButton buttonStart;  
boolean buttonStartPressed = false;
ControllButton buttonESC;
boolean buttonESCpressed = false;
ControllButton buttonSendSettings;
boolean buttonSendSettingsPressed = false;
ControllButton buttonResetOnFlatline;
boolean buttonResetOnFlatlinePressed = false;
 
ControllButton buttonGEARUP;  
boolean buttonGEARUPpressed = false;
ControllButton buttonGEARDOWN;
boolean buttonGEARDOWNpressed = false;
 
ControllButton buttonCentreCamera;
boolean buttonCentreCameraPressed = false;
ControllButton buttonAutoCamera;
boolean buttonAutoCameraPressed = false;
ControllButton buttonCameraMoveLeft;
boolean buttonCameraMoveLeftPressed = false;
ControllButton buttonCameraMoveRight;
boolean buttonCameraMoveRightPressed = false;
 
ControllButton buttonCLUTCH;
boolean buttonCLUTCHpressed = false; //here mostly for consistency...
 
ControllButton buttonForward;     //NOT for acceleration, but for direction indication when using one accelerator in some circumstances.
boolean buttonForwardPressed = false;  
ControllButton buttonReverse;     //NOT for acceleration, but for direction indication when using one accelerator in some circumstances.
boolean buttonReversePressed = false;
 
ControllButton buttonGEAR1;     //Example: For Shifter Gear 1 on G27.
boolean buttonGEAR1Pressed = false;
ControllButton buttonGEAR2;     //Example: For Shifter Gear 2 on G27.
boolean buttonGEAR2Pressed = false;  
ControllButton buttonGEAR3;     //Example: For Shifter Gear 3 on G27.
boolean buttonGEAR3Pressed = false;  
ControllButton buttonGEAR4;     //Example: For Shifter Gear 4 on G27.
boolean buttonGEAR4Pressed = false;  
ControllButton buttonGEAR5;     //Example: For Shifter Gear 5 on G27.
boolean buttonGEAR5Pressed = false;  
ControllButton buttonGEAR6;     //Example: For Shifter Gear 6 on G27.
boolean buttonGEAR6Pressed = false;  
ControllButton buttonGEARreverse;     //Example: For Shifter Gear REVERSE on G27.
boolean buttonGEARreversePressed = false;  
 
ControllButton buttonCalibrateSensors;   //instructs vehicle to calibrate pots attached to steering
boolean buttonCalibrateSensorsPressed = false;
ControllButton buttonSendFFaccels;   //instructs vehicle to send (or not send) accelerometer feedback...
boolean buttonSendFFaccelsPressed = false;
 
 
//////////////////////////////////
//                              //
//            Setup             //
//                              //
//////////////////////////////////
void setup() {
  udp = new UDP( this, listenPort );  // create a new datagram connection on port 6000
  //udp.log( true );         // <-- printout the connection activity
  udp.listen( true );           // and wait for incoming message  
 
  setController();  //setup controller
 
  println("Setup complete");
}
 
////////////////////////////////////////////
//                                        //
//            Controller Maps             //
//                                        //
////////////////////////////////////////////
void mapPS3mac()
{
  controlDevice = controll.getDevice("PLAYSTATION(R)3 Controller");
  printDeviceItems();
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelMovementMin = 0.001;  //wheel has to be moved this much in order to calculate new value.
  wheelStickDeadZone = 0.01;    //stick neutral deadzone
  wheelHardValue = 0.93;
  wheelPOW = 1.0;
  throttleStickDeadzone = 0.05; //throttle neutral deadzone.
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.004;
  pedalHardValue = 0.95;
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.07;
  cameraHardValue = 0.90;
  cameraMaxStep = 7;
  cameraMinStep = 4;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = false;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = false;
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //one stick, on the left, signifies both directions...
  hasReversePedal = false; //this controller does not have a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick. (as opposed to a wheel, like the G27)
  throttleIsStick = true; //make sure you use stick deadzones.
  hasCameraStick = false;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers, though I can't get them to work yet.
  maxRumbleIntensity = 1.0;
  throttleBase = 2;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
  reverseBase = 0;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(2);
  sliderThrottle = controlDevice.getSlider(1);
  //sliderCameraX = controlDevice.getSlider(1);
  //buttons
  buttonStart = controlDevice.getButton(3);      //START BUTTON
  buttonESC = controlDevice.getButton(0);     //SELECT BUTTON
  buttonSendSettings = controlDevice.getButton(3);      //START BUTTON, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(0);      //SELECT BUTTON (clutch must be in for this one to work).
 
  buttonGEARDOWN = controlDevice.getButton(10);       //LTB
  buttonGEARUP = controlDevice.getButton(11);        //RTB
  buttonCLUTCH = controlDevice.getButton(2);    //RJD
 
  buttonCentreCamera = controlDevice.getButton(1);    //LJD (depress to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton(14);    //X. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(8);       //LEFT PADDLE
  buttonCameraMoveRight = controlDevice.getButton(9);        //RIGHT PADDLE
 
  buttonCalibrateSensors = controlDevice.getButton(12);        //TRIANGLE  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(12);        //TRIANGLE (when clutch is NOT in).
 
  //cooliehat
  //broken into buttons for this controller.
  //(left arrow pad)
  coolieUP = controlDevice.getButton(4);    //cooliehat UP
  coolieDOWN = controlDevice.getButton(6);    //cooliehat DOWN
  coolieLEFT = controlDevice.getButton(7);    //cooliehat LEFT
  coolieRIGHT = controlDevice.getButton(5);    //cooliehat RIGHT 
}
void mapXBOXwindows()
{
  controlDevice = controll.getDevice("XBOX 360 For Windows (Controller)");
  //printDeviceItems();
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelMovementMin = 0.001;  //wheel has to be moved this much in order to calculate new value.
  wheelStickDeadZone = 0.005;    //stick neutral deadzone
  wheelHardValue = 0.93;
  wheelPOW = 1.0;
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.002;
  pedalHardValue = 0.95;
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.2;
  cameraHardValue = 0.90;
  cameraMaxStep = 6;
  cameraMinStep = 1;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = true;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = false;
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //even though there are two pedals, on this controller in windows one slider represents both pedals.
  hasReversePedal = false; //this controller does not have a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick.
  throttleIsStick = false; //make sure you use pedal deadzones.
  hasCameraStick = true;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers, though I can't get them to work.
  maxRumbleIntensity = 1.0;
  throttleBase = 2;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
  reverseBase = 0;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(3);
  sliderThrottle = controlDevice.getSlider(4);
  sliderCameraX = controlDevice.getSlider(1);
  //stick = new ControllStick(sliderWheel,sliderThrottle);
  //buttons
  buttonStart = controlDevice.getButton("Button 7");      //START BUTTON
  buttonESC = controlDevice.getButton("Button 6");     //BACK BUTTON
  buttonSendSettings = controlDevice.getButton("Button 7");      //START BUTTON, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton("Button 6");      //BACK BUTTON (clutch must be in for this one to work).
 
  buttonGEARUP = controlDevice.getButton("Button 5");        //RB
  buttonGEARDOWN = controlDevice.getButton("Button 4");       //LB
  buttonCLUTCH = controlDevice.getButton("Button 9");    //RJD
 
  buttonCentreCamera = controlDevice.getButton("Button 8");    //LJD  (press to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton("Button 0");    //A. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton("Button 2");       //X
  buttonCameraMoveRight = controlDevice.getButton("Button 1");        //B
 
  buttonCalibrateSensors = controlDevice.getButton("Button 3");        //Y  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton("Button 3");        //Y (when clutch is NOT in).
 
  //cooliehat
  //handled by cooliehat: steering trim (when clutch is depressed) and camera trim (when cameraCentre is depressed).
  cooliehat = controlDevice.getCoolieHat(10);
}
void mapXBOXmac()
{
  controlDevice = controll.getDevice("Controller");
  //printDeviceItems();
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelStickDeadZone = 0.005;    //stick neutral deadzone
  wheelHardValue = 0.93;
  wheelMovementMin = 0.002;  //wheel has to be moved this much in order to calculate new value.
  wheelPOW = 1.0;
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.002;
  pedalHardValue = 0.95;
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.2;
  cameraHardValue = 0.90;
  cameraMaxStep = 6;
  cameraMinStep = 1;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = false;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //don't need a clutch to go up a gear.
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = false; //this controller has one slider for each pedal...
  hasReversePedal = true; //this controller has a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick.
  throttleIsStick = false; //make sure you use pedal deadzones.
  hasCameraStick = true;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers, though I can't get them to work.
  maxRumbleIntensity = 1.0;
  throttleBase = 1;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2 is SPLIT, rests at 0, goes from 0 to -1, 0 to 1.
  reverseBase = 1;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(2);
  sliderThrottle = controlDevice.getSlider(5);
  sliderReverse = controlDevice.getSlider(4);
  sliderCameraX = controlDevice.getSlider(0);
  //buttons
  buttonStart = controlDevice.getButton(4);      //START BUTTON
  buttonESC = controlDevice.getButton(5);     //BACK BUTTON
  buttonSendSettings = controlDevice.getButton(4); //start button, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(5);      //back BUTTON (clutch must be in for this one to work).
 
  buttonGEARUP = controlDevice.getButton(9);        //RB
  buttonGEARDOWN = controlDevice.getButton(8);       //LB
 
  buttonCLUTCH = controlDevice.getButton(7);    //RJD
 
  buttonCentreCamera = controlDevice.getButton(6);    //LJD (also pressed to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton(11);    //A. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(13);       //X
  buttonCameraMoveRight = controlDevice.getButton(12);        //B
 
  buttonCalibrateSensors = controlDevice.getButton(14);        //Y  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(14);        //Y (when clutch is NOT in).
 
  //cooliehat
  //handled by cooliehat: steering trim (when clutch is depressed) and camera trim (when cameraCentre is depressed).
  coolieUP = controlDevice.getButton(0);    //cooliehat UP
  coolieDOWN = controlDevice.getButton(1);    //cooliehat DOWN
  coolieLEFT = controlDevice.getButton(2);    //cooliehat LEFT
  coolieRIGHT = controlDevice.getButton(3);    //cooliehat RIGHT 
}
void mapG27windows()
{
  controlDevice = controll.getDevice("Logitech G27 Racing Wheel USB");
  printDeviceItems();
  controlDevice.setTolerance(0.0);  //was 0.5f.
  wheelMovementMin = 0.000;  //wheel has to be moved this much in order to calculate new value.
  wheelHardValue = 0.989;  //very sensitive and accurate controller... good job logitech!
  wheelDeadZone = 0.00;    //WHEEL, NOT stick, neutral deadzone. None for the G27.
  wheelPOW = 1.0;
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.002;
  pedalHardValue = 0.95;
  clutchThrow = 0.52;  //percentage the clutch must be pushed to activate.
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //NOT on this one. if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.2;  //no
  cameraHardValue = 0.90;   //no
  cameraMaxStep = 8;   
  cameraMinStep = 2;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = true;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = true;
  hasForRevGear = true;  //it does... used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //even though there are two pedals, on this controller in windows one slider represents both pedals.
  hasReversePedal = true; //I use the brake pedal as the reverse pedal... keeps things simple
  wheelIsStick = false;    //steering is controlled by a stick.
  throttleIsStick = false; //make sure you use pedal deadzones.
  hasCameraStick = false;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 5;  //5 rumblers...?
  maxRumbleIntensity = 0.60;
  throttleBase = 4;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1, 4 is 1 to -1 and rests at 1.
  reverseBase = 4;
  clutchBase = 4;
  //sliders
  sliderWheel = controlDevice.getSlider(0);
  sliderThrottle = controlDevice.getSlider(3);
  sliderReverse = controlDevice.getSlider(2);  //not needed
  sliderClutch = controlDevice.getSlider(4);
  //sliderCameraX = controlDevice.getSlider(1);
  //buttons
  buttonStart = controlDevice.getButton(1);      //STICKSHIFT REDBUTTON 1
  buttonESC = controlDevice.getButton(2);     //STICKSHIFT REDBUTTON 2
  buttonSendSettings = controlDevice.getButton(1);      //STICKSHIFT REDBUTTON 1, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(2);      //STICKSHIFT REDBUTTON 2 (clutch must be in for this one to work).
 
  buttonGEARDOWN = controlDevice.getButton(6);       //Left Paddle
  buttonGEARUP = controlDevice.getButton(5);        //Right Paddle
  //buttonCLUTCH = controlDevice.getButton("Button 9");    //NOT a button clutch for this controller.
 
  buttonCentreCamera = controlDevice.getButton(16);    //POV UP/FORWARD/TOP. Need to push this while adjusting camera trim.  
  buttonAutoCamera = controlDevice.getButton(18);    //POV DOWN. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(19);       //POV LEFT.
  buttonCameraMoveRight = controlDevice.getButton(17);        //POV RIGHT.
 
  buttonCalibrateSensors = controlDevice.getButton(3);        //STICKSHIFT REDBUTTON 3  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(3);        //STICKSHIFT REDBUTTON 3 (when clutch is NOT in).
 
  //cooliehat
  //handled by cooliehat: steering trim (when clutch is depressed) and camera trim (when cameraCentre is depressed).
  cooliehat = controlDevice.getCoolieHat(0);
 
  //THE G27 has a gear shifter!!
  //currently using it as direction button (1,3,5 = forward, 2,4,6= reverse)
  buttonGEAR1 = controlDevice.getButton(9);
  buttonGEAR2 = controlDevice.getButton(10);
  buttonGEAR3 = controlDevice.getButton(11);
  buttonGEAR4 = controlDevice.getButton(12);
  buttonGEAR5 = controlDevice.getButton(13);
  buttonGEAR6 = controlDevice.getButton(14);
  buttonGEARreverse = controlDevice.getButton(15);
 
  //spur the device with a rumble command... (activates G27 for me).
  controlDevice.rumble(0.3, 1);
  delay(333);
  controlDevice.rumble(0.0, 1);
}
void mapF710xInputWindows()
{
  controlDevice = controll.getDevice("Controller (Rumble Gamepad F510)");
  //printDeviceItems();
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelMovementMin = 0.001;  //wheel has to be moved this much in order to calculate new value.
  wheelStickDeadZone = 0.005;    //stick neutral deadzone
  wheelHardValue = 0.95;
  wheelPOW = 1.0;
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.002;
  pedalHardValue = 0.95;
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.2;
  cameraHardValue = 0.90;
  cameraMaxStep = 6;
  cameraMinStep = 1;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = true;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = false;
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //even though there are two pedals, on this controller in windows one slider represents both pedals.
  hasReversePedal = false; //this controller does not have a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick.
  throttleIsStick = false; //make sure you use pedal deadzones.
  hasCameraStick = true;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers
  maxRumbleIntensity = 1.0;
  throttleBase = 2;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
  reverseBase = 0;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(3);
  sliderThrottle = controlDevice.getSlider(4);
  sliderCameraX = controlDevice.getSlider(1);
  //stick = new ControllStick(sliderWheel,sliderThrottle);
  //buttons
  buttonStart = controlDevice.getButton(7);      //START BUTTON
  buttonESC = controlDevice.getButton(6);     //BACK BUTTON
  buttonSendSettings = controlDevice.getButton(7);      //START BUTTON, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(6);      //BACK BUTTON (clutch must be in for this one to work).
 
  buttonGEARUP = controlDevice.getButton(5);        //RB
  buttonGEARDOWN = controlDevice.getButton(4);       //LB
  buttonCLUTCH = controlDevice.getButton(9);    //RJD
 
  buttonCentreCamera = controlDevice.getButton(8);    //LJD  (press to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton(0);    //A. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(2);       //X
  buttonCameraMoveRight = controlDevice.getButton(1);        //B
 
  buttonCalibrateSensors = controlDevice.getButton(3);        //Y  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(3);        //Y (when clutch is NOT in).
 
  //cooliehat
  //handled by cooliehat: steering trim (when clutch is depressed) and camera trim (when cameraCentre is depressed).
  cooliehat = controlDevice.getCoolieHat(10);
}
void mapF710DirectInputWindows()
{
  controlDevice = controll.getDevice("Logitech Rumblepad 2 USB");
  //printDeviceItems();
  controlDevice.setTolerance(0.05f);  //was 0.5f.
  wheelMovementMin = 0.001;  //wheel has to be moved this much in order to calculate new value.
  wheelStickDeadZone = 0.005;    //stick neutral deadzone
  wheelHardValue = 0.95;
  wheelPOW = 1.0;
  throttleStickDeadzone = 0.02; //throttle neutral deadzone.
  pedalDeadZone = 0.004;
  pedalMovementMin = 0.002;
  pedalHardValue = 0.95;
  cameraMoveInterval = 6; //milliseconds between camera steps.
  cameraMovementMin = 0.01; //if camera movement is controlled by a stick, the stick has to be moved this much to calculate a new value.
  cameraStickDeadZone = 0.2;
  cameraHardValue = 0.90;
  cameraMaxStep = 5;
  cameraMinStep = 1;
  hasCoolieHat = true;    //the controller has a cooliehat.
  coolieIsCoolie = true;  //the cooliehat is broken into buttons.
  clutchForGears = true;  //does the clutch need to be depressed to change gears?
  hasSlidingClutch = false;
  hasForRevGear = false;  //false. used to determine if we should pay attention to "direction".
  oneSliderForThrottle = true; //it's a stick on this one.
  hasReversePedal = false; //this controller does not have a dedicated reverse pedal.
  wheelIsStick = true;    //steering is controlled by a stick.
  throttleIsStick = true; //make sure you use stick deadzones
  hasCameraStick = false;   //is camera control a stick on this controller?
  hasCameraButtons = true;
  hasCameraCentreButton = true;
  hasRumblers = 2;  //controller has rumblers
  maxRumbleIntensity = 5.0;
  throttleBase = 2;  //0 = -1 to 1, rests at 0. 1 = -1 to 1, rests at -1. 2= SPLIT across two padels, rests at 0, range: 1 to -1.
  reverseBase = 0;
  clutchBase = 0;
  //sliders
  sliderWheel = controlDevice.getSlider(1);
  sliderThrottle = controlDevice.getSlider(2);
  //sliderCameraX = controlDevice.getSlider(3);
  //stick = new ControllStick(sliderWheel,sliderThrottle);
  //buttons
  buttonStart = controlDevice.getButton(10);      //START BUTTON
  buttonESC = controlDevice.getButton(9);     //BACK BUTTON
  buttonSendSettings = controlDevice.getButton(10);      //START BUTTON, clutch must be in.
  buttonResetOnFlatline = controlDevice.getButton(9);      //BACK BUTTON (clutch must be in for this one to work).
 
  buttonGEARUP = controlDevice.getButton(6);        //RB
  buttonGEARDOWN = controlDevice.getButton(5);       //LB
  buttonCLUTCH = controlDevice.getButton(12);    //RJD
 
  buttonCentreCamera = controlDevice.getButton(11);    //LJD  (press to adjust camera trim).
  buttonAutoCamera = controlDevice.getButton(2);    //A. autoCamera ON/OFF
  buttonCameraMoveLeft = controlDevice.getButton(1);       //X
  buttonCameraMoveRight = controlDevice.getButton(3);        //B
 
  buttonCalibrateSensors = controlDevice.getButton(4);        //Y  (clutch must be in. calibrates sensors. Vehicle should be stopped!!)
  buttonSendFFaccels = controlDevice.getButton(4);        //Y (when clutch is NOT in).
 
  //cooliehat
  //handled by cooliehat: steering trim (when clutch is depressed) and camera trim (when cameraCentre is depressed).
  cooliehat = controlDevice.getCoolieHat(0);
}
void printDeviceItems()
{
  controlDevice.printSticks();
  controlDevice.printSliders();
  controlDevice.printButtons(); 
}
void setController()
{
  controll = ControllIO.getInstance(this);
  println("Controller: " + usingController);
  if(usingController == 0) {
    //we're using the ps3 controller on the Mac OS X
    mapPS3mac();
  } else if (usingController == 1) {
    //using XBOX360 on Windows 7
    mapXBOXwindows();
  } else if (usingController == 2) {
    //using XBOX360 on Mac OS X
    mapXBOXmac();
  } else if (usingController == 3) {
    //using XBOX360 on Windows 7
    mapG27windows();
  } else if (usingController == 4) {
    //using Logitech F510 in xInput mode on Windows
    mapF710xInputWindows();
  } else if (usingController == 5) {
    //using Logitech F510 in DirectInput mode on Windows
    mapF710DirectInputWindows();
  }
}
 
 
////////////////////////////////////////////
//                                        //
//         Data Handling Functions        //
//                                        //
////////////////////////////////////////////
String ints(int theInt)
{
  //a shorthand function for turning strings into integers.
  return Integer.toString(theInt);
}
String intf(float theFloat) {
  //a shorthand function for turning strings into floats.
  return Float.toString(theFloat);
}
float convertSliderBase(float theReading, int theSliderBase)
{
  //this converts the slider to a 0 to 1 base
  float theConvertedReading = 0.0;
  if(theSliderBase == 0) {
    //it's already 0 to 1, do nothing.
    theConvertedReading = theReading;
  } else if (theSliderBase == 1) {
    //the slider base is -1 to 1.
    theConvertedReading = (theReading + 1.0) / 2.0;
  } else if (theSliderBase == 2) {
    //SPLIT. rests at 0, and travels from 1 to -1.
    //xbox 360 on windows has this.
    theConvertedReading = theReading * -1; //flip the reading
  } else if (theSliderBase == 3) {
    //on logitech gamepad, pedals are combined. Right pedal produces -1, left +1.
    //so, flip that:
    theReading = theReading * -1.0;
    //now treat like sliderbase 1:
    theConvertedReading = (theReading + 1.0) / 2.0;
  } else if (theSliderBase == 4) {
    //the slider base is 1 to -1, rests at 1.
    theConvertedReading = ((theReading - 1.0) * -1.0) / 2.0;
  } 
  //delay(666);
  //println("theReading: " + theReading + ", theConvertedReading = " + theConvertedReading);
  return(theConvertedReading);
}
float accountForSliderZones(float theCurrentReading, float theDeadZone, float theHardVal)
{
  float hardNegative = theHardVal * -1.0;
  float theDeadZoneNegative = theDeadZone * -1.0;
  float readingAdjusted;
  float theDenominator = theHardVal - theDeadZone;
  if(theCurrentReading < theDeadZoneNegative) {
      //stick is moved to the left
      if(theCurrentReading > hardNegative) {
        float posReading = theCurrentReading * -1.0;
        readingAdjusted = ((posReading - theDeadZone) / theDenominator) * -1.0;
      } else {
        readingAdjusted = -1.0;
      }
  } else if(theCurrentReading > theDeadZone) {
      //stick is moved to the right.
      if(theCurrentReading < theHardVal) {
        readingAdjusted = (theCurrentReading - theDeadZone) / theDenominator;
      } else {
        readingAdjusted = 1.0;
      }
  } else {
    //in the deadzone
    readingAdjusted = 0.0;
  }
  return(readingAdjusted);
}
 
 
////////////////////////////////////////////
//                                        //
//          FORCE FEEDBACK                //
//          Handling/Execution            //
//                                        //
////////////////////////////////////////////
void handleRumbling() {
  //you might notice this is geared to the G27, since it's the only device I can get to rumble through processing...so far.
  if(rumbling) {
    //first, are any of the rumblers activated?
    theTime = millis();
    if(theTime > timeFFreduce) {
      //has the time come to reduce the force feedback intensity if it hasn't already been zeroed?
      //make sure the values are not less than zero.
      float rumbleMin = 0.04;
      float rumbleReduce = 0.077;
      if((rumble0val <= rumbleMin) && rumbling0) {
        rumble0val = 0.0;
        controlDevice.rumble(rumble0val, 0);
        rumbling0 = false;
        //println("feedback Z zeroed");
      } else if ((rumble1val <= rumbleMin) && rumbling1) {
        rumble1val = 0.0;
        controlDevice.rumble(rumble1val, 1);
        rumbling1 = false;
        //println("feedback Y zeroed");
      } else if ((rumble2val <= rumbleMin) && rumbling2) {
        rumble2val = 0.0;
        controlDevice.rumble(rumble2val, 2);
        rumbling2 = false;
        //println("feedback X zeroed");
      }
      //handle rumbler0
      if(rumble0val > rumbleMin) {
        controlDevice.rumble(rumble0val, 0);
        rumbling0 = true;
        rumble0val = rumble0val - rumbleReduce;  //reduce the value slightly for the next run.
 
      }
      //handle rumbler1
      if(rumble1val > rumbleMin) {
        controlDevice.rumble(rumble1val, 1);
        rumbling1 = true;
        rumble1val = rumble1val - rumbleReduce;  //reduce the value slightly for the next run.
      }
      //handle rumbler2
      if(rumble2val > rumbleMin) {
        controlDevice.rumble(rumble2val, 2);
        rumbling2 = true;
        rumble2val = rumble2val - rumbleReduce;  //reduce the value slightly for the next run.
      }
      timeFFreduce = theTime + timeFFdelay;
    }
  } else if (!rumbling) {
    //we've likely received the "no more rumbling" indicator.
    if(rumbling0) {
      controlDevice.rumble(0.0, 0);
    }
    if(rumbling1) {
      controlDevice.rumble(0.0, 1);
    }
    if(rumbling2) {
      controlDevice.rumble(0.0, 2);
    }
  }
 
  if(!rumbling0 && !rumbling1 && !rumbling2) {
    rumbling = false;
  }
}
void doRumble(int rumbleID, int rumbleValue) {
  //this function will probably become fairly complex...
  //VERY simple at the moment.
  theTime = millis();
  feedbackreceived = rumbleValue;   //int to float.
 
  println("rumbleID: " + rumbleID + ",  rumbleValue: " + rumbleValue);
 
  feedbacksetting = ((float)rumbleValue * 1000.0) / ((float)aDivider * 1000.0);
  if (feedbacksetting > maxRumbleIntensity) {
    feedbacksetting = maxRumbleIntensity;
  }
  //println("feedbacksetting = " + feedbacksetting);
 
  rumbling = true;  //set rumbling true, tentatively...
  if(rumbleID == 2) {
    rumble2val = feedbacksetting;
    //println("it's X");
  } else if (rumbleID == 1) {
    rumble1val = feedbacksetting;
    //println("it's Y");
  } else if (rumbleID == 0) {
    rumble0val = feedbacksetting;
    //println("it's Z: " + feedbacksetting);
  } else if (rumbleID == 77) {
    //println("CANCEL ALL RUMBLING");
    //rumbling = false;
  }
}
 
////////////////////////////////////////////
//                                        //
//          Vehicle Commands              //
//                                        //
////////////////////////////////////////////
////////////////////////////////////////////
//                                        //
//         Communication Functions        //
//                                        //
////////////////////////////////////////////
void sendCom()
{
  if(comDelayRequired) {
    //for commands that would otherwise get discarded  
    comMsg = delayedCommands + comMsg;
    println(comMsg);
    delayedCommands = "";
    comDelayRequired = false;
  }
 
  //build comMsg
  if(!autocameraOn) {
    comMsg = CsetCamera + cameraSetting + comMsg; //add camera
  }
  comMsg = CsetWheel + wheelSetting + comMsg; //add wheel
  comMsg = CsetThrottle + throttleSetting + comMsg; //add throttle
 
  if(comMsg != "") {
    udp.send(comMsg, ip, sendPort );  //send command
    //println(comMsg);
    commandCounter++;
    delay(repeatDelay);
    comMsg = "";
  }
}
void setSteeringRange(int theSetting)
{
  instruction = CsetSteeringRange + ints(theSetting);
  comMsg = comMsg + instruction;
}
void setWheelCentre(int theSetting)
{
 instruction = CsetWheelCentre + ints(theSetting);
 comMsg = comMsg + instruction;
}
//void sendSensorThresholds()
//{
  //intf used for float to string conversion
  //instruction = CaAmin + ints(minAccelFFamplitudeZ) + endOfCommand;
  //sendCom(instruction);  //set minimum accelerometer force feedback amplitude.
  //delay(27);
  //instruction = CaDivider + ints(aDivider) + endOfCommand;
  //sendCom(instruction);  //set minimum accelerometer force feedback amplitude.
  //delay(27);
  //println("Sensor Thresholds Sent"); 
//}
void sendSettings()
{
  println("Sending Settings..."); 
  //this function is meant to send every possible setting that the arduino needs to remember...
  sCS = 0; //settings checksum, reset to zero.
  instruction = CSCS + ints(sCS);
  comMsg = comMsg + instruction; //tell arduino wifly mini to reset its settings checksum to zero.
  //set wheelCentre
  setWheelCentre(wheelCentre);  //sets the wheel centre... the function adds the instruction to comMsg itself
  sCS++; //add one to the checksum every time we send a setting.
  //set the steering range
  setSteeringRange(steeringRange);  //sets steering range, function adds instruction to comMsg.
  sCS++; //add one to the checksum every time we send a setting.
 
 
  //set the camera centre 
  instruction = CsetCameraCentre + ints(cameraCentre);
  comMsg = comMsg + instruction;
  sCS++; //add one to the checksum every time we send a setting.
  //set autocamera range.
  instruction = CsetAutoCameraRange + ints(autoCameraRange);
  comMsg = comMsg + instruction;
  sCS++; //add one to the checksum every time we send a setting.
 
 
  //set minimum accelerometer force feedback amplitudes, X, Y, then Z.
  instruction = CaAminX + ints(minAccelFFamplitudeX);
  comMsg = comMsg + instruction;
  sCS++; //add one to the checksum every time we send a setting.
 
  instruction = CaAminY + ints(minAccelFFamplitudeY);
  comMsg = comMsg + instruction;
  sCS++; //add one to the checksum every time we send a setting.
 
  instruction = CaAminZ + ints(minAccelFFamplitudeZ);
  comMsg = comMsg + instruction;
  sCS++; //add one to the checksum every time we send a setting.
 
  //ask the WIFLY for a settings comparison.
  println("Ask WIFLY for confirmation checksum: " + sCS);
  instruction = CSCS + ints(sCS);
  comMsg = comMsg + instruction;
}
void calibrateSensors()
{
  if(settingsSent) {
    println("Calibrate Sensors");
    sendCom();
    comMsg = comMsg + CcalibrateSensors;
  } else {
    println("Can't calibrate sensors: settings haven't been sent.");
  }
}
void ffSendAccels()
{
  //println("Switch On/Off ACCELEROMETER ForceFeedback");
  comMsg = comMsg + CsendFFaccels;
}
void neutralAndCentre()
{
      wheelSetting = wheelCentre;
      setSteeringRange(steeringRange);
      setWheelCentre(wheelCentre);  //sets the default wheel centre on arduino.
      throttleSetting = throttleNeutral;
}
void startVehicle()
{
  println("Sent S.");
  comMsg = comMsg + Cstart;
}
void escPOWER()
{
  if(started == true){
    println("Sent E.");
    neutralAndCentre();
    comMsg = comMsg + CswitchESC;  //tell the ESC to power up.
  } else {
    println("Not Started");
  }
}
void resetOnFlatline()
{
    comMsg = comMsg + CresetOnFlatline + CresetOnFlatline + CresetOnFlatline;    
}
void centreCamera()
{
  theTime = millis();
  cameraSetting = cameraCentre;
  cameraSetting = cameraCentre;
  cameraStep = 0;
  cameraMoveTime = theTime + 555;
  println("Camera Centered");
}
void autoCamera()
{
  comMsg = comMsg + CautoCamera;
}
void moveCamera(int theStep) 
{
  if((theStep < 0) || (theStep > 0)) {
      theTime = millis();
      if(cameraMoveTime < theTime) {
        if(cameraMaxLeft > cameraMaxRight) {
          //camera max left greater than max right.
          if((cameraSetting >= cameraMaxRight) && (cameraSetting <= cameraMaxLeft)) {
            cameraSetting = cameraSetting + theStep;
          }
          if(cameraSetting < cameraMaxRight) {
            //camera setting has travelled beyond camera limit
            cameraSetting = int(cameraMaxRight);  
          } else if (cameraSetting > cameraMaxLeft) {
            cameraSetting = int(cameraMaxLeft);
          }
        } else if (cameraMaxLeft < cameraMaxRight) {
          //camera max left less than max right.
          if((cameraSetting >= cameraMaxLeft) && (cameraSetting <= cameraMaxRight)) {
            cameraSetting = cameraSetting + theStep;
          }
          if(cameraSetting < cameraMaxLeft) {
            //camera setting has travelled beyond camera limit
            cameraSetting = int(cameraMaxLeft);  
          } else if (cameraSetting > cameraMaxRight) {
            cameraSetting = int(cameraMaxRight);
          }
        }
        //println("theStep: " + theStep + ", cameraSetting: " + cameraSetting);
        //setCamera(cameraSetting);
        cameraMoveTime = theTime + cameraMoveInterval;
      }
  }  
}
void adjustSteeringTrim(int trimAdjustAmt)
{
  if(clutchIn) {
    wheelCentre = wheelCentre + trimAdjustAmt;
    wheelSetting  = wheelSetting + trimAdjustAmt;
    println("wheelCentre: " + wheelCentre);
  }
}
void adjustCameraTrim(int trimAdjustAmt)
{
  if(buttonCentreCameraPressed) {
    cameraCentre = cameraCentre + trimAdjustAmt;
    cameraSetting  = cameraSetting + trimAdjustAmt;
    println("cameraCentre: " + cameraCentre);
  }
}
void updateheart()
{
  theTime = millis();
  beatTimer = theTime + heartRate;
}
void heartbeat()
{
  if (started == true) { 
    theTime = millis();
    if ((theTime >= beatTimer) || (commandCounter > commandsSentToHeartbeat)) {
      //if the beattimer has expired, or the number of commands sent has exceeded the commandcounter, a heartbeat is sent.
      //println(beatTimer);
      comMsg = comMsg + CheartBeat;
      updateheart();
      commandCounter = 0;
    }
  }
}
void delegate() {
      if (aCommand.equals("A")) {
        println("WIFLY: ESC is ON.");
        escOn = true;
      } else if (aCommand.equals("a")) {
        println("WIFLY: ESC is OFF.");
        escOn = false;
      } else if (aCommand.equals("S")) {
        println("WIFLY: STARTED");
        started = true;
      } else if (aCommand.equals("s")) {
        println("WIFLY: STOPPED");
        started = false;
      } else if (aCommand.equals("X")) {
        //wifly telling us accel send switch is on or off
        if(aCommandVal == 0) {
          println("WIFLY: Stop Accel FF");  
        } else if (aCommandVal == 1) {
          println("WIFLY: Start Accel FF");
        }
      } else if (aCommand.equals("h")) {
        //wifly telling us accel send switch is on or off
        if(aCommandVal == 0) {
          println("WIFLY: AutoCamera OFF");
          autocameraOn = false;
        } else if (aCommandVal == 1) {
          println("WIFLY: AutoCamera ON");
          autocameraOn = true;
        }
      } else if (aCommand.equals("p")) {
        //arduino says it has not yet received settings. please send them.
        aCommand = "p";
        settingsSent = false;
        println("WIFLY: Please Send Settings!");
      } else if (aCommand.equals("M")) {
        //receiving settings checksum.
        if(sCS == aCommandVal) {
          println("Settings Checksum Good, Sending Confirmation...");
          //send confirmation of good checksum.
          settingsSent = true;
          comDelayRequired = true;
          delayedCommands = CsCSgood + delayedCommands;
        } else {
          println("Settings Checksum BAD (" + aCommandVal + "). Try again.");
          //do nothing.
        }
      } else if (aCommand.equals("Z")) {
        //wifly telling us the WIFLY RESET ON FLATLINE switch is on or off.
        if(aCommandVal == 0) {
          println("WIFLY: Will NOT Reset on Flatline");  
        } else if (aCommandVal == 1) {
          println("WIFLY: Will Reset on Flatline");
        }
      } else if (aCommand.equals("G")) {
        println("WIFLY: Finished Sensor Calibration");
      } else {
        //do nothing
      }
 
  if(hasRumblers > 0) {
    if(aCommand.equals("<")) {
      doRumble(2, aCommandVal);
    } else if (aCommand.equals(">")) {
      doRumble(1, aCommandVal);
    } else if (aCommand.equals("^")) {
      doRumble(0, aCommandVal);
    } else if (aCommand.equals("<")) {
      //no more rumbling!
      doRumble(77, aCommandVal);
    }
  }
 
  //println("Delegate Says: aCommand = " + aCommand + ", aCommandVal = " + aCommandVal);
}
void receive( byte[] data ) {
  //void receive( byte[] data, String ip, int port ) {   // <-- extended handler
  char[] mrTdata = new char[data.length]; //FOR CONVERTING BYTE TO CHAR. here is stored information coming from the arduino.
  String incomingDataString = ""; //makes a whole string out of data presently being received...
  String firstChar = "";
  String currentChar = "";
 
 
  for (int i=0; i < data.length; i++) {
    mrTdata[i] = char(data[i]);   //BYTE TO CHAR.
    incomingDataString = incomingDataString + mrTdata[i];  //added to whole string used for reference.
    firstChar = "" + mrTdata[0];   //store the first char for reference... "" appears to be necessary to force conversion from CHAR to STRING
    currentChar = "" + mrTdata[i];
 
    if(!currentChar.equals("1") && !currentChar.equals("2") && !currentChar.equals("3") && !currentChar.equals("4") && !currentChar.equals("5") && !currentChar.equals("6") && !currentChar.equals("7") && !currentChar.equals("8") && !currentChar.equals("9") && !currentChar.equals("0") && !currentChar.equals(".")) { 
      //the character is not a number, not a value to go along with a command,
      //so it is probably a command.
      if(instructionDataString != "") {
        aCommandVal = Integer.parseInt(instructionDataString);
      }
      if(aCommand != "") {
        delegate();
      }
      aCommand = currentChar;
      instructionDataString = "";
    } else {
      //in this case, we're probably receiving a command value.
      //store it
      instructionDataString = instructionDataString + currentChar;
    }
 
  }
}
 
////////////////////////////////////////////
//                                        //
//            SLIDER Handling             //
//                                        //
////////////////////////////////////////////
void shThrottle()
{
  //handles brakes, reverse, and forward throttle.
  //Note: convertSliderBase's function is to convert outputs from sliders to a negative one to positive one scale
  //NOTE: accountForSliderZones should be used after convertSliderBase
  //float readingAdjusted = accountForSliderZones(throttleCurrentReading, pedalDeadZone, pedalHardValue);
 
  //reset currentReadingValues
  throttleCurrentReading = 0.0;
  reverseCurrentReading = 0.0;
  float throttleReadingAccounted = 0.0;
  float reverseReadingAccounted = 0.0;
 
  //READ SLIDERS AND SORT CASES
 
    if (hasReversePedal) {
      //this case exists for controllers like the xbox 360 on mac, which has two pedals...
      //note: the Xbox 360 controller on Windows are not accounted for in these if statements, "hasReversePedal", since it's technically ONE slider.
      reverseCurrentReading = convertSliderBase(sliderReverse.getValue(), reverseBase);
      reverseReadingAccounted = accountForSliderZones(reverseCurrentReading, pedalDeadZone, pedalHardValue);  //should always be a pedal.
      if(reverseReadingAccounted < 0.0) {
        reverseReadingAccounted = reverseReadingAccounted * -1.0;  //make sure the reading is positive. (functions below pay attention to CASES reversing, braking, etc.
      }
      //now determine if we are reversing.
      if(reverseReadingAccounted > 0.0) {
        //since reverseReadingAccounted is zeroed at the beginning of this function,
        //we can go ahead and put the car into reverse if reverseReadingAccounted is above zero...
        reversing = true;
      } else {
        reversing = false;
      }
    }
 
  //!reversing should be negative in order to enter this function.
  //all controllers will (should) have a throttleSlider
  throttleCurrentReading = convertSliderBase(sliderThrottle.getValue(), throttleBase);
 
  //account for throttle slider zones
  if(throttleIsStick) {
    //use stick deadzones.
    throttleReadingAccounted = accountForSliderZones(throttleCurrentReading, throttleStickDeadzone, pedalHardValue);
  } else {
    //use pedal deadzones.
    throttleReadingAccounted = accountForSliderZones(throttleCurrentReading, pedalDeadZone, pedalHardValue);
  }
 
  boolean testingThrottleReadings = false; //set to true if you want to watch the readings up to this point.
  if(testingThrottleReadings) {
    delay(500);
    println("reverseCurrentReading: " + reverseCurrentReading + ", reverseReadingAccounted = " + reverseReadingAccounted);
    println("throttleCurrentReading: " + throttleCurrentReading + ", throttleReadingAccounted = " + throttleReadingAccounted);
    println("====================================================");
  } else if(!testingThrottleReadings) {
  //to be deleted later. just for testing my current controllers.
 
 
  //now, determine what kind of throttle and amplitude should be applied as throttle.
  if (reversing) {
    //we're reversing!
    //set reverse throttle.
    if(hasForRevGear && (direction == 2)) {
      //the user has indicated the main throttle slider should cause the vehicle to move in a reverse direction
      //this means that the reverse pedal should now cause the car to go forward.
      throttleSettingFloat =  throttleNeutral + (reverseReadingAccounted * throttleMax);
    } else {
      throttleSettingFloat =  throttleNeutral - (reverseReadingAccounted * throttleMaxReverse);
    }
    throttleSetting = int(throttleSettingFloat);
  } else if ( throttleReadingAccounted != 0.0 && (!hasForRevGear || (hasForRevGear && (direction!=0))) ) {
 
 
    if (hasForRevGear) {
      //this controller has a forward/reverse gear indicator...
      if(direction == 2) {
        //user has indicated the vehicle should move in reverse when throttle is pressed.
        if(throttleReadingAccounted > 0.0) {
          //user is pressing on the gas, so they want to go backward
          reversing = true;
          reverseReadingAccounted = throttleReadingAccounted;
          if(throttleReadingAccounted > 0.0) {
            throttleReadingAccounted = throttleReadingAccounted * -1.0;  
          }
        }
      } else if(direction == 1) {
        //user has indicated they want to go forward
        reversing = false;
      }  
    }
 
 
    if(throttleReadingAccounted > 0) {
        //should be going forward
        throttleSettingFloat =  throttleNeutral + (throttleReadingAccounted * throttleMaxForward);
    } else if (throttleReadingAccounted < 0) {
        //should be going in reverse... all preceding if statements should account for controller type
        //enough that there should be no mixups... fingers crossed.
        throttleSettingFloat =  throttleNeutral + (throttleReadingAccounted * throttleMaxReverse);
    }
    throttleSetting = int(throttleSettingFloat);
  } else {
       //account for neutral possibility.
       throttleSetting = throttleNeutral;
  } //end of braking if and then elses...
  } //end of testing throttle readings else if statement.
 
 
  //NOW FOR THE ACTUAL SETTING OF THE THROTTLE
  theTime = millis();
  if(throttleSetting != lastThrottleSetting) {
    //belongs in reverse too, I think...
    //setThrottle(throttleSetting);
    lastThrottleSetting = throttleSetting;
    //println(throttleSetting);
  }
 
}
void shWheel()
{
  wheelCurrentReading = sliderWheel.getValue();
  float limitLower = wheelLastReading - wheelMovementMin;
  float limitUpper = wheelLastReading + wheelMovementMin;
  float readingAdjusted;
  float rBC; //reading bell-curved.
 
  if((wheelCurrentReading < limitLower) || (wheelCurrentReading > limitUpper)) {
    //the slider has moved enough to justify new calculation...
    if(wheelIsStick) {
      //if the wheel is a stick, adjust like so
      readingAdjusted = accountForSliderZones(wheelCurrentReading,wheelStickDeadZone,wheelHardValue);
      rBC = pow(readingAdjusted, wheelPOW); //curve the reading...
    } else {
      //if the wheel is a wheel, adjust thusly...
      readingAdjusted = accountForSliderZones(wheelCurrentReading,wheelDeadZone,wheelHardValue);
      rBC = readingAdjusted;
    }
 
    wheelSettingFloat = wheelCentre + (rBC * wheelMaxTravel);
    wheelSetting = (int)wheelSettingFloat;
 
    //println("wheelCurrentReading = " + wheelCurrentReading);
    //delay(500);
 
 
  }
 
 
    if(wheelSetting == 0) {
      println("Error: Wheel Setting 0"); 
    }
    theTime = millis();
    if(wheelSetting != lastWheelSetting) {
      //did the move affect a change in wheel setting? if so, send new value.
      //setWheel(wheelSetting);
      lastWheelSetting = wheelSetting;
      //println("readingAdjusted = " + readingAdjusted + ", rBC = " + rBC + ", instruction = " + instruction);
      wheelLastReading = wheelCurrentReading;
    }
}
void shClutch()
{
  clutchCurrentReading = sliderClutch.getValue();
  float translatedReading = convertSliderBase(clutchCurrentReading, clutchBase);
  if((translatedReading > clutchThrow) && !clutchIn) {
    //clutch is depressed
    clutchIn = true;
  } else if ((translatedReading <= clutchThrow) && clutchIn) {
    clutchIn = false;
  }
}
void shCamera()
{
  //camera is controlled by slider (stick, something of that nature).
  cameraCurrentReading = sliderCameraX.getValue();
  float limitLower = cameraLastReading - cameraMovementMin;
  float limitUpper = cameraLastReading + cameraMovementMin;
  float readingAdjusted;
  float rBC; //reading bell-curved.
  if((cameraCurrentReading < limitLower) || (cameraCurrentReading > limitUpper)) {
    //the slider has moved enough to justify new calculation...
    readingAdjusted = accountForSliderZones(cameraCurrentReading,cameraStickDeadZone,cameraHardValue);  //remember this will return 0 if in deadzone.
    //rBC = pow(readingAdjusted, 2) * cameraMaxStep * -1.0;
    rBC = pow(readingAdjusted, 3) * cameraMaxStep;
    //rBC = readingAdjusted * cameraMaxStep; //linear
    if(flipCameraStickX) {
      rBC = rBC * -1.0;
    }
    if((rBC < 0.0) && (rBC > -1.0)) {
      rBC = rBC - cameraMinStep;  //make sure the movement is always a minimum amt.
    } else if ((rBC > 0.0) && (rBC < 1.0)) {
      rBC = rBC + cameraMinStep; 
    } else {
      //rBC is 0.0
    }
    cameraLastReading = cameraCurrentReading;
    cameraStep = int(rBC);   //the step size of camera movement.
  }
  moveCamera(cameraStep);
}
void sliderHandling() {
  //controller-specific
  if (hasSlidingClutch) {
    shClutch();
  }
  if (hasCameraStick) {
    shCamera();
  }
 
  //common
  shThrottle();
  shWheel();
}
////////////////////////////////////////////
//                                        //
//            Button Handling             //
//                                        //
////////////////////////////////////////////
void bhStart()
{
  //START BUTTON
  if (buttonStart.pressed() && !buttonStartPressed && !clutchIn) {
    startVehicle();
    buttonStartPressed = true;
  } else if (!buttonStart.pressed() && buttonStartPressed) {
    buttonStartPressed = false;
  }
}
void bhSendSettings()
{
  //Send Settings BUTTON
  if (buttonSendSettings.pressed() && !buttonSendSettingsPressed && clutchIn) {
    sendSettings();
    buttonSendSettingsPressed = true;
  } else if (!buttonSendSettings.pressed() && buttonSendSettingsPressed) {
    buttonSendSettingsPressed = false;
  }
}
void bhESC()
{
  //ESC ON/OFF BUTTON
  if (buttonESC.pressed() && buttonESCpressed == false) {
    escPOWER();
    buttonESCpressed = true;
  } else if (!buttonESC.pressed() && buttonESCpressed == true) {
    buttonESCpressed = false;
  } 
}
void bhResetOnFlatline()
{
  //RESET ON FLATLINE BUTTON
  if (buttonResetOnFlatline.pressed() && !buttonResetOnFlatlinePressed && clutchIn) {
    resetOnFlatline();
    buttonResetOnFlatlinePressed = true;
  } else if (!buttonResetOnFlatline.pressed() && buttonResetOnFlatlinePressed && !clutchIn) {
    buttonResetOnFlatlinePressed = false;
  }
}
void bhCentreCamera()
{
    //This button centres the camera.
    //and acts like a clutch for camera centre trim adjustment...
    if (buttonCentreCamera.pressed() && !buttonCentreCameraPressed) {
      buttonCentreCameraPressed = true;
      centreCamera();
    } else if (!buttonCentreCamera.pressed() && buttonCentreCameraPressed) {
      buttonCentreCameraPressed = false;
    }
}
void bhAutoCamera()
{
    //This button centres the camera.
    //and acts like a clutch for camera centre trim adjustment...
    autocameraOn = false; //made false in case of communication problems.
    if (buttonAutoCamera.pressed() && !buttonAutoCameraPressed) {
      buttonAutoCameraPressed = true;
      autoCamera();
    } else if (!buttonAutoCamera.pressed() && buttonAutoCameraPressed) {
      buttonAutoCameraPressed = false;
    }
}
void bhClutch()
{
  //Clutch in or out?
  if (buttonCLUTCH.pressed()) {
    clutchIn = true;
  } else if (!buttonCLUTCH.pressed()) {
    clutchIn = false;
  }
}
void adjustReverseSpeed() {
  if(gear > 2) {
    throttleMaxReverse = throttleRevMaxDefault;  
  } else if(gear < 3) {
    throttleMaxReverse = (throttleRevMaxDefault * 2) / 3;  
  }
}
void bhGearUP()
{
  if(gear < numberOfGears) {
    gear++;
    throttleMaxForward = throttleMax * (gear / numberOfGears);
    println("Gear: " + gear + ", throttleMaxForward = " + throttleMaxForward);
  }
  adjustReverseSpeed();
}
void bhGearDOWN() 
{
  if(gear > 1) {
    gear--;
    throttleMaxForward = throttleMax * (gear / numberOfGears);
    println("Gear: " + gear + ", throttleMaxForward = " + throttleMaxForward);
  }
  adjustReverseSpeed();
}
void bhGearUpOrDown() 
{
  if(clutchIn || !clutchForGears || (hasForRevGear && direction == 0)) {
    //Gear up
    if (buttonGEARUP.pressed() && buttonGEARUPpressed == false) {
      bhGearUP();
      buttonGEARUPpressed = true; 
    } else if (!buttonGEARUP.pressed() && buttonGEARUPpressed == true) {
      buttonGEARUPpressed = false; 
    }
    //gear down
    if (buttonGEARDOWN.pressed() && buttonGEARDOWNpressed == false) {
      bhGearDOWN();
      buttonGEARDOWNpressed = true; 
    } else if (!buttonGEARDOWN.pressed() && buttonGEARDOWNpressed == true) {
      buttonGEARDOWNpressed = false; 
    }
  }
}
void bhDirection()
{
  //handles gearshifter on g27...
  //buttonForward and buttonReverse ARE ALREADY defined, just not used here for now.
  if(buttonGEAR1.pressed() || buttonGEAR2.pressed() || buttonGEAR3.pressed()) {
    if(buttonForwardPressed == false) {
      direction = 1;  //0 = neutral, 1 = forward, 2 = reverse.
      buttonForwardPressed = true;
      buttonReversePressed = false;
      //println("FORWARD");
    }
  } else if (buttonGEAR2.pressed() || buttonGEAR4.pressed() || buttonGEAR6.pressed()) {
    if(buttonReversePressed == false) {
      direction = 2;  //0 = neutral, 1 = forward, 2 = reverse.
      buttonForwardPressed = false;
      buttonReversePressed = true;
      //println("REVERSE");
    }
  } else {
    if(direction != 0) {
      direction = 0; //0 = neutral, 1 = forward, 2 = reverse.
      buttonForwardPressed = false;
      buttonReversePressed = false;
      throttleSetting = throttleNeutral;
      //lastBrakeSetting = throttleNeutral;
      //println("NEUTRAL");
    }  
  }
}
void bhCalibrateSensors()
{
  if(buttonCalibrateSensors.pressed() && !buttonCalibrateSensorsPressed && clutchIn) {
    calibrateSensors(); 
    buttonCalibrateSensorsPressed = true;
  } else if (!buttonCalibrateSensors.pressed() && buttonCalibrateSensorsPressed) {
    buttonCalibrateSensorsPressed = false;
  }
}
void bhSendFFaccels()
{
  if(buttonSendFFaccels.pressed() && !buttonSendFFaccelsPressed && !clutchIn) {
    ffSendAccels(); 
    buttonSendFFaccelsPressed = true;
  } else if (!buttonSendFFaccels.pressed() && buttonSendFFaccelsPressed) {
    buttonSendFFaccelsPressed = false;
  }
}
void bhCamera() {
  if(buttonCameraMoveLeft.pressed() && buttonCameraMoveRight.pressed()) {
    if(!buttonCameraMoveLeftPressed || !buttonCameraMoveRightPressed) {
      buttonCameraMoveLeftPressed = true;
      buttonCameraMoveRightPressed = true;
      centreCamera();
    }
  } else if(buttonCameraMoveLeft.pressed()) {
    int negativeStep = int(cameraMinStep) * -1;
    moveCamera(negativeStep); 
    buttonCameraMoveLeftPressed = true;
  } else if (!buttonCameraMoveLeft.pressed() && buttonCameraMoveLeftPressed) {
    buttonCameraMoveLeftPressed = false;
  } else if (buttonCameraMoveRight.pressed()) {
    int intStep = int(cameraMinStep);
    moveCamera(intStep); 
    buttonCameraMoveRightPressed = true;
  } else if (!buttonCameraMoveRight.pressed() && buttonCameraMoveRightPressed) {
    buttonCameraMoveRightPressed = false;
  }
}
void buttonHandling()
{
  //controller-specific
  if(!hasSlidingClutch) {
    //if a button is used to activate the clutch, call the bhClutch function.
    bhClutch();        //button-handled clutch
  } 
  if (hasForRevGear) {
    //does the controller have a button that indicates the direction the
    //model should be moving? On the G27 it's the gearshifter.
    bhDirection();     //button for direction handling. Forward/Reverse.
  }
  if (hasCameraButtons) {
    bhCamera();
  }
  if (hasCameraCentreButton) {
    bhCentreCamera();
  }
 
  //Common
  bhAutoCamera();
  bhSendSettings();
  bhCalibrateSensors();
  bhSendFFaccels();
  bhGearUpOrDown();  //gear up or down.
  bhResetOnFlatline(); //switch for turning on or off automatic reset on flatline.
  bhESC();    //ESC button  
  bhStart();  //start button
}
 
////////////////////////////////////////////
//                                        //
//          CoolieHat Handling            //
//          Including cooliehats broken   //
//          into buttons                  //
//                                        //
////////////////////////////////////////////
void coolieLeft()
{
  if(buttonCOOLIELEFTpressed && !cooliePressedOnce) {
    //the clutch needs to be active to adjust the steering trim.
    cooliePressedOnce = true;
    adjustSteeringTrim(-1);
    adjustCameraTrim(-1);
    delay(400);
  } else if (!buttonCOOLIELEFTpressed && cooliePressedOnce) {
  }
}
void coolieRight()
{
  if(buttonCOOLIERIGHTpressed && !cooliePressedOnce) {
    //the clutch needs to be active to adjust the steering trim.
    cooliePressedOnce = true;
    adjustSteeringTrim(1);
    adjustCameraTrim(1);
  } else if (!buttonCOOLIERIGHTpressed && cooliePressedOnce) {
  }
}
void coolieUp()
{
 
}
void coolieDown()
{
 
}
void fauxCoolieHatHandling()
{
    if(coolieLEFT.pressed() && !buttonCOOLIELEFTpressed) {
      buttonCOOLIELEFTpressed = true;
      coolieLeft();
    } else if (!coolieLEFT.pressed() && buttonCOOLIELEFTpressed) {
      buttonCOOLIELEFTpressed = false;
      cooliePressedOnce = false;
    } else if (coolieRIGHT.pressed() && !buttonCOOLIERIGHTpressed) {
      buttonCOOLIERIGHTpressed = true;
      coolieRight();
    } else if (!coolieRIGHT.pressed() && buttonCOOLIERIGHTpressed) {
      buttonCOOLIERIGHTpressed = false;
      cooliePressedOnce = false;
    } else if (coolieUP.pressed() && !buttonCOOLIEUPpressed) {
      buttonCOOLIEUPpressed = true;
      coolieUp();
    } else if (!coolieUP.pressed() && buttonCOOLIEUPpressed) {
      buttonCOOLIEUPpressed = false;
      cooliePressedOnce = false;
    } else if (coolieDOWN.pressed() && !buttonCOOLIEDOWNpressed) {
      buttonCOOLIEDOWNpressed = true;
      coolieDown();
    } else if (!coolieDOWN.pressed() && buttonCOOLIEDOWNpressed) {
      buttonCOOLIEDOWNpressed = false;
      cooliePressedOnce = false;
    } 
}
void coolieHatHandling()
{
  //attempting to map a cooliehat
  if (cooliehat.pressed()) {
    float coolieXvalue = cooliehat.getX();
    float coolieYvalue = cooliehat.getY();
    if(coolieXvalue < 0) {
      buttonCOOLIELEFTpressed = true;
      coolieLeft();
    } else if (coolieXvalue > 0) {
      buttonCOOLIERIGHTpressed = true;
      coolieRight();
    } else if (coolieYvalue < 0) {
      buttonCOOLIEUPpressed = true;
      coolieUp();
    } else if (coolieYvalue > 0) {
      buttonCOOLIEDOWNpressed = true;
      coolieDown();
    }
    cooliehatPressed = true;
  } else if (!cooliehat.pressed() && cooliehatPressed) {
    //cooliehat is not pressed, execute "released" functions
    buttonCOOLIELEFTpressed = false;
    buttonCOOLIERIGHTpressed = false;
    buttonCOOLIEUPpressed = false;
    buttonCOOLIEDOWNpressed = false;
    coolieLeft();
    coolieRight();
    coolieUp();
    coolieDown();
    cooliehatPressed = false;
    cooliePressedOnce = false;
  }
}
 
////////////////////////////////////////////
//                                        //
//            Orchestrate                 //
//            DEVICE READING              //
//                                        //
////////////////////////////////////////////
void readControlDevice() {
  //if using this controller, execute these params, etc.
  sliderHandling();
  buttonHandling();
  if(hasCoolieHat && coolieIsCoolie) {
    coolieHatHandling();
  } else if (hasCoolieHat && !coolieIsCoolie) {
    //cooliehat is broken into buttons.
    fauxCoolieHatHandling();
  }
}
 
////////////////////////////////////////////
//                                        //
//                 LOOP                   //
//                                        //
////////////////////////////////////////////
void draw()
{
  heartbeat();
  if(hasRumblers > 0) {
    //if the controller has rumblers, call handle rumbling...
    handleRumbling();
  }
  readControlDevice();
  sendCom();
}

function fixedCropImage($originalImage, $newImageLoc, $originalImageID, $destwidth, $destheight) {
	//get the blog URL and add a slash to the end.
	$theWPurl = get_bloginfo('wpurl').'/';
	//find the relative path of the original file. This is useful if you use a testing server.
	$originalFileRelativePath = str_replace($theWPurl, '', $originalImage);
	//check to see if the file we're asking to resize exists.
	if (file_exists($originalFileRelativePath) == FALSE) {
		//if it doesn't, point to a default image.
		$originalImage = 'images/default.jpg'; //PATH MUST BE RELATIVE FOR file_exists to work!
	}
	//gather basic info about the original file, for use in making the path of the new file.
	$path_parts = pathinfo($originalImage);
	//create the path of the new file.
	$theNewImage = 'wp-content/'.$newImageLoc.'/'.$originalImageID.'-'.$destwidth.'x'.$destheight.'.'.$path_parts['extension'];
	//first, check to see if this file exists already. If it does, then don't do anything. Just return the location of the resized file.
	if (file_exists($theNewImage) == FALSE) //check to see if the file already exists. if it does, then don't perform the function!
	{	
		//dest ratio - the image dimension ratio of desired destination image!
		$destratio = $destwidth / $destheight;
		// Create image instance of $src.
		if(strtolower($path_parts['extension']) == "jpg" || strtolower($path_parts['extension']) == "jpeg"){
			$src = imagecreatefromjpeg($originalImage);
		} else if(strtolower($path_parts['extension']) == "png"){
			$src = imagecreatefrompng($originalImage);
		}
		//create instance of $dest file.
		$dest = imagecreatetruecolor($destwidth, $destheight);
		// Ifs - determine the layout of the image, portrait or landscape.
		$imageSize = getimagesize($originalImage); //returns array, 0 width, 1 height.
		$sourceRatio = $imageSize[0]/$imageSize[1];
		//determine size of sample!
		if($destratio < $sourceRatio) {
			//if the destination ratio is smaller than the source, we must use the whole height of the image and a portion of the width.
			$sampleY = round($imageSize[1]);
			$sampleX = round($sampleY * $destratio);
			//this means the sample must start on the y axis at 0, and on the x axis, somewhere in between.
			$startY = 0;
			$startX = ($imageSize[0] - $sampleX) / 2; //take sample from the middle of X axis
		} else {
			//the destination ratio is larger than the source. use whole X width and a portion of the source height.
			$sampleX = round($imageSize[0]); //always use 100% of the sample width in this case.
			$sampleY = round($sampleX / $destratio);
			//this means the sample must start on the x axis at 0, and on the y axis, somewhere in between.
			$startX = 0;
			$startY = ($imageSize[1] - $sampleY) / 2; //take sample from the middle of Y axis
		}
		//actually crop or resize the image.
		imagecopyresampled($dest, $src, 0, 0, $startX, $startY, $destwidth, $destheight, $sampleX, $sampleY);
		// Output and free from memory
		imagejpeg($dest, $theNewImage, 100);
		imagedestroy($dest);
		imagedestroy($src);	
	}
	//set up the new image location and return
	$theNewImage = get_bloginfo('wpurl').'/'.$theNewImage;
	return $theNewImage;
}

//gets the attachment id by giving it the source URL of the image or other attachment.
function get_attachment_id_from_src ($attachment_src) {
	global $wpdb;
	$query = "SELECT ID FROM {$wpdb->posts} WHERE guid='$attachment_src'";
	$id = $wpdb->get_var($query);
	return $id;
}