Autonomous Driving Car

#include <IfxMotorControlShield.h>


// servo pin
int servo = 8;

// variables for servo control
int pwm;
int Servo_pos = 0;
int Servo_pos_aktuell;

// serial communication
String inString = "";
int neg;

//variables for motor control 
int speedvalue = 0;
int speedvalue_aktuell;
int speedflag = false;


void servoMove(int servo, int Servo_pos){
  pwm = (Servo_pos) + 1500;
  digitalWrite(servo, HIGH);
  delayMicroseconds(pwm);
  digitalWrite(servo, LOW);
}

//i2c communication
int on1;
int off1;

int on0;
int off0;

int r;        //register
int x;        //value

int received;

#include <Wire.h>

void setup() {
//communications
  Wire.begin(64);                // join i2c bus with address #8
  Wire.onReceive(receiveEvent);  // register event
  Wire.onRequest(requestEvent);  // register event
  Serial.begin(115200);          // start serial for output
  delay(10);
//servo
 pinMode(servo, OUTPUT);
 servoMove(servo, Servo_pos);

//motor   
 if(ifxMcsBiDirectionalMotor.begin())
    Serial.println("Init Fail!");
  Serial.println("Initialising speed");
  ifxMcsBiDirectionalMotor.setBiDirectionalSpeed(speedvalue);
  Serial.println("Starting motor");
  ifxMcsBiDirectionalMotor.start();

  
  Serial.println("Setup Done");
}

void loop() {
    
if(received == 0) {
  Serial.println("fail!");
}
  
//servo
  if(abs(Servo_pos_aktuell - Servo_pos)<200){
  servoMove(servo, Servo_pos);   
  }
Servo_pos_aktuell = Servo_pos; 

//motor
 if(abs(speedvalue_aktuell - speedvalue)<200){
 ifxMcsBiDirectionalMotor.setBiDirectionalSpeed(speedvalue*0.4);
    //if speed was set to 0, the motor has to be restarted!
      if(!ifxMcsBiDirectionalMotor.getRunning())
        ifxMcsBiDirectionalMotor.start();   
  }
speedvalue_aktuell = speedvalue; 
 
  delay(50);
 
}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent() {
  Wire.write(0x01); // respond with 0x01 as expected by master
  Serial.println("0x01");
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
  if(howMany == 2){
  received = true;
  }
  else{
    received = false;
  }
     
// read
  while (1 < Wire.available()) { // loop through first of two bytes
      r = Wire.read();           // receive register byte
  }
      x = Wire.read();           // receive Value byte as an integer


//process
  switch (r) {  
    case 8:
      off0 = x;
      speedflag = true;
      break;
    case 9:
      if (speedflag == true) {
      off0 = off0+(x<<8);
      speedvalue = off0-255;
      speedflag = false;
      }   
      break;
    case 12:
      off1 = x;
      break;
    case 13:
      off1 = off1+(x<<8);
      Servo_pos = off1-600;
      break;   
 }
}