Safety ebike electronic circuit

/*
 * proyect name: safety perfomance ebike circuit
 * author: adrian gonzalez
 * last date modified: 08/05/2022
 * 
 * This proyect has been developed for the Spresense Challenge organized by hacksterio
 * It aims to be a tutorial for future Spresense Developers
 * 
 * The purpose of this proyect is to design a simple electronic circuit to improve safety measures of an ebike,
 * and to serve as a guide to learn how to use Sony's Spresense board.
 * 
 * It consist of two main subsystems:
 *   A system to detect presence at the rear back of a cyclist, indicating the unseen danger
 *   Another system to assist breaking if an obstacle appears to be close at the front
 * 
 * In this project the following elements have been used:
 * 2 ultrasonics sensors, 2 servos, 1 red LED, 1 yellow LED, resistors and wires
 * 
 */

// to use the servos we include the corresponding library

#include <Servo.h>

Servo myservo1;  // we create our servo objects
Servo myservo2;  

const int Trigger1 = 2;   // for the first ultrasonic sensor we configure its corresponding signals
const int Echo1 = 3;      // to the PIN_D02 and PIN_D03 from the Spresense extension board

const int Trigger2 = 4;   // for the first ultrasonic sensor we configure its corresponding signals
const int Echo2 = 5;      // to the PIN_D02 and PIN_D03 from the Spresense extension board

const int yellowLedPIN = 7;   // to set the PINs for the LEDs, PIN_D07 and PIN_D08 have been chosen
const int redLedPIN = 8;      // in order to protect both LEDs a resistor has been added in series

// we set the values for our safety distance thresholds
// the sensor works better within 200cm 
// so smaller distance wre to be selected to avoid sensing measurements

const int forwardDistanceWarningThreshold = 150;
const int forwardDistanceDangerThreshold = 90;
const int backwardDistanceWarningThreshold = 130;
const int backwardDistanceDangerThreshold = 60;

void setup() {
  myservo1.attach(6);    // to use a servo as intended, a PWM port must be selected
  myservo2.attach(9);    // in this project PIN_D06 and PIN_D09 have been selected

  Serial.begin(9600);          // used to begin serial communication
  
  pinMode(Trigger1, OUTPUT);   // to use the ultrasonic sensor a pulse must be triggered, so we set up its respective port as an output
  pinMode(Echo1, INPUT);       // a pulse must be collected from the sensor, so we set it up as an input
  digitalWrite(Trigger1, LOW); // we start with the sensor inactive

  pinMode(Trigger2, OUTPUT);   // the second ultrasonic sensor must be set up as the one just done above
  pinMode(Echo2, INPUT); 
  digitalWrite(Trigger2, LOW);

  pinMode(yellowLedPIN , OUTPUT);  // both LEDs will be used as outputs
  pinMode(redLedPIN , OUTPUT);  
}

void loop() {

  // we create some variables to measure our distances
  
  long t_forward; // time detected by the front ultrasonic sensor
  long d_forward; // distance converted from the front sensor
  long t_backward; // time detected by the back ultrasonic sensor
  long d_backward; // distance converted from the back sensor

   /*
   * DISTANCE-TIME RELATION
   * 
   * the time measurement unit in this program is 1us (microsecond)
   * 2 times the distance is what takes air at normal airspeed to move to the obstacle and back, therefore:
   * 
   * d(cm) = 340(m/s)*(100cm/1m)*(1s/1000000us)/2 = t(us)/59 approximately
   * 
   */

  digitalWrite(Trigger1, HIGH);          // to make the ultrasonic sensor work we must send an impulse signal
  delayMicroseconds(10);          
  digitalWrite(Trigger1, LOW);
  
  t_forward = pulseIn(Echo1, HIGH);      // we get the time gap detected by the sensor and we get its respective measured distance
  d_forward = t_forward/59;           

  digitalWrite(Trigger2, HIGH);          // same as before
  delayMicroseconds(10);          
  digitalWrite(Trigger2, LOW);
  
  t_backward = pulseIn(Echo2, HIGH); 
  d_backward = t_backward/59;             
  
  Serial.print("Front distance: ");      // we transfer the front-distance data
  Serial.print(d_forward);         
  Serial.print("cm");
  Serial.println();
  
  Serial.print("Back distance: ");       // we transfer the back-distance data
  Serial.print(d_backward);     
  Serial.print("cm");
  Serial.println();
  
  delay(80);                             // small delay of microseconds
  
  if (d_forward < forwardDistanceWarningThreshold) {
    if (d_forward < forwardDistanceDangerThreshold) {
      myservo1.write(180);                            // when an object is detected to be to close, the servos will rotate 180 degrees, forcing the bicycle to brake completely
      myservo2.write(180);                  
    }
    else {
      myservo1.write(90);                             // when there is an obstacle ahead, both servos brake the bicycle slightly, reducing its speed
      myservo2.write(90);                  
    }
  }
  else {
    myservo1.write(0);                                // both servos stay inactive when the front is free of danger, without pulling the brakes
    myservo2.write(0);                   
  }
  
  delay(15);                           // small wait time to let the servos move in time


  if (d_backward >= backwardDistanceWarningThreshold) {
    digitalWrite(yellowLedPIN , LOW);                     // both LEDs stay inactive
    digitalWrite(redLedPIN , LOW);       
  }
  else {
    if (d_backward >= backwardDistanceDangerThreshold){
      digitalWrite(yellowLedPIN , HIGH);                  // yellow LED illumninates to indicate presence behind, red LED stays off
      digitalWrite(redLedPIN , LOW);        
    }
    else{
      digitalWrite(yellowLedPIN , HIGH);                  // when an object is detected to close behind the bicycle the red LED illuminates 
      digitalWrite(redLedPIN , HIGH);                     // to indicate a possible dangerous aproach or careless safety distance
    }
  }
  
  delay(20);                   // small delay to start over
}