arduino truck control

// modified from Michal Rinott servo by potentiometer code <http://people.interaction-ivrea.it/m.rinott>
#include <Wire.h>
#include <Servo.h>
Servo myservo;
uint16_t led[8];
char ds[8];
const byte fog = 4;
const byte beam = 2;
const byte flashleft = 8;
const byte flashright = 7;
const byte brakes = 3;
const byte turnleft = 12; //fil orange mobo truck
const byte turnright = 13; // fil blanc/orange truck
const byte avance = 10; //fil bleu mobo truck
const byte reverse = 5; // fil vert mobo truck
const byte horn = 6;
char buffer[25];
byte cam = 88;


void setup() {
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  myservo.attach(9);  // attaches the servo on pin 9 to the servo object

  Serial.begin(19200); // Speed of serial port
  Serial.flush();
  Wire.begin(0x02); // join i2c bus (address optional for master)
   Wire.onRequest(requestEvent); // register event
  digitalWrite(reverse,LOW);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,LOW);
}


void loop() {
  pan();

  if (Serial.available() > 0) {
    byte index=0;
    delay(100); // let the buffer fill up
    byte numChar=Serial.available();
    if (numChar>20) {
      numChar=20;
      }
        while (numChar--) {
          buffer[index++] = Serial.read();
          }
    splitString(buffer);
  }
  }



void pan(){
  myservo.write(cam);
  delay(15);
}


void splitString(char* data) {

   char * parameter;
   parameter = strtok (data, " ,");
   while (parameter != NULL) {
     setLED(parameter);
     parameter = strtok (NULL, " ,");
     }// Clear the text and serial buffers
       for (byte x=0; x<20; x++) {
         buffer[x]='\0';
         }
  Serial.flush();
}


void setLED(char *data) {

  Serial.println(data);

 ds[0]=data[0];
 ds[1]=data[1];
 ds[2]=data[2];
 ds[3]=data[3];
 ds[4]=data[4];
 ds[5]=data[5];
 ds[6]=data[6];
 Serial.println(ds);
  if (data[0]=='9') { //STALLED
    digitalWrite(reverse,LOW);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,LOW);
    }
  if (data[0]=='1') { //FORWARD
    digitalWrite(reverse,LOW);
    digitalWrite(avance,HIGH);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,LOW);
    }
  if (data[0]=='2') { //FORWARD-RIGHT
    digitalWrite(reverse,LOW);
    digitalWrite(avance,HIGH);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,HIGH);
    }
  if (data[0]=='3') { //RIGHT STALLED
    digitalWrite(reverse,LOW);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,HIGH);
    }
  if (data[0]=='4') { //REVERSE-RIGHT
    digitalWrite(reverse,HIGH);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,HIGH);

    }
  if (data[0]=='5') { //REVERSE
    digitalWrite(reverse,HIGH);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,LOW);
    digitalWrite(turnright,LOW);

    }
  if (data[0]=='6') { //REVERSE-LEFT
    digitalWrite(reverse,HIGH);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,HIGH);
    digitalWrite(turnright,LOW);

    }
  if (data[0]=='7') { // LEFT-STALLED
    digitalWrite(reverse,LOW);
    digitalWrite(avance,LOW);
    digitalWrite(turnleft,HIGH);
    digitalWrite(turnright,LOW);
    }
  if (data[0]=='8') { // FORWARD-LEFT
    digitalWrite(reverse,LOW);
    digitalWrite(avance,HIGH);
    digitalWrite(turnleft,HIGH);
    digitalWrite(turnright,LOW);
    }


    if ((data[1]=='9')&&(data[0]=='9')) { //BEAM off w/o FOG w/o BRAKES
      digitalWrite(beam,LOW);
      analogWrite(brakes,0);
      digitalWrite(fog,LOW);
      }
    if ((data[1]=='9')&&((data[0]=='4')||(data[0]=='5')||(data[0]=='6'))) { //BEAM off w/o FOG w BRAKES
      digitalWrite(beam,LOW);
      analogWrite(brakes,255);
      digitalWrite(fog,LOW);
      }
    if ((data[1]=='1')&&((data[0]!='4')||(data[0]!='5')||(data[0]!='6'))) { //BEAM on w/o FOG w/o BRAKES
      digitalWrite(beam,HIGH);
      analogWrite(brakes,65);
      digitalWrite(fog,LOW);
      }
    if ((data[1]=='1')&&((data[0]=='4')||(data[0]=='5')||(data[0]=='6'))) { //BEAM on w/o FOG w BRAKES
      digitalWrite(beam,HIGH);
      analogWrite(brakes,255);
      digitalWrite(fog,LOW);
      }
    if ((data[1]=='2')&&((data[0]!='4')||(data[0]!='5')||(data[0]!='6'))) { //BEAM on w FOG w/o BRAKES
      digitalWrite(beam,HIGH);
      analogWrite(brakes,65);
      digitalWrite(fog,HIGH);
      }
    if ((data[1]=='2')&&((data[0]=='4')||(data[0]=='5')||(data[0]=='6'))) { //BEAM on w FOG w BRAKES
      digitalWrite(beam,HIGH);
      analogWrite(brakes,255);
      digitalWrite(fog,HIGH);
      }

   if (data[1]=='3') { //cam full left
     cam = 0;
      }
   if (data[1]=='4') { //Cam front left
     cam = 45;
      }
   if (data[1]=='5') { //Cam full front
     cam = 88;
      }
       if (data[1]=='6') { //Cam front right
     cam = 135;
      }
       if (data[1]=='7') { //cam full right
     cam = 180;
      }
      if (data[2]=='9') { //ALL FLASHER off
        digitalWrite(flashleft,LOW);
        digitalWrite(flashright,LOW);
        }
      if (data[2]=='1') { //LEFT FLASHER on
        digitalWrite(flashright,LOW);
        digitalWrite(flashleft,HIGH);
        }
      if (data[2]=='2') { //RIGHT FLASHER on
        digitalWrite(flashleft,LOW);
        digitalWrite(flashright,HIGH);
        }

         if (data[3]=='9'){ //turn off horn
         digitalWrite(horn,LOW);
         }

        if  (data[3]=='1'){ //turn on horn
        digitalWrite(horn,HIGH);
        }

}


void requestEvent() {
  Wire.write(ds); // respond with message of 6 bytes
  // as expected by master
}