#include <HX711.h>
#define servo 2
#define speed 5//lebih kecil lebih cepat;0=direct mode
#define step  10
#define rangeLoad 21
unsigned long now;
unsigned long t0;
unsigned long t1;
int last;
int pos = 1500;
int in;
int input;

int sample = 5;

int wait;
int arah;  //1=Naik 0=Turun 2=Stop
//int mode = 0;  //0=directMode 1=speed 2=wait/refreshInput 3=step 4=CalibrateMinLoadsensor 5=CalibrateMaxLoadsensor

int minLoad = 0;
int maxLoad = 180;

//hx711 setting
#define DOUT 13
#define SCK 12
HX711 scale;


void setup() {
  // put your setup code here, to run once:
  //step = speed * wait;
  //hx711
   Serial.begin(115200);  
  scale.begin(DOUT, SCK);
  Serial.println("Before setting up the scale:");
  Serial.print("read: \t\t");
  Serial.println(scale.read());  // print a raw reading from the ADC
  Serial.print("read average: \t\t");
  Serial.println(scale.read_average(10));  // print the average of 20 readings from the ADC
  Serial.print("get value: \t\t");
  Serial.println(scale.get_value(5));  // print the average of 5 readings from the ADC minus the tare weight (not set yet)
  Serial.print("get units: \t\t");
  Serial.println(scale.get_units(5), 1);  // print the average of 5 readings from the ADC minus tare weight (not set) divided

  // by the SCALE parameter (not set yet)
  scale.set_scale(10);  // this value is obtained by calibrating the scale with known weights; see the README for details
  scale.tare();         // reset the scale to 0

  Serial.println("After setting up the scale:");
  Serial.print("read: \t\t");
  Serial.println(scale.read());  // print a raw reading from the ADC
  Serial.print("read average: \t\t");
  Serial.println(scale.read_average(10));  // print the average of 20 readings from the ADC
  Serial.print("get value: \t\t");
  Serial.println(scale.get_value(5));  // print the average of 5 readings from the ADC minus the tare weight, set with tare()
  Serial.print("get units: \t\t");
  Serial.println(scale.get_units(5), 1);  // print the average of 5 readings from the ADC minus tare weight, divided
  minLoad = scale.read() / 100;
  maxLoad = minLoad + rangeLoad;
  // by the SCALE parameter set with set_scale

  pinMode(servo, OUTPUT);
 // opens serial port, sets data rate to 115200 bps
}


void loop() {
  // put your main code here, to run repeatedly:

  now = millis();
  if (now % 20 <= 1) {

    digitalWrite(servo, 1);
    delayMicroseconds(pos);
    digitalWrite(servo, 0);
    input = scale.read_average(sample) / 100;
    if (input < minLoad) {
      input = minLoad;
    }
    if (input > maxLoad) {
      input = maxLoad;
    }
    in = map(input, minLoad, maxLoad, 500, 2500);
    if (abs(pos - in) >= step / 2) {
      if (speed == 0) {
        pos = in;
      }
      if (speed >=1) {
        if(in>pos){
          pos = pos+10*speed;
        }
        if(in<pos){
          pos = pos-10*speed;
        }        
        if (abs(pos - in) <= speed / 2) {
          pos=in;
        }
      }
    }
    Serial.println("");
    Serial.print("Input=");
    Serial.print(input);
    Serial.print("   ");
    Serial.print("Pulse=");
    Serial.print(pos);
    Serial.print("Microsecond   ");
    Serial.print("minLoad=");
    Serial.println(minLoad);
  }

  //sweep
  /*
  if (now % wait <= 2) {
    in = map(input, minLoad, maxLoad, 500, 2500);
    if (in<=500){
      in=500;
    }
    if (in>=2500){
      in=2500;
    }
    if (in - pos >= step) {
      pos = in;
      arah = 1;
    }
    if (pos - in >= step) {
      pos = in;
      arah = 0;
    }
    if (arah = 1) {
      if (pos - in >= step / 2) {
        pos = in;
      }
    }
    if (arah = 0) {
      if (in - pos >= step / 2) {
        pos = in;
      }
    }
  }

  //Start Serial Input

  if (Serial.available() > 0) {
    // read the incoming byte:
    input = Serial.parseInt();
    if (input >= 0) {
      in = map(input, 0, 180, 500, 2500);
      pos = in;
      delay(1000);
    }
  }

  //end serial input
*/
}