//------------------------------------------------------------------------------
//  RCSId: $Id: Motorcontrol.ino,v 1.9 2015/06/30 14:27:54 thba Exp thba $
//------------------------------------------------------------------------------
//
//  Author(s)     :  Theo Band
//
//  Contents      :  Regeling voor fluistermotor
//
//------------------------------------------------------------------------------
//                                HISTORY
//------------------------------------------------------------------------------
//
//  $Log: Motorcontrol.ino,v $
//  Revision 1.9  2015/06/30 14:27:54  thba
//  Increase FPWM to 20kHz.
//  Some checks around 10.0V rounded for proper display.
//
//  Revision 1.8  2015/06/04 20:48:17  thba
//  Current and voltage calibrated (when warm)
//
//  Revision 1.7  2015/05/23 23:21:06  thba
//  VMIN increased to 10.5
//  Offset lowered by 0.5A to 6A
//  Corrected time overflow
//
//  Revision 1.6  2015/05/20 23:32:21  thba
//  Battery status display improved
//
//  Revision 1.5  2015/05/20 19:25:01  thba
//  AH used indication added
//  Improved current control and accuracy
//
//  Revision 1.4  2015/05/19 18:12:55  thba
//  Current calibrated, overload filtered
//
//  Revision 1.3  2015/05/19 16:47:00  thba
//  Current measurement averaging added
//
//  Revision 1.2  2015/05/17 15:02:49  thba
//  Corrected ICAL and increased IMAX
//
//  Revision 1.1  2015/05/17 13:23:12  thba
//  Initial revision
//

// include the library code:
#include <LiquidCrystal.h>

// Pin D0-7 = PD0-PD7
// Pin D8  = PB0
// Pin D9  = PB1 OC1A
// Pin D10 = PB2 OC1B
// Pin D12 = PB4
// Pin D13 = PB5 (led)
// Pin A0-5  = PC0-PC5
// PWM=3, 5, 6, 9, 10, and 11
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

byte BatEmpty[8] = {
  B01110,
  B11111,
  B10001,
  B10001,
  B10001,
  B10001,
  B11111,
};

byte BatHalf[8] = {
  B01110,
  B11111,
  B10001,
  B10001,
  B11111,
  B11111,
  B11111,
};
byte BatFull[8] = {
  B01110,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
};

byte Motor[8] = {
  B11000,
  B01100,
  B00110,
  B11111,
  B00110,
  B01100,
  B11000,
};
int controlVal;
int pwmout;
float vbat;
float iload;

const int none  = 0;
const int select= 1;
const int left  = 2;
const int down  = 3;
const int up    = 4;
const int right = 5;

const float VCAL = 15.47;  // vbat=vsenseVal/1023*VCAL; VCAL= 5V/220K*(220K+470K)=~15.7
const float ICAL = 0.0369; // 37mA/bit
const float IOFFSET = 3;   // Measured offset after curve fitting

const int FPWM=20E3;  // 16MHz/OCR1A/2=FPWM
const int PWMMAX=100; // 0-100%
const float VMIN=10.5;// Minimum voltage
const int IMAX=40;    // Maximum controlled current (overload at +10%)
                      // Full scale is always overload = 1023*ICAL+IOFFSET=44.25A

const int vsensePin = A0;
const int isensePin = A1;
const int backlightPin = 3;
const int overloadPin = 11;
const int pwmoutPin = 10;
const int aKey = 0; // analog lcd key input
const int PotpPin = A2; // A2 Potmeter plus
const int PotcPin = A3; // A3 Potmeter center
const int PotnPin = A4; // A4 Potmeter min

