// Arduino pins for the shift register
#define MOTORLATCH 12
#define MOTORCLK 4
#define MOTORENABLE 7
#define MOTORDATA 8

// 8-bit bus after the 74HC595 shift register
// (not Arduino pins)
// These are used to set the direction of the bridge driver.
#define MOTOR1_A 2
#define MOTOR1_B 3
#define MOTOR2_A 1
#define MOTOR2_B 4
#define MOTOR3_A 5
#define MOTOR3_B 7
#define MOTOR4_A 0
#define MOTOR4_B 6

// Arduino pins for the PWM signals.
#define MOTOR1_PWM 11
#define MOTOR2_PWM 3
#define MOTOR3_PWM 6
#define MOTOR4_PWM 5
#define SERVO1 10
#define SERVO2 9

void shiftWrite(int output, int high_low)
{
  static int latch_copy;
  static int shift_register_initialized = false;

  // Do the initialization on the fly, 
  // at the first time it is used.
  if (!shift_register_initialized)
  {
    // Set pins for shift register to output
    pinMode(MOTORLATCH, OUTPUT);
    pinMode(MOTORENABLE, OUTPUT);
    pinMode(MOTORDATA, OUTPUT);
    pinMode(MOTORCLK, OUTPUT);

    // Set pins for shift register to default value (low);
    digitalWrite(MOTORDATA, LOW);
    digitalWrite(MOTORLATCH, LOW);
    digitalWrite(MOTORCLK, LOW);
    // Enable the shift register, set Enable pin Low.
    digitalWrite(MOTORENABLE, LOW);

    // start with all outputs (of the shift register) low
    latch_copy = 0;

    shift_register_initialized = true;
  }

  // The defines HIGH and LOW are 1 and 0.
  // So this is valid.
  bitWrite(latch_copy, output, high_low);

  // Use the default Arduino 'shiftOut()' function to
  // shift the bits with the MOTORCLK as clock pulse.
  // The 74HC595 shiftregister wants the MSB first.
  // After that, generate a latch pulse with MOTORLATCH.
  shiftOut(MOTORDATA, MOTORCLK, MSBFIRST, latch_copy);
  delayMicroseconds(5);    // For safety, not really needed.
  digitalWrite(MOTORLATCH, HIGH);
  delayMicroseconds(5);    // For safety, not really needed.
  digitalWrite(MOTORLATCH, LOW);
}

void motors_init() {
  //PWMS
  pinMode(MOTOR1_PWM, OUTPUT);
  pinMode(MOTOR2_PWM, OUTPUT);
  pinMode(MOTOR3_PWM, OUTPUT);
  pinMode(MOTOR4_PWM, OUTPUT);
}

void motor1_set(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    shiftWrite(MOTOR1_A, 0);
    shiftWrite(MOTOR1_B, 0);
  } else if (speed < 0) {
    shiftWrite(MOTOR1_A, 0);
    shiftWrite(MOTOR1_B, 1);
    speed *= -1;
  } else {    
    shiftWrite(MOTOR1_A, 1);
    shiftWrite(MOTOR1_B, 0); 
  }
  analogWrite(MOTOR1_PWM, speed);
}
void motor2_set(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    shiftWrite(MOTOR2_A, 0);
    shiftWrite(MOTOR2_B, 0);
  } else if (speed < 0) {
    shiftWrite(MOTOR2_A, 0);
    shiftWrite(MOTOR2_B, 1);
    speed *= -1;
  } else {
    shiftWrite(MOTOR2_A, 1);
    shiftWrite(MOTOR2_B, 0);
  }
  analogWrite(MOTOR2_PWM, speed);
}
void motor3_set(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    shiftWrite(MOTOR3_A, 0);
    shiftWrite(MOTOR3_B, 0);
  } else if (speed < 0) {
    shiftWrite(MOTOR3_A, 0);
    shiftWrite(MOTOR3_B, 1);
    speed *= -1;
  } else {
    shiftWrite(MOTOR3_A, 1);
    shiftWrite(MOTOR3_B, 0);
  }
  analogWrite(MOTOR3_PWM, speed);
}
void motor4_set(int speed) {
  speed = constrain(speed, -255, 255);
  if (speed == 0) {
    shiftWrite(MOTOR4_A, 0);
    shiftWrite(MOTOR4_B, 0);
  } else if (speed < 0) {
    shiftWrite(MOTOR4_A, 0);
    shiftWrite(MOTOR4_B, 1);
    speed *= -1;
  } else {
    shiftWrite(MOTOR4_A, 1);
    shiftWrite(MOTOR4_B, 0);
  }
  analogWrite(MOTOR4_PWM, speed);
}
void motor1_stop() {
  shiftWrite(MOTOR1_A, 1);
  shiftWrite(MOTOR1_B, 1);
}
void motor2_stop() {
  shiftWrite(MOTOR2_A, 1);
  shiftWrite(MOTOR2_B, 1);
}
void motor3_stop() {
  shiftWrite(MOTOR3_A, 1);
  shiftWrite(MOTOR3_B, 1);
}
void motor4_stop() {
  shiftWrite(MOTOR4_A, 1);
  shiftWrite(MOTOR4_B, 1);
}
void go(int m1, int m2, int m3, int m4) {
  motor1_set(m1);
  motor2_set(m2);
  motor3_set(m3);
  motor4_set(m4);
}
void motors_stop() {
  motor1_stop();
  motor2_stop();
  motor3_stop();
  motor4_stop();
}
void setup() {
  motors_init();
}
void loop() {
  go(255, 255, 255, 255);
}