/**
 *  This program tests the ultrasonic
 *  distance sensor
 * 
 * @author Addison Sears-Collins
 * @version 2.0 2021-04-11
 */
 
/* Give a name to a constant value before
 * the program is compiled. The compiler will 
 * replace references to Trigger and Echo with 
 * 11 and 12, respectively, at compile time.
 * These defined constants don't take up 
 * memory space on the Arduino.
 */
#define Trigger 11
#define Echo 12
 
/*   
 *  This setup code is run only once, when 
 *  Arudino is supplied with power.
 */
void setup(){
 
  // Set the baud rate to 9600. 9600 means that 
  // the serial port is capable of transferring 
  // a maximum of 9600 bits per second.
  Serial.begin(9600);
 
  // Define each pin as an input or output.
  pinMode(Echo, INPUT);
  pinMode(Trigger, OUTPUT);
}
 
void loop(){
 
  // Make the Trigger LOW (0 volts) 
  // for 2 microseconds
  digitalWrite(Trigger, LOW);
  delayMicroseconds(2);
 
  // Emit high frequency 40kHz sound pulse
  // (i.e. pull the Trigger) 
  // by making Trigger HIGH (5 volts) 
  // for 10 microseconds
  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigger, LOW); 
 
  // Detect a pulse on the Echo pin 8. 
  // pulseIn() measures the time in 
  // microseconds until the sound pulse
  // returns back to the sensor.
  int distance = pulseIn(Echo, HIGH);
 
  // Speed of sound is:
  // 13511.811023622 inches per second
  // 13511.811023622/10^6 inches per microsecond
  // 0.013511811 inches per microsecond
  // Taking the reciprocal, we have:
  // 74.00932414 microseconds per inch 
  // Below, we convert microseconds to inches by 
  // dividing by 74 and then dividing by 2
  // to account for the roundtrip time.
  distance = distance / 74 / 2;
 
  // Print the distance in inches
  Serial.println(distance);
 
  // Pause for 100 milliseconds
  delay(100);
}

/**
 * This robot avoids obstacles 
 * using an ultrasonic sensor.
 * 
 * @author Addison Sears-Collins
 * @version 1.0 2021-04-11
 */
 
// Motor A connections
const int enA = 9;
const int in1 = 5;
const int in2 = 6;

// Motor B connections
const int enB = 10;
const int in3 = 7;
const int in4 = 8;

// Set the speed (0 = off and 255 = max speed)
// If your wheels are not moving, check your connections,
// or increase the speed.
const int motorSpeed = 80;
 
/* Give a name to a constant value before
 * the program is compiled. The compiler will 
 * replace references to Trigger and Echo with 
 * 11 and 12, respectively, at compile time.
 * These defined constants don't take up 
 * memory space on the Arduino.
 */
#define Trigger 11
#define Echo 12
 
/*   
 *  This setup code is run only once, when 
 *  Arudino is supplied with power.
 */
void setup(){
   
  // Set the baud rate to 9600. 9600 means that 
  // the serial port is capable of transferring 
  // a maximum of 9600 bits per second.
  //Serial.begin(9600);
 
  // Motor control pins are outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  // Turn off motors - Initial state
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  // Set the motor speed
  analogWrite(enA, motorSpeed); 
  analogWrite(enB, motorSpeed); 
 
  // Define each pin as an input or output.
  pinMode(Echo, INPUT);
  pinMode(Trigger, OUTPUT);
 
  // Initializes the pseudo-random number generator
  // Needed for the robot to wander around the room
  randomSeed(analogRead(3));
 
  delay(200);     // Pause 200 milliseconds               
  go_forward();   // Go forward
}
 
/*
 * This is the main code that runs again and again while
 * the Arduino is connected to power.
 */
void loop(){
  int distance = doPing();
 
  // If obstacle <= 16 inches away
  if (distance >= 0 && distance <= 16) {  
      
    //Serial.println("Obstacle detected ahead");  
    go_backwards();   // Move in reverse
    delay(2000);
 
    /* Go left or right to avoid the obstacle*/
    if (random(2) == 0) {  // Generates 0 or 1, randomly        
      go_right();  // Turn right
    }
    else {
      go_left();  // Turn left
    }
    delay(3000);
    go_forward();  // Move forward
  }
  delay(50); // Wait 50 milliseconds before pinging again
}
 
/*
 * Returns the distance to the obstacle as an integer
 */
