// Motor steering for Arduino Cars

int thrustPin = A6;
int steerPin = A7;
int thrust = 0, steer = 0;

// LEFT motor
int IN1 = 8; // forward, DIO, only HIGH or LOW
int IN2 = 7; // reverse, DIO, only HIGH or LOW
int ENA = 6; // enable,  PWM, 0 - 255

// RIGHT motor
int IN3 = 2; // forward, DIO, only HIGH or LOW
int IN4 = 4; // reverse, DIO, only HIGH or LOW
int ENB = 3; // enable,  PWM, 0 - 255

// adjustable PWM, range 0 (no speed) - 255 (full speed)
int speed = 63; // slow
int speedL=0, speedR=0;
int yaw = 0;
// int speed = 127; // medium
// int speed = 255; // maximum

int thisDelay = 1000; // delay during motor movement

void setup() {
  Serial.begin(9600); // for Serial Monitor

  pinMode(ENA, OUTPUT); // configure ENABLE pins for output
  pinMode(ENB, OUTPUT);

  pinMode(IN1, OUTPUT); // configure MOTOR DRIVER pins for output
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  digitalWrite(ENA, speed); // PWM speed control, 0 - 255
  digitalWrite(ENB, speed);

  // functions in setup() run only one time.
  if(0){
  allStop();
  forwardSkidLeft();
  reverseSkidRight();// placed this out of order to keep car stationary
  forwardSkidRight();
  reverseSkidLeft();
  rotateLeft();
  rotateRight();
  forward();
  reverse();
  allStop();
  }
}

void loop() {
  // functions in setup() run only one time.
  // allStop();
  // forwardSkidLeft();
  // reverseSkidRight();// placed this out of order to keep car stationary
  // forwardSkidRight();
  // reverseSkidLeft();
  // rotateLeft();
  // rotateRight();
  // forward();
  // reverse();
  // allStop();
  readJoystick();
}

//**************************************************
// MOTOR DIRECTIONS
//**************************************************
//  ENA   ENB   IN1   IN2   IN3   IN4
//  HIGH  HIGH  HIGH  LOW   LOW   HIGH  forward - left forward, right forward
//  HIGH  HIGH  LOW   HIGH  HIGH  LOW   reverse - left reverse, right reverse
//  HIGH  HIGH  LOW   HIGH  LOW   HIGH  face left - left reverse, right forward
//  HIGH  HIGH  HIGH  LOW   HIGH  LOW   face right - left forward, right reverse
//  HIGH  HIGH  HIGH  LOW   LOW   LOW   skid left - stop left, forward right
//  HIGH  HIGH  LOW   LOW   LOW   LOW   stop - all LOW
//  HIGH  HIGH  HIGH  HIGH  HIGH  HIGH  stop - all HIGH
//  LOW   LOW   N/A   N/A   N/A   N/A   stop - both ENABLE LOW
//
// *** LEFT motor and RIGHT motor are facing opposite ***
// *** LEFT motor will use HIGH, LOW to go forward ***
// *** RIGHT motor will use LOW, HIGH to go forward ***

//**************************************************
// MOTOR FUNCTIONS
//**************************************************
void driveMotor(bool p1, bool p2, bool p3, bool p4) {
  digitalWrite(IN1, p1);
  digitalWrite(IN2, p2);
  digitalWrite(IN3, p3);
  digitalWrite(IN4, p4);
  delay(thisDelay);
}

void allStop() {
  Serial.println("All Stop.");
  driveMotor(LOW, LOW, LOW, LOW);
  delay(2000); // allow motor to be stopped
}

void forwardSkidLeft() {
  Serial.print("\tForward Skid-Left. ");
  driveMotor(LOW, LOW, LOW, HIGH); // left motor stop, right motor forward
}

void forwardSkidRight() {
  Serial.print("\tForward Skid-Right. ");
  driveMotor(HIGH, LOW, LOW, LOW); // left motor forward, right motor off
}

void reverseSkidLeft()  {
  Serial.print("\tReverse Skid-Left.\n");
  driveMotor(LOW, HIGH, LOW, LOW); // left motor reverse, right motor stop
}

void reverseSkidRight() {
  Serial.print("\tReverse Skid-Right.\n");
  driveMotor(LOW, LOW, HIGH, LOW); // left motor off, right motor reverse
}

void rotateLeft() {
  Serial.print("\tRotate Left. ");
  driveMotor(LOW, HIGH, LOW, HIGH); // left motor reverse, right motor forward
}

void rotateRight() {
  Serial.print("\t\tRotate Right.\n");
  driveMotor(HIGH, LOW, HIGH, LOW); // left motor forward, right motor reverse
}

void forward() {
  Serial.print("\tForward. ");
  driveMotor(HIGH, LOW, LOW, HIGH); // left motor forward, right motor forward
}

void reverse() {
  Serial.print("\t\tReverse.\n");
  driveMotor(LOW, HIGH, HIGH, LOW ); // left motor reverse, right motor reverse
}

void readJoystick(void) {
  static uint32_t last = 0;
  uint32_t now = millis();
  static int lastSteer = 0;
  static int lastThrust = 0;
  if (now - last >= 100) {
    thrust = analogRead(thrustPin) / 4 ;
    steer = (analogRead(steerPin) - 512) / 2;
    yaw = steer/2;
    if (steer != lastSteer || thrust != lastThrust) {
      lastThrust = thrust;
      lastSteer = steer;
      int thrustYawCode = 10 * ((thrust > 0) - (thrust < 0)) + ((yaw > 0) - (yaw < 0));
      switch (thrustYawCode) {
        default:
          Serial.print("default with thrustYawCode:");
          Serial.println(thrustYawCode);
        case 0 :
          allStop();
          speedL = speedR = 0;
          break;
        case -10:
        case -9:
        case-11 :
          reverse();
          speedL = -(thrust - yaw);
          speedR = -(thrust + yaw);
          break;
        case +10:
        case 9:
        case 11:
          forward();
          speedL = thrust + yaw;
          speedR = thrust - yaw;
          break;
        case -1:
          rotateLeft();
          speedL = speedR = yaw;
          break;
        case 1:
          rotateRight();
          speedL = speedR = yaw;
      }
      updatePwms();
      Serial.print("Thrust:");
      Serial.print(thrust);
      Serial.print("steer:");
      Serial.print(steer);
      Serial.print(" pwmL:");
      Serial.print(speedL);
      Serial.print(" pwmR:");
      Serial.print(speedR);
      Serial.println();
    }
  }

}
void updatePwms() {
  speedL = constrain(speedL,0,255);
  speedR = constrain(speedR,0,255);
  analogWrite(ENA, speedL);
  analogWrite(ENB, speedR);

}