EVShield motor control

Projects General Guidance
4

Ahh, good points - the problem might be outside of what I am specifically doing.
Here goes:
Hardware: Arduino UNO VID: 0x2341 PID: 0x0043
With EVShield-v2 by mindsensors.com.
6 AA rechargeable battery pack.

3 Lego NXT Large motors connected to ports A-M1, A-M2, B-M1.

I am using the EVShield library from here: GitHub - mindsensors/EVShield: EVShield Arduino library

My code is this:

#include <Wire.h>
#include <EVShield.h>
#include "math.h"

EVShield          evshield(0x34,0x36);
int32_t center_pos;
int carState = 0;
int timerState = 0;
unsigned long timer = 0;

void changeCarStateAfterTime(int newState, int delay)
{
  timer = millis() + delay;
  timerState = newState;
}

void setup()
{
    Serial.begin(115200);       // start serial for output
    delay(1000);                // wait two seconds, allowing time to
                                // activate the serial monitor

    evshield.init( SH_HardwareI2C );

    evshield.ledSetRGB(0,25,0);
    evshield.bank_a.motorReset();
    evshield.bank_b.motorReset();

    while (!evshield.getButtonState(BTN_GO)) {
        if (millis() % 1000 < 6) {
            Serial.println("Press GO button to continue");
        }
    }
  changeCarStateAfterTime(0, 1000);

  center_pos = evshield.bank_a.motorGetEncoderPosition(SH_Motor_2);
}

void turn_left()
{
  int32_t newPos = center_pos + 50;
  evshield.bank_a.motorRunTachometer(SH_Motor_2, SH_Direction_Forward, 100, newPos, SH_Move_Absolute, SH_Completion_Dont_Wait, SH_Next_Action_BrakeHold);
}

void turn_right()
{
  int32_t newPos = center_pos - 50;
  evshield.bank_a.motorRunTachometer(SH_Motor_2, SH_Direction_Forward, 100, newPos, SH_Move_Absolute, SH_Completion_Dont_Wait, SH_Next_Action_BrakeHold);
}

void go_straight()
{
  int32_t newPos = center_pos;
  evshield.bank_a.motorRunTachometer(SH_Motor_2, SH_Direction_Forward, 100, newPos, SH_Move_Absolute, SH_Completion_Dont_Wait, SH_Next_Action_BrakeHold);
}

void checkCarTimer(void)
{
  if (timer && millis() >= timer) {
    timer = 0;
    changeCarState(timerState);
  }
}

void goForwardInches(int inches)
{
  int degrees = 19 * inches;
  evshield.bank_a.motorRunDegrees(SH_Motor_1, SH_Direction_Forward, 30, degrees, SH_Completion_Dont_Wait, SH_Next_Action_BrakeHold);
  evshield.bank_b.motorRunDegrees(SH_Motor_1, SH_Direction_Forward, 30, degrees, SH_Completion_Dont_Wait, SH_Next_Action_BrakeHold);
}

// Two rotations = 37.5", therefore 1 degree = 0.05208". 19 degrees = 1"
void changeCarState(int newState)
{
  carState = newState;
  switch (carState)
  {
    case 0:
      go_straight();
      goForwardInches(88);  
      changeCarStateAfterTime(1, 8000);
    break;   
    case 1:
      turn_left();      
      changeCarStateAfterTime(2, 1000);
    break;
    case 2:
      goForwardInches(20);  
      changeCarStateAfterTime(3, 3000);
    break;
    case 3:
      go_straight();
      changeCarStateAfterTime(4, 1000);
    break;
    case 4:
      goForwardInches(30);  
      changeCarStateAfterTime(99, 5000);
    break;
    case 5:
      go_straight();
      changeCarStateAfterTime(5, 2000);
    break;
  default:
      evshield.bank_a.motorReset();
      evshield.bank_b.motorReset();
    break;
 }  

}

void loop()
{
  checkCarTimer();
}