void setup() {

pinMode(7, INPUT);
attachInterrupt(digitalPinToInterrupt(2), count, RISING);
Serial.begin(115200);

pinMode(12, OUTPUT); // Direction

pinMode(3, OUTPUT); // Voltage
pinMode(9, OUTPUT); // Brake
pinMode(10, INPUT_PULLUP); // Switch
digitalWrite(9, LOW); // Disable Brake

while (digitalRead(10)) {};

delay(1000);
}

volatile long int pulses = 0;

long int initial_pulses = 0, dPulses = 0;
double v_motor, current_rpm = 0, error;
int target;
unsigned long int t0 = 0, initialPulses = 0, delta_pulses = 0, delta_t, t;

void loop() {
// 1ST APPROACH at 35mm/s
// 262.5rpm

t = micros();
v_motor = 175;
Serial.println(" 1st Approach");
while (digitalRead(10) == HIGH) {
target = 262;
digitalWrite(12, HIGH);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = (target - current_rpm);
v_motor = v_motor + 0.1 * error;

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
 Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

delayMicroseconds(3000);

// 1ST RETREAT 10mm at 20 mm/s

// 467.5 pulses and 150 rpm
v_motor = 40;
Serial.println(" 1st Retreat");
initial_pulses = pulses;
while (dPulses <= 467) {
target = 150;
dPulses = initial_pulses - pulses;
digitalWrite(12, LOW);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = target - current_rpm;
v_motor = v_motor + 0.1 * error;

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

}
delayMicroseconds(3000);

// 2ND APPROACH at lowest speed possible

v_motor = 40;
Serial.println(" 2nd Approach");
while (digitalRead(10) == HIGH) {
digitalWrite(12, HIGH);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

delayMicroseconds(3000);

// 2ND RETREAT 30 mm at 10 mm/s

// 1402.5 pulses and 75 rpm
dPulses = 0;
Serial.println(" 2nd Retreat");
initial_pulses = (pulses);
v_motor = 55;
while (abs(dPulses) <= 1402) {
dPulses = (initial_pulses) - (pulses);
target = 75;
digitalWrite(12, LOW);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = target - current_rpm;
v_motor = v_motor + 0.1 * error;
 Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

// Motor stops
digitalWrite(3, 0);
delay(1000);

while (true);
}

void count() {
if (digitalRead(7) == LOW)
pulses++;
else
pulses--;
}

double getSpeed(long int us) {

long int t0 = 0, initialPulses = 0, delta_pulses = 0, delta_t;
double rpm = 0;
initialPulses = pulses; // Set initial pulse count
t0 = micros(); // Set initial time, in us
while (micros() - t0 < us) {} // Allow sampling time (us) to elapse
delta_t = us; // Duration of the sample, in microseconds(passed as argument)
delta_pulses = abs(pulses - initialPulses); // Pulse increase during sample period
 rpm = double(delta_pulses) / double(delta_t) * 1E+6 / 374.0 * 60.0; // w=d(theta)/dt, convert
to r.p.m.
// Encoder gives 374 pulses/rev
return (rpm);
}