/*
################################################################################
# By DrCrowller                                                                #
#                                                                              #
# Feel free to copy, modify and do whatever you want with this project         #
# Be aware that the use of many variables may consume more memory than wanted  #
#                                                                              #
# Created : 7 Apr. 2022                                                        #
################################################################################
*/

#include <Servo.h>

Servo arm; // Create a "Servo" object called "arm"
Servo arm2; // Create a "Servo" object called "arm"
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 1.0; // Variable used for the arm's position step

#define VERT_PIN A0
#define HORZ_PIN A1
#define SEL_PIN  2
#define LED_PIN 13

const int trigPin = 3;
const int echoPin = 4;
long duration;

void setup()
{
  pinMode(trigPin, OUTPUT);  
	pinMode(echoPin, INPUT);  

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);
  pinMode(SEL_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  arm.attach(8);
  arm2.attach(9);
  arm.write(pos);
  arm2.write(180 - pos);
  Serial.begin(9600);
}

int calc_distance(void) {
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  return duration * 0.034 / 2;
}

void loop()
{
  bool stop;
  int distance = calc_distance();
  if (distance < 5) {
    digitalWrite(LED_PIN, HIGH);
    stop = true;
  } else {
    digitalWrite(LED_PIN, LOW);
    stop = false;
  }

  // speed goes from 0 (bottom) to 1023 (top)
  float speed = map(analogRead(VERT_PIN), 0, 1023, 0, 180);
  if (stop) {
    if (speed > 90) {
      speed = 95;
    }
    if (speed < 90) {
      speed = 85;
    }
  }

  // horz goes from 0 (right) to 1023 (left)
  float steering = map(analogRead(HORZ_PIN), 0, 1023, 0, 180) - 90;

  // selPressed is true is the joystick is pressed
  bool selPressed = digitalRead(SEL_PIN) == LOW;

  float s1 = speed + steering;
  float s2 = speed - steering;

  arm.write(s1);
  arm2.write(s2);

  Serial.print(s1);
  Serial.print(",");
  Serial.print(s2);
  Serial.print(",");
  Serial.print(speed);
  Serial.print(",");
  Serial.print(steering);
  Serial.print(",");
  Serial.print(selPressed);
  Serial.print(",");
  Serial.println(distance);
  delay(100);
}