#include <Servo.h>
#include <SPI.h>
#include <HX711.h>
#include <SoftwareSerial.h> 

SoftwareSerial Serial1(2, 3);
unsigned char order[4] = {0xAA, 0x06, 0x00, 0xB0};

HX711 scale1;
#define scale1_DOUT  4
#define scale1_SCK  5
float calibration_factor1 = -1200;
int weight1 = 0;

HX711 scale2;
#define scale2_DOUT  6
#define scale2_SCK  7
float calibration_factor2 = 445;
int weight2 = 0;

HX711 scale3;
#define scale3_DOUT  8
#define scale3_SCK  9
float calibration_factor3 = -1170;
int weight3 = 0;

int ledPin = 12;    // LED connected to digital pin 12

Servo myservo1;  // create servo1 object to control a servo
Servo myservo2;  // create servo2 object to control a servo

int pos = 0;    //  change the servo degrees
int pos_now = 1;

void setup() {
  Serial.swap(1);
  Serial.begin(9600);

  scale1.begin(scale1_DOUT, scale1_SCK);
  scale1.set_scale();
  scale1.tare(); //Reset the scale to 0

  scale2.begin(scale2_DOUT, scale2_SCK);
  scale2.set_scale();
  scale2.tare(); //Reset the scale2 to 0

  scale3.begin(scale3_DOUT, scale3_SCK);
  scale3.set_scale();
  scale3.tare(); //Reset the scale3 to 0

  myservo1.attach(0);  // attaches the servo1 on pin 0 (PWM) to the servo object
  myservo2.attach(1);  // attaches the servo2 on pin 1 (PWM) to the servo object

  //Set the servos to initial positions
  myservo1.write(0);
  myservo2.write(135);

  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);

  Serial1.begin(9600);
  volume(0x1E);//Volume settings 0x00-0x1E
}

void loop() {
  Serial.println("Loop");

  scale1.set_scale(calibration_factor1); //Adjust to this calibration factor1
  scale2.set_scale(calibration_factor2); //Adjust to this calibration factor2
  scale3.set_scale(calibration_factor3); //Adjust to this calibration factor3

  Serial.print("Scale1_Reading: ");
  Serial.print(scale1.get_units(), 1);
  Serial.print("g");
  Serial.println();
  weight1 = scale1.get_units();

  Serial.print("Scale2_Reading: ");
  Serial.print(scale2.get_units(), 1);
  Serial.print("g");
  Serial.println();
  weight2 = scale2.get_units();

  Serial.print("Scale3_Reading: ");
  Serial.print(scale3.get_units(), 1);
  Serial.print("g");
  Serial.println();
  weight3 = scale3.get_units();

//Weight1
  Serial.println(weight1);

  if (weight1 < 30) {
    // do nothing
    Serial.println("do nothing under 30g");
  } else if (weight1 > 90) {
    // do nothing
    Serial.println("do nothing over 90g");
  } else {

    // Servo1
    if ((weight1 >= 30) && (weight1 <= 90)) { //If the weight1 is 30-90g
      Serial.println("masu is ON");

      // Servo2
      for (pos = 180; pos >= 85; pos -= 1) { // goes from 170 degrees to 70 degrees
        myservo2.write(pos);
        delay(20);
      }

       //mp3 Dozo Dozo Voice
      delay(1000);
      play(0x01);//0x01 is Dozo_male 01.mp3
      delay(1000);

      // Servo2
      for (pos = 85; pos <= 180; pos += 1) { // goes from 60 degrees to 170 degrees
        myservo2.write(pos);
        delay(20);
      }

    }//if
  }//else
  delay(500);

//Weight2
  Serial.println(weight2);

  if (weight2 < 30) {
    // do nothing
    Serial.println("do nothing under 30g");
  } else if (weight2 > 90) {
    // do nothing
    Serial.println("do nothing over 90g");
  } else {

    // Servo1
    if ((weight2 >= 30) && (weight2 <= 90)) { //If the weight2 is 60-90g
      Serial.println("masu is ON");
      for (pos = 0; pos <= 90; pos += 1) {
        myservo1.write(pos);
        delay(20);
      }

      // Servo2
      for (pos = 180; pos >= 85; pos -= 1) { // goes from 170 degrees to 60 degrees
        myservo2.write(pos);
        delay(20);
      }

      //mp3 Dozo Dozo Voice
      delay(1000);
      play(0x02);//0x02 is Dozo_robot 03.mp3
      delay(1000);
      
      // Servo2
      for (pos = 85; pos <= 180; pos += 1) { // goes from 60 degrees to 170 degrees
        myservo2.write(pos);
        delay(20);
      }

      // Servo1
      for (pos = 90; pos >= 0; pos -= 1) { // goes from 90 degrees to 0 degrees
        // in steps of 1 degree
        myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
        delay(20);                       // waits 30ms for the servo to reach the position
        //
      }

    }//if
  }//else
  delay(500);

//Weight3
  Serial.println(weight3);

  if (weight3 < 30) {
    // do nothing
    Serial.println("do nothing under 60g");
  } else if (weight3 > 90) {
    // do nothing
    Serial.println("do nothing over 90g");
  } else {


    // Servo1
    if ((weight3 >= 30) && (weight3 <= 90)) { //If the weight3 is 60-90g
      Serial.println("masu is ON");
      for (pos = 0; pos <= 180; pos += 1) {
        myservo1.write(pos);
        delay(20);
      }


      // Servo2
      for (pos = 180; pos >= 85; pos -= 1) { // goes from 170 degrees to 60 degrees
        myservo2.write(pos);
        delay(20);
      }

      //mp3 Dozo Dozo Voice
      delay(1000);
      play(0x03);//0x03 is Dozo_female 05.mp3
      delay(1000);

      // Servo2
      for (pos = 85; pos <= 180; pos += 1) { // goes from 60 degrees to 170 degrees
        myservo2.write(pos);
        delay(20);
      }

      // Servo1
      for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
        // in steps of 1 degree
        myservo1.write(pos);              // tell servo1 to go to position in variable 'pos'
        delay(20);                       // waits 30ms for the servo to reach the position
        //
      }

    }//if
  }//else
  delay(500);

} //loop

void play(unsigned char Track)
{
  unsigned char play[6] = {0xAA, 0x07, 0x02, 0x00, Track, Track + 0xB3};
  Serial1.write(play, 6);
}
void volume( unsigned char vol)
{
  unsigned char volume[5] = {0xAA, 0x13, 0x01, vol, vol + 0xBE};
  Serial1.write(volume, 5);
}