ErgoSense V2

#include <EEPROM.h>

#define trigPin 10
#define echoPin 13

#define redPin 4
#define greenPin 5
#define yellowPin 6

#define savePin 2

#define modePin 9


// NEW
float duration, distance, desired_distance, desired_distance_sit, desired_distance_stand;
int distanceAddressSit = 0;

// NEW
int distanceAddressStand = 1;

// Error threshold for distance
int limit = 1;

void setup() {

  Serial.println("SYSTEM STARTING!");
  
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(yellowPin, OUTPUT);

  // Set save pin to LOW
  pinMode(savePin, OUTPUT);
  digitalWrite(savePin, HIGH);
  pinMode(savePin, INPUT);

  // Mode pin
  pinMode(modePin, INPUT);

  
  // Get persistent data
  EEPROM.get(distanceAddressSit, desired_distance_sit);
  EEPROM.get(distanceAddressStand, desired_distance_stand);

}

void loop() {
  // Write a pulse to the HC-SR04 Trigger Pin
  
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Measure the response from the HC-SR04 Echo Pin
 
  duration = pulseIn(echoPin, HIGH);
  
  // Determine distance from duration
  // Use 343 metres per second as speed of sound
  
  distance = (duration / 2) * 0.0343;
  
  // Send results to Serial Monitor
 

    Serial.print(distance);
    Serial.println(" cm");
    delay(50);

    // Check which mode
    int mode_result = digitalRead(modePin);

    Serial.print("MODE RESULT: ");
    Serial.println(mode_result);

    
    // Saving logic
    int save_result = digitalRead(savePin);

    Serial.print("SAVED RESULT: ");
    Serial.println(save_result);
    
    // If pushed
    if(save_result == HIGH){
      if(mode_result == LOW){
        desired_distance_sit = distance;
        EEPROM.put(distanceAddressSit, desired_distance_sit);
        Serial.print("NEW DESIRED SIT DISTANCE SAVED: ");
        Serial.println(desired_distance_sit);
        delay(1000);
      } else{
        desired_distance_stand = distance;
        EEPROM.put(distanceAddressStand, desired_distance_stand);
        Serial.print("NEW DESIRED STAND DISTANCE SAVED: ");
        Serial.println(desired_distance_stand);
        delay(1000);
      }

    }

    // Update correct distance for the mode
    if(mode_result == LOW){
      desired_distance = desired_distance_sit;
    } else {
      desired_distance = desired_distance_stand;
    }

    if(distance >= desired_distance + limit){
      // red led
      Serial.println("Red");
      digitalWrite(redPin, HIGH);
      digitalWrite(greenPin, LOW);
      digitalWrite(yellowPin, LOW);
      
    }
    else if(distance <= desired_distance - limit){
      // Yellow led
      Serial.println("Yellow");
      digitalWrite(redPin, LOW);
      digitalWrite(greenPin, LOW);
      digitalWrite(yellowPin, HIGH);
    } else{
      // Green led
      Serial.println("Green");
      digitalWrite(redPin, LOW);
      digitalWrite(greenPin, HIGH);
      digitalWrite(yellowPin, LOW);
    }
  
  //delay(500);

}