int doPing () {
  int distance = 0;
  int average = 0;
 
  // Grab four measurements of distance and calculate
  // the average.
  for (int i = 0; i < 4; i++) {
 
    // Make the Trigger LOW (0 volts) 
    // for 2 microseconds
    digitalWrite(Trigger, LOW);
    delayMicroseconds(2);
 
     
    // Emit high frequency 40kHz sound pulse
    // (i.e. pull the Trigger) 
    // by making Trigger HIGH (5 volts) 
    // for 10 microseconds
    digitalWrite(Trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(Trigger, LOW);
      
    // Detect a pulse on the Echo pin 8. 
    // pulseIn() measures the time in 
    // microseconds until the sound pulse
    // returns back to the sensor.    
    distance = pulseIn(Echo, HIGH);
 
    // Speed of sound is:
    // 13511.811023622 inches per second
    // 13511.811023622/10^6 inches per microsecond
    // 0.013511811 inches per microsecond
    // Taking the reciprocal, we have:
    // 74.00932414 microseconds per inch 
    // Below, we convert microseconds to inches by 
    // dividing by 74 and then dividing by 2
    // to account for the roundtrip time.
    distance = distance / 74 / 2;
 
    // Compute running sum
    average += distance;
 
    // Wait 10 milliseconds between pings
    delay(10);
  }
 
  // Return the average of the four distance 
  // measurements
  return (average / 4);
}
 
/*   
 *  Forwards, backwards, right, left, stop.
 */
void go_forward() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void go_backwards() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void go_right() {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
}
void go_left() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
}
void stop_all() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

/*
 * Author: Automatic Addison
 * Website: https://automaticaddison.com
 * Description: Calculate the angular velocity in radians/second of a DC motor
 * with a built-in encoder (forward = positive; reverse = negative) 
 */

// Motor encoder output pulses per 360 degree revolution (measured manually)
#define ENC_COUNT_REV 620

// Encoder output to Arduino Interrupt pin. Tracks the pulse count.
#define ENC_IN_RIGHT_A 2

// Other encoder output to Arduino to keep track of wheel direction
// Tracks the direction of rotation.
#define ENC_IN_RIGHT_B 4

// True = Forward; False = Reverse
boolean Direction_right = true;

// Keep track of the number of right wheel pulses
volatile long right_wheel_pulse_count = 0;

// One-second interval for measurements
int interval = 1000;
 
// Counters for milliseconds during interval
long previousMillis = 0;
long currentMillis = 0;

// Variable for RPM measuerment
float rpm_right = 0;

// Variable for angular velocity measurement
float ang_velocity_right = 0;
float ang_velocity_right_deg = 0;

const float rpm_to_radians = 0.10471975512;
const float rad_to_deg = 57.29578;

void setup() {

  // Open the serial port at 9600 bps
  Serial.begin(9600); 

  // Set pin states of the encoder
  pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP);
  pinMode(ENC_IN_RIGHT_B , INPUT);

  // Every time the pin goes high, this is a pulse
  attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_pulse, RISING);
  
}

void loop() {

  // Record the time
  currentMillis = millis();

  // If one second has passed, print the number of pulses
  if (currentMillis - previousMillis > interval) {

    previousMillis = currentMillis;

    // Calculate revolutions per minute
    rpm_right = (float)(right_wheel_pulse_count * 60 / ENC_COUNT_REV);
    ang_velocity_right = rpm_right * rpm_to_radians;   
    ang_velocity_right_deg = ang_velocity_right * rad_to_deg;
    
    Serial.print(" Pulses: ");
    Serial.println(right_wheel_pulse_count);
    Serial.print(" Speed: ");
    Serial.print(rpm_right);
    Serial.println(" RPM");
    Serial.print(" Angular Velocity: ");
    Serial.print(rpm_right);
    Serial.print(" rad per second");
    Serial.print("\t");
    Serial.print(ang_velocity_right_deg);
    Serial.println(" deg per second");
    Serial.println();

    right_wheel_pulse_count = 0;
  
  }
}

// Increment the number of pulses by 1
void right_wheel_pulse() {
  
  // Read the value for the encoder for the right wheel
  int val = digitalRead(ENC_IN_RIGHT_B);

  if(val == LOW) {
    Direction_right = false; // Reverse
  }
  else {
    Direction_right = true; // Forward
  }
  
  if (Direction_right) {
    right_wheel_pulse_count++;
  }
  else {
    right_wheel_pulse_count--;
  }
}