/*
   CG scale for F3F & F3B models
   Olav Kallhovd 2016-2017
   CG Scale main components:
   1 pc load sensor front YZC-133 2kg
   1 pc load sensor rear YZC-133 3kg
   2 pc HX711 ADC, one for each load sensor (128bit resolution)
   1 pc Arduino Pro for the scale
   1 pc Arduino Pro for the Serial display
   1 pc 16*2 HD44780 LCD for the Serial display
   3D printed parts
   Max model weight with sensors above: 4 to 4,5kg depending on CG location
*/

#include <HX711_ADC.h> //https://github.com/olkal/HX711_ADC can be installed from the library manager
//Number of samples and some filtering settings can be adjusted in the HX711_ADC.h library file
//The best RATE setting is usually 10SPS, see HX711 data sheet (HX711 pin 15, can usually be set by a solder jumper on the HX711 module)
//RATE 80SPS will also work fine, but conversions will be more noisy, so consider increasing number of samples in HX711_ADC.h

//HX711 constructor (dout pin, sck pint):
HX711_ADC LoadCell_1(8, 9); //HX711 pins front sensor (DOUT, PD_SCK)
HX711_ADC LoadCell_2(2, 3); //HX711 pins rear sensor (DOUT, PD_SCK)

#include <ss_oled.h>

SSOLED ssoled;
#define SDA_PIN -1
#define SCL_PIN -1
// no reset pin needed
#define RESET_PIN -1
// let ss_oled find the address of our display
#define OLED_ADDR 0x3C
#define FLIP180 0
#define INVERT 0
// Use the default Wire library
#define USE_HW_I2C 1


byte ledPin = 13;

char toLCD[20];
byte output;
boolean ledState;
long t1;
long t2;
const int printInterval = 500; // LCD/Serial refresh interval
//const int printInterval = 2000; // LCD/Serial refresh interval

//*** configuration:
//*** set dimensional calibration values:
const long WingPegDist = 1201
; //calibration value in 1/10mm, projected distance between wing support points, measure with calliper
const long LEstopperDist = 310; //calibration value 1/10mm, projected distance from front wing support point to leading edge (stopper pin), measure with calliper
//*** set scale calibration values (best to have the battery connected when doing calibration):
//const float ldcell_1_calfactor = 1044.2; // user set calibration factor load cell front (float)
//const float ldcell_2_calfactor = 1064.0; // user set calibration factor load cell rear (float)
const float ldcell_1_calfactor = 10.442; // user set calibration factor load cell front (float)
const float ldcell_2_calfactor = 10.640; // user set calibration factor load cell rear (float)
//***
const long stabilisingtime = 10000; // tare precision can be improved by adding a few seconds of stabilising time
//***
const long CGoffset = ((WingPegDist / 2) + LEstopperDist) * 10;

void setup() {

  oledInit(&ssoled, OLED_128x64, OLED_ADDR, FLIP180, INVERT, USE_HW_I2C, SDA_PIN, SCL_PIN, RESET_PIN, 400000L);       // Standard HW I2C bus at 400Khz

  Serial.begin(9600);
  Serial.write(254);
  Serial.write(128);
  Serial.print("F3X COG scale   ;");
  Serial.write(254);
  Serial.write(192);
  Serial.println();
  Serial.println("Wait for stabilising and tare...");
  oledFill(&ssoled, 0, 1);
  oledWriteString(&ssoled, 0, 0, 0, (char *)"  CoG scale", FONT_NORMAL, 0, 1);
  oledWriteString(&ssoled, 0, 10, 2, (char *)"Wait for", FONT_NORMAL, 0, 1);
  oledWriteString(&ssoled, 0, 10, 4, (char *)"   stabilising", FONT_NORMAL, 0, 1);
  oledWriteString(&ssoled, 0, 10, 6, (char *)"    and tare...", FONT_NORMAL, 0, 1);

  LoadCell_1.begin();
  LoadCell_2.begin();
  byte loadcell_1_rdy = 0;
  byte loadcell_2_rdy = 0;
  while ((loadcell_1_rdy + loadcell_2_rdy) < 2) { //run startup, stabilisation and tare, both modules simultaneously
    if (!loadcell_1_rdy) loadcell_1_rdy = LoadCell_1.startMultiple(stabilisingtime);
    if (!loadcell_2_rdy) loadcell_2_rdy = LoadCell_2.startMultiple(stabilisingtime);
  }
  LoadCell_1.setCalFactor(ldcell_1_calfactor); // set calibration factor
  LoadCell_2.setCalFactor(ldcell_2_calfactor); // set calibration factor
  

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

  delay(2000);
  oledFill(&ssoled, 0, 1);
}



