updated minitankpi code

minitankpi.ino

int motorspeedA;
int motorspeedB;

// connections to DRV8833
int AIN1pin = 9;
int AIN2pin = 10;

int BIN1pin = 14;
int BIN2pin = 15;

int STBYpin = 13;

#define fastdecay 0
#define slowdecay 1

#define motorA 1
#define motorB 2

int decay = fastdecay;

//#include <MPU6050_tockn.h>
#include <Wire.h>

//MPU6050 mpu6050(Wire);

void setup() {
  motorspeedA = 128;
  motorspeedB = 128;
  Serial.begin(9600);

  Wire.begin();
//  mpu6050.begin();
//  mpu6050.calcGyroOffsets(false);

  digitalWrite(11,1);
  
  pinMode(STBYpin, INPUT_PULLUP);
}

void configureMotor ( int motor , int speed ) {

  // speed = 128 = stop
  // speed = 0 = fullspeed backwards
  // speed = 255 = fullspeed forwards

  int IN1;
  int IN2;

  if (motor == motorA) {
    IN1 = AIN1pin;
    IN2 = AIN2pin;
  } else {
    IN1 = BIN1pin;
    IN2 = BIN2pin;
  }

  if (speed == 128){
    digitalWrite(IN1, 1);
    digitalWrite(IN2, 1);    
  }

  if (speed < 128) {
    digitalWrite(IN1, decay);
    analogWrite(IN2, 255-map(speed,0,127,0,255));
  }

  if (speed > 128) {
    digitalWrite(IN2, decay);
    analogWrite(IN1, map(speed,129,255,0,255));
  }

}

void configureMotorEasy ( int motor , int speed , int direction ) {
  // speed = 0 = stop 
  // speed = 255 = fullspeed
  // direction = 0 = backwards

  int newspeed;

  if (direction == 0) {
    newspeed = map(speed,0,255,128,0);
    configureMotor ( motor , newspeed );
  } else {
    newspeed = map(speed,0,255,128,255);
    configureMotor ( motor , newspeed );
  }
}


//int maxspeed = 0;
//int minspeed = 40;

int speedoffset = 40;

boolean newData = false;
const byte numChars = 32; // max command size is 31 characters for now
char receivedChars[numChars];

void recvWithEndMarker() {
    static byte ndx = 0;
    char endMarker = '\n';
    char rc;
   
    while (Serial.available() > 0 && newData == false) {
        rc = Serial.read();

        if (rc != endMarker) {
            receivedChars[ndx] = rc;
            ndx++;
            if (ndx >= numChars) {
                ndx = numChars - 1;
            }
        }
        else {
            receivedChars[ndx] = '\0'; // terminate the string
            ndx = 0;
            newData = true;
        }
    }
}

void loop() {

  // poll mpu for data
  //mpu6050.update();

  // poll for new serial data
  recvWithEndMarker();

  if (newData == true) {
    digitalWrite(11,1);
    // got new serial data
    // check first character for actual command

    switch (receivedChars[0]) {
      case 'p':
      case 'P':
       Serial.println("pong");
       break;
      case 'F':
      case 'f':
       motorspeedA = 0 + speedoffset;
       motorspeedB = 255 - speedoffset;
       break;
      case 'B':
      case 'b':
       motorspeedA = 255 - speedoffset;
       motorspeedB = 0 + speedoffset;
       break;       
      case 'S':
      case 's':
       motorspeedA = 128;
       motorspeedB = 128;
       break;
      case 'L':
      case 'l':
       motorspeedA = 255 - speedoffset;
       motorspeedB = 255 - speedoffset;
       break;
      case 'R':
      case 'r':
       motorspeedA = 0 + speedoffset;
       motorspeedB = 0 + speedoffset;
       break;
      case '1':
       speedoffset = 40;
       break;
      case '2':
       speedoffset = 35;
       break;
      case '3':
       speedoffset = 30;
       break;
      case '4':
       speedoffset = 25;
       break;
      case '5':
       speedoffset = 20;
       break;
      case '6':
       speedoffset = 15;
       break;
      case '7':
       speedoffset = 10;
       break;
      case '8':
       speedoffset = 5;
       break;
      case '9':
       speedoffset = 0;
       break;
      case 'z':
      case 'Z':
//        Serial.print("Status ");
//        Serial.print("T");
//        Serial.print(mpu6050.getTemp());
//        Serial.print(" AX");
//        Serial.print(mpu6050.getAccX());
//        Serial.print(" AY");
//        Serial.print(mpu6050.getAccY());
//        Serial.print(" AZ");
//        Serial.print(mpu6050.getAccZ());

//        Serial.print(" X");Serial.print(mpu6050.getAngleX());
//        Serial.print(" Y");Serial.print(mpu6050.getAngleY());
//        Serial.print(" Z");Serial.println(mpu6050.getAngleZ());
        
        break;
    }

    newData = false;
  }

//  motorspeed++;

//  digitalWrite(AIN2pin, LOW);
//  analogWrite(AIN1pin, motorspeed);

  //motorspeed = 128;
  //motorspeed = 30;

  // update actual motor speeds

  configureMotor(motorA ,  motorspeedA);
  configureMotor(motorB ,  motorspeedB);

  delay(50);
  digitalWrite(11,0);

//  if (motorspeed>255) { motorspeed = -1; }

}