#define xPin A5
#define yPin A6
#define leftMax 255//215
#define leftMin -255//-100
#define rightMax 255
#define rightMin -255

volatile int16_t xValue = 0;
volatile int16_t yValue = 0;
volatile int16_t leftPWM = 0;
volatile int16_t rightPWM = 0;

const int16_t deadZone = 40;

void motorMapping() {
  static long temp = millis();
  koordinaten(analogRead(xPin), analogRead(yPin));
  motorPWM();
  
  pwmB = map(leftPWM, -255,255,leftMin,leftMax);
  pwmA = map(rightPWM, -255,255,rightMin,rightMax);
  if((millis() - temp) > 100) {
 //   lcd.clear();
 //   lcd.println("Links: ");
    lcdLines[0] = "Links: " + String(pwmB, DEC);
 //   lcd.println(pwmB);
 //   lcd.gotoXY(0,2);
 //   lcd.println("Rechts: ");
    lcdLines[1] = "Rechts: " + String(pwmA, DEC);
 //  lcd.println(pwmA);
  }

  senden();
}

void senden() {
        commands[0] = speedA;
        commands[1] = highByte(pwmA);
        commands[2] = lowByte(pwmA);
        commands[3] = speedB;
        commands[4] = highByte(pwmB);
        commands[5] = lowByte(pwmB);
        commands[6] = timeToDrive;
        commands[7] = highByte(driveTimeout);
        commands[8] = lowByte(driveTimeout);
        commands[9] = goDrive;
		    commands[10] = getDistance;
        radio.write(&commands, sizeof(commands));
}

void koordinaten(uint16_t x, uint16_t y) {
  //9-bit reichen, der ADC schafft bestenfalls 8-bit praezision
  x = x >> 1;
  y = y >> 1;
  xValue = map(x, 0, 511, -255, 255);
  yValue = map(y, 0, 511, 255, -255);
}

void motorPWM() {
 if((abs(xValue) > deadZone) || (abs(yValue) > deadZone)) {
  if(yValue >= 0) {
    if(xValue >= 0) {
    //+y , +x  
      leftPWM = yValue;
      rightPWM = yValue - xValue;
      if(xValue >= yValue) {
        leftPWM = 255;
        rightPWM = -255;
      }
    } else {
      //+y , -x
        leftPWM = yValue;
        rightPWM = yValue + xValue;
        if(abs(xValue) >= yValue) {
          leftPWM = -255;
          rightPWM = 255;
        }
    }
  } else {
      if(xValue >= 0) {
      //-y , +x  
        leftPWM = yValue;
        rightPWM = yValue + xValue;
        if(xValue >= abs(yValue)) {
          leftPWM = 255;
          rightPWM = -255;
        }
      } else {
        //-y , -x
          leftPWM = yValue;
          rightPWM = yValue - xValue;
          if(abs(xValue) >= abs(yValue)) {
            leftPWM = -255;
            rightPWM = 255;
          }
      }
  }
  if(abs(xValue) < deadZone) {
    leftPWM = yValue;
    rightPWM = yValue;
  }
 } else {
    leftPWM = 0;
    rightPWM = 0;
 }
}