#include <NewPing.h>
#include <PID_v1.h>

#define TRIGGER_PIN  10  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     9  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 50 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 50cm.

int fan1 = 2;	//  IN1
int fan2 = 4;   //  IN2
int pwm = 0;    // N1 used as fan speed control

// Physical constants for the apparatus -----------------------------
float BALL_DIAMETER = 4.0; // in cm
float TUBE_LENGTH = 33.0;   // in cm
float SENSOR_ZERO = (TUBE_LENGTH - BALL_DIAMETER);

//PID parameters.
double kp=2;   //proportional parameter
double ki=5;   //integral parameter
double kd=1;   //derivative parameter

//SetPoint
double SetP = 15;



double heightInt, heightExt, heightDiff, command;


PID myPID(&heightInt, &command, &SetP,kp,ki,kd, DIRECT);


void setup(void)
{
  Serial.begin(57600);
   // PWM output
  pinMode(fan1,OUTPUT);
  pinMode(fan2,OUTPUT);
  pinMode(pwm,OUTPUT);




  //turn the PID on
  myPID.SetOutputLimits(0, 255);
  myPID.SetMode(AUTOMATIC);
  myPID.SetTunings(Kp, Ki, Kd);
}

void loop()
{
    digitalWrite(fan1, HIGH);
    digitalWrite(fan2, LOW);
    unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
    unsigned int distance = uS / US_ROUNDTRIP_CM ;
    heightInt = distance

    myPID.Compute();
    analogWrite(pwm, command);
}