void setup() {
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  lcd.createChar(0, BatEmpty);
  lcd.createChar(1, BatHalf);
  lcd.createChar(2, BatFull);
  lcd.createChar(3, Motor);

  // Initialiaseer
  //lcd.setCursor(0, 1);
  pinMode(PotpPin, OUTPUT); // Power
  pinMode(PotcPin,  INPUT); // Control value
  pinMode(PotnPin, OUTPUT); // Ground
  digitalWrite(PotpPin, HIGH);
  digitalWrite(PotnPin, LOW);
  pinMode(pwmoutPin, OUTPUT);

  // Pins 15/PB1/9 and 16/PB2/10: PWM
  //  TCCR1A - Timer/Counter1 Control Register A
  //  TCCR1B - Timer/Counter1 Control Register B
  // COM1B1/COM1B0=10 = Set/clear OC1A/OC1B when up/downcounting
  // WGM10 = PWM 8 bit. Count up. F=16MHz/512=31.25kHz
  // WGM10+WGM13 = PWM. Count up and down to OCR1A. F=16MHz/OCR1A/2
  // CS10 = no prescaling
  TCCR1A = _BV(COM1B1) | _BV(WGM10);
  TCCR1B = _BV(CS10) | _BV(WGM13);
  // OCR1A used as TOP value
  OCR1A = 16E6/2/FPWM;

  analogWrite(backlightPin,150);//255=off 0=on
  lcd.clear();
  lcd.print("Whisper 1.9");
  delay(1000);

  // initialize serial communications at 9600 bps:
  Serial.begin(9600);
}

void loop() {
  static boolean overload, overloaded;
  static unsigned long qload;
  static int qn;
  static float batAH, motAH;
  int overload_cnt;
  
  if (clk1()) {
    float deltaAH=0;
    if (qn>0) {
      deltaAH=calI(qload,qn)/3600;
    } else {
      deltaAH=0;
    }
    motAH+=deltaAH;
    batAH+=deltaAH*pwmout/PWMMAX;
    overloaded=overload;
    overload=false;
    qn=0;qload=0;
  }
  if (clkMenu()) {
    if (overloaded)  {
      analogWrite(backlightPin,0);
      }
    else {
      analogWrite(backlightPin,150);
    }
    printStatus(vbat,iload,pwmout,overloaded,batAH,motAH);
  }
  // Read out sensors at max speed
  int isense=analogRead(isensePin); // full scale is about 44A
  //isense=500;

  //Coulomb counter
  qload+=isense;
  qn++;

  if (clk()) {
    iload=QtoI(qload,qn);
    vbat=analogRead(vsensePin)/1023.0*VCAL;
    controlVal=map(analogRead(PotcPin),0,1023,0,PWMMAX);
    MotorControl();
    //Serial.println(pwmout); 
  }

  // Emergency brake when a short occurs and the current rises too quick
  // Three successive overloads switch off the load for a second
  // MotorControl() control time from max to min is 100*clk()=1s
  if (isense>=(IMAX*1.1-IOFFSET)/ICAL | isense>=1023 ) {
    overload_cnt++;
  }
  else {
    overload_cnt=0;
  }
  if (overload_cnt >= 3) {
    pwmout=0;
    overload=true;
    OCR1B = map(pwmout,0,PWMMAX,0,OCR1A);
    //Serial.println("Overload");
    delay(1000);
  }

}

// return average current since last call
float QtoI(unsigned long int qload, int qn) {
  static int qn_d;
  static unsigned long qload_d;
  float Ival=calI((qload-qload_d),(qn-qn_d));
  qload_d=qload;
  qn_d=qn;
  return Ival;
}

// Return the average current Q/n*ICAL+IOFFSET (calibrate offset and scale)
float calI(unsigned long int Q, int n) {
  float Ival=Q/n*ICAL;
  // there is offset in the detector and negative values are reported as 0
  if (Ival < 1) {
    Ival=0;
  }
  else {
    Ival+=IOFFSET;
  }
  return Ival;
}
  

void MotorControl() {
  //static int pwmout;
  if (iload>IMAX | vbat<VMIN) {
    pwmout--;
  }
  else if (controlVal>pwmout) {
    pwmout++;
  }
  else if (controlVal<pwmout) {
    pwmout--;
  }
  pwmout=constrain(pwmout,0,PWMMAX);
  OCR1B = map(pwmout,0,PWMMAX,0,OCR1A);
}


