/**
 * Prototype
 * Arduino Uno
 * L298N 
 **/

#define Apos 9
#define Aneg 10

#define Bpos 5
#define Bneg 6

volatile int Aspeed = 255;
volatile int Bspeed = 255;

volatile boolean Arunning = false;
volatile boolean Brunning = false;

void setup() {
  Serial.begin(57600);
  Serial.println("Started takeup demo");
  pinMode(Apos, OUTPUT);
  pinMode(Aneg, OUTPUT);
  pinMode(Bpos, OUTPUT);
  pinMode(Bneg, OUTPUT);
}


void loop() {
  clockwise(0);
  counter_clockwise(1);
  set_speed(0, 255); 
  set_speed(1, 255); 
  delay(5000);
  brake();
  delay(2000);
  counter_clockwise(0);
  clockwise(1);
  set_speed(0, 150);
  set_speed(1, 150);
  delay(5000);
  brake();
  delay(2000);
}

//input value from 0 to 255
void set_speed (int motor, int val){ 
  if (motor == 0) {
    Aspeed = val;
  } else if (motor == 1) {
    Bspeed = val;
  }
}

void clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
  int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
  int stepMs = round((float) ms / (float) steps);
  int motorSpeed = startSpeed;
  for (int i = 0; i < steps; i++) {
      if (motor == 0) {
        analogWrite(Apos, motorSpeed);
        analogWrite(Aneg, 0);
        Arunning = true;
      } else if (motor == 1) {
        analogWrite(Bpos, motorSpeed);
        analogWrite(Bneg, 0);
        Brunning = true;
      }
      delay(stepMs);
      motorSpeed += stopSpeed > startSpeed ? 1 : -1;
  }
  if (motor == 0) {
    Aspeed = stopSpeed;
  } else if (motor == 1) {
    Bspeed = stopSpeed;
  }
}

void counter_clockwise_fade (int motor, int startSpeed, int stopSpeed, int ms) {
  int steps = ceil((float) ms / (float) abs(stopSpeed - startSpeed));
  int stepMs = round((float) ms / (float) steps);
  int motorSpeed = startSpeed;
  if (motor == 0) {
    analogWrite(Apos, 0);
  } else if (motor == 1) {
    analogWrite(Bpos, 0);
  }
  for (int i = 0; i < steps; i++) {
      if (motor == 0) {
        analogWrite(Aneg, motorSpeed);
        Arunning = true;
      } else if (motor == 1) {
        analogWrite(Bneg, motorSpeed);
        Brunning = true;
      }
      delay(stepMs);
      motorSpeed += stopSpeed > startSpeed ? 1 : -1;
  }
  if (motor == 0) {
    Aspeed = stopSpeed;
  } else if (motor == 1) {
    Bspeed = stopSpeed;
  }
}

void clockwise (int motor){
  if (motor == 0) {
    analogWrite(Apos, Aspeed);
    analogWrite(Aneg, 0);
    Arunning = true;
  } else if (motor == 1) {
    analogWrite(Bpos, Bspeed);
    analogWrite(Bneg, 0);
    Brunning = true;
  }
}

void counter_clockwise (int motor){
  if (motor == 0) {
    analogWrite(Apos, 0);
    analogWrite(Aneg, Aspeed);
    Arunning = true;
  } else if (motor == 1) {
    analogWrite(Bpos, 0);
    analogWrite(Bneg, Bspeed);
    Brunning = true; 
  }
}

void brake (){
  if (Arunning) {
    digitalWrite(Apos, LOW);
    digitalWrite(Aneg, LOW);
    Arunning = false;
  }
  if (Brunning) {
    digitalWrite(Bpos, LOW);
    digitalWrite(Bneg, LOW);
    Brunning = false;
  }
}

/**
 * Vex = Higher voltage that can power the motor and/or the shield as well as the Arduino if wanted
 * 5V = Must be a clean 5V that can power the Arduino and shield, and if wanted also the motor itself
 * GND = Ass always, the ground of everything
 * B- = Connection of the first motor or first winding of the stepper motor
 * B+ = Connection of the first motor or first winding of the stepper motor
 * A- = Connection of the second motor or second winding of the stepper motor
 * A+ = Connection of the second motor or second winding of the stepper motor
 **/