void flashLED() {
  if (t2 < millis()) {
    if (ledState) {
      t2 = millis() + 500;
      ledState = 0;
    }
    else {
      t2 = millis() + 100;
      ledState = 1;
    }
    digitalWrite(ledPin, ledState);
  }
}

void loop() {
  //library function update() should be called at least as often as HX711 sample rate; >10Hz@10SPS, >80Hz@80SPS
  //longer delay in scetch will reduce effective sample rate (be careful with delay() in loop)
  LoadCell_1.update();
  LoadCell_2.update();
  char szTemp[32];
  // calculate CG and update serial/LCD
  if (t1 < millis()) {
    t1 = millis() + printInterval;
    float a = LoadCell_1.getData();
    
    float b = LoadCell_2.getData();
    long weightAvr[3];
    float CGratio;
    long CG;
    weightAvr[0] = a * 100;
  
    weightAvr[1] = b * 100;


    long weightTot = weightAvr[0] + weightAvr[1];

    if (weightAvr[0] > 500 && weightAvr[1] > 500) {
      long a = weightAvr[1] / 10;
      
      long b = weightAvr[0] / 10;
      CGratio = (((a * 10000) / (a + b)));
      CG = ((((WingPegDist) * CGratio) / 1000) - ((WingPegDist * 10) / 2) + CGoffset);

    }
    else {
      CG = 0;
    }

    Serial.print("weight_LdCell_1");
    Serial.print(": ");
    long i = weightAvr[0];
    if (i < 0) {
      Serial.print('-');
      i = ~weightAvr[0] + 1;
    }
    Serial.print(i / 100);
    Serial.print('.');
    if ((i % 100) < 10) {
      Serial.print("0");
    }
    Serial.print(i % 100);
    Serial.print("      ");

    oledWriteString(&ssoled, 0, 0, 0, (char *)"Front  ", FONT_NORMAL, 0, 1);
    sprintf(szTemp, "%dg  ", i/100);
    Serial.println((int)weightAvr[0]);
    oledWriteString(&ssoled, 0, -1, -1, szTemp, FONT_NORMAL, 0, 1);
//oledWriteString(&ssoled, 0, -1, -1, "10", FONT_NORMAL, 0, 1);

    Serial.print("weight_LdCell_2");
    Serial.print(": ");
    long j = weightAvr[1];
    if (j < 0) {
      Serial.print('-');
      j = ~weightAvr[1] + 1;
    }
    Serial.print(j / 100);
    Serial.print('.');
    if ((j % 100) < 10) {
      Serial.print("0");
    }
    Serial.print(j % 100);
    Serial.print("      ");

    oledWriteString(&ssoled, 0, 0, 2, (char *)"Rear   ", FONT_NORMAL, 0, 1);
    sprintf(szTemp, "%dg  ", j / 100);
    oledWriteString(&ssoled, 0, -1, -1, szTemp, FONT_NORMAL, 0, 1);

    Serial.print("Total weight");
    Serial.print(weightTot / 100);
    oledWriteString(&ssoled, 0, 0, 4, (char *)"Weight  ", FONT_NORMAL, 0, 1);
    sprintf(szTemp, "%dg  ", (i+j) / 100);
    oledWriteString(&ssoled, 0, -1, -1, szTemp, FONT_NORMAL, 0, 1);
    Serial.print("     ");

    Serial.print("CG:");
    Serial.print(CG / 100);
    Serial.print('.');
    Serial.println(CG % 100);

    oledWriteString(&ssoled, 0, 0, 6, (char *)"CG ", FONT_STRETCHED, 0, 1);
    sprintf(szTemp, "%dmm  ", (int)CG / 100);
    oledWriteString(&ssoled, 0, -1, -1, szTemp, FONT_STRETCHED, 0, 1);

  }
  flashLED();
}