boolean clk() {
  static boolean clk;
  boolean s = ((millis()/10) % 2)==0;
  if (s != clk) {
    clk = ! clk;
    return true;
  }
  return false;
}

boolean clkMenu() {
  static boolean clk;
  boolean s = ((millis()/250) % 2)==0;
  if (s != clk) {
    clk = ! clk;
    return true;
  }
  return false;
}

boolean clk1() {
  static boolean clk;
  boolean s = ((millis()/1000) % 2)==0;
  if (s != clk) {
    clk = ! clk;
    return true;
  }
  return false;
}

//B=12.80V 16.0A
//M=09.80V 20.0A
//1234567890123456
void printStatus(float vbat,float iload,int pwmout, boolean overload, float batAH, float motAH) {
  int batpercent=batStatus(vbat,(iload>10));
  byte batIcon;
  lcd.setCursor(0,0);
  if (batpercent>75) {
    batIcon=2;
  } else if (batpercent>25) {
    batIcon=1;
  } else {
    batIcon=0;
  }
  lcd.write(batIcon);
  lcd.print(" ");
  if (vbat+0.005<10) {
    lcd.print(" ");
  }
  lcd.print(vbat,2);
  lcd.print("V ");
  if (pwmout>0) {
    float ibat=iload*pwmout/PWMMAX;
    if (ibat+0.05<10) {
      lcd.print(" ");
    }
    lcd.print(ibat,1);
    lcd.print("A  ");
  } else {
    //  1:34:05
    unsigned long seconds=millis()/1000;
    unsigned int minutes=(seconds/60) % 60;
    unsigned int hours=seconds/3600;
    // minutes=minutes-hours*60;
    seconds=seconds % 60;
    lcd.print(hours);
    lcd.print(":");
    if (minutes<10) {
      lcd.print("0");
    }
    lcd.print(minutes);
    lcd.print(":");
    if (seconds<10) {
      lcd.print("0");
    }
    lcd.print(seconds);
  }

  lcd.setCursor(0,1);
  if (pwmout>0) {
    lcd.write(byte(3));
    lcd.print(" ");
    float vmotor=vbat*pwmout/PWMMAX;
    if (vmotor+0.005<10) {
      lcd.print(" ");
    }
    lcd.print(vmotor,2);
    lcd.print("V ");
    if (iload+0.05<10) {
      lcd.print(" ");
    }
    lcd.print(iload,1);
    lcd.print("A ");
    lcd.print("  ");
  } else {
    // State of charge, no load
    //lcd.write(batIcon);
    lcd.print("    ");

    if (batpercent<10) {
      lcd.print("  ");
    } else if (batpercent<100) {
      lcd.print(" ");
    }
    lcd.print(batpercent);
    lcd.print("% ");
//    if (batAH+0.05<10) {
//      lcd.print(" ");
//    }
    lcd.print(batAH,2);
    lcd.print("AH ");
//    if (motAH<10) {
//      lcd.print(" ");
//    }
//    lcd.print(motAH,2);
//    lcd.print("AH ");
  }
  if (overload) {
    lcd.setCursor(2,1);
    lcd.print(" OVERLOAD!   ");
  }
}

int batStatus(float vbat, boolean discharge) {
  const float vbat_min=11.9;
  const float vbat_max=12.7;
  const float vbat_min_load=11.0;
  const float vbat_max_load=12.5;
  if (! discharge) {
    if (vbat<vbat_min) { return 0 ;}
    if (vbat>vbat_max) { return 100 ;}
    return int((vbat-vbat_min)/(vbat_max-vbat_min)*100) ;
  } else {
    if (vbat<vbat_min_load) { return 0 ;}
    if (vbat>vbat_max_load) { return 100 ;}
    return int((vbat-vbat_min_load)/(vbat_max-vbat_min_load)*100) ;
  }
}




