/*
 Arduino code to phase control a triac
ffffffffff
 Copyright (C) 2012 Dave Berkeley projects@rotwang.co.uk

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 USA
*/

// set pin numbers:
const int rotPin = 3;   // pulse per rotation, and interrupt 1
const int ledPin = 13;
const int optoPowerPin = 8;  // was 3, should be 8
const int triacPin = 4;
const int optoPin = 2;  // Zero crossing pin, and interrupt 0
const int BtStatePin = 5;
const int BtVccPin = 6;
const int BtEnablePin = 7;

 /*
  * The timer is set to ck/8 prescalar. At 16MHz this gives
  * 500us per count. With 50Hz mains, half cycle is 10ms long.
  * Therefore 20000 timer counts (10000us) equals one half cycle.
  */

#define COUNTS(n) ((n) << 1) // convert us to counts

static int percent;
static int percfract;
static long time;
static long start_t = 0;
static long rpm;
static int PIDint;
static int PIDprop;

//  empirically determined overhead for state switch
#define OVERHEAD 18

// Triac pulse train
#define PWM_MARK (100 - OVERHEAD)
#define PWM_SPACE (250 - OVERHEAD)

 /*
  *  Timer / counter config
  */

void clear_timer()
{
  TIMSK1 = 0; // no interrupts
  TCCR1A = 0; // counter off
  TCCR1B = 0;
  TCCR1C = 0;
  TCNT1 = 0;
  OCR1A = 0;
  OCR1B = 0;
}

void set_timer(unsigned long us)
{
  TCCR1A = 0;
  // divide by 8 prescaling
  TCCR1B = 0 << CS22 | 1 << CS21 | 0 << CS20;
  TCCR1B |= 0 << WGM13 | 1 << WGM12; // CTC mode

  OCR1A = COUNTS(us);
  // enable output compare A match interrupt
  TIMSK1 = 1 << OCIE1A;
}

/* no load rpm to integrator value.
 Found by setting p=0, i=500, i=50, i=1, then setting abritrary RPMs to see what integrator is required.
 Change i to speed up convertance, start high then step down in magnitudes down to 1.
 This removes the need for the integrator to take a long time seeking the correct no-load power
 level. Also means p=35, i=1, can be used as a general parameter rather than being set after
 the integrator is found (p=35 is good for steady state, but would go to 100% power if
 starting fresh).
 */
static long rpm2int[] = { 
  1000, 119245 , // unstable
  1500, 122293 , 
  2000, 127103 ,
  3000, 133869 ,
  4000, 141783 ,
  5000, 152228 ,
  6000, 160169 ,
  7000, 167953,
  8000, 177788,
  9000, 189315,
 10000, 197557,  // a little more testing said this was unstable too, opto giving up.
 11000, 207715
/* 12000, ,  Opto could no longer keep up. That noise filter must be getting in the way.
 13000, ,
 14000, ,
 15000, ,
 16000, ,
 17000, ,
 18000, ,
 19000, ,
 20000, ,
 21000, ,
 22000, ,
 23000, ,
 24000, ,
 25000, ,
 27000, ,
 30000, , */
};

  /*
  *  Auto generated table of % power to timer count.
  * 
  *  Uses http://www.ditutor.com/integrals/integral_sin_squared.html
  *  curve to divide the power into even bins.
  */

static int power_lut_50Hz[100] = {
  0,      1161,   1472,   1694,   1872,   2022,   2155,   2277,   2388,   2494,
  2594,   2683,   2772,   2855,   2933,   3011,   3088,   3161,   3233,   3299,
  3366,   3433,   3494,   3555,   3616,   3677,   3738,   3794,   3855,   3911,
  3966,   4022,   4077,   4133,   4183,   4238,   4288,   4344,   4394,   4444,
  4500,   4550,   4600,   4650,   4700,   4750,   4800,   4850,   4900,   4949,
  5000,   5055,   5105,   5155,   5205,   5255,   5305,   5355,   5405,   5455,
  5505,   5561,   5611,   5661,   5716,   5766,   5822,   5872,   5927,   5983,
  6038,   6094,   6150,   6211,   6266,   6327,   6388,   6450,   6511,   6572,
  6638,   6705,   6772,   6844,   6916,   6994,   7072,   7149,   7233,   7322,
  7411,   7511,   7616,   7727,   7850,   7983,   8133,   8311,   8533,   8844,
};

int percent_to_count(int percent)  // 6 bits of interpolation
{
  if (percent >= 99*64)
    return 0;
  if (percent <= 64)
    return 10000;
  int frac=percent&63;
  int pcent=percent>>6;
  
  return power_lut_50Hz[100 - pcent]-
         (((power_lut_50Hz[100 - pcent]-power_lut_50Hz[100 - pcent-1])*frac)>>6);
}

 /*
  *  State Machine
  */

typedef enum { 
  ZERO,    // zero crossing period
  PHASE,   // start of cycle
  PWM_HI,  // pulse the triac
  PWM_LO   // rest the triac
} triac_state;

static int cycle_state;

void set_state(int next)
{
  clear_timer();
  const unsigned long now = micros();

  switch (next) {
    case ZERO :
      digitalWrite(triacPin, 0);
      digitalWrite(ledPin, 0);
      start_t = now;

    case PHASE :
      digitalWrite(triacPin, 0);
      digitalWrite(ledPin, 1);
      set_timer(percent_to_count(percent));
      break;

    case PWM_HI :
      digitalWrite(triacPin, 1);
      digitalWrite(ledPin, 0);
      set_timer(PWM_MARK);

      if (cycle_state == PHASE) {
        time = now - start_t;
      }
      break;

    case PWM_LO :
      digitalWrite(triacPin, 0);
      digitalWrite(ledPin, 0);
      set_timer(PWM_SPACE);
      break;
  }

  cycle_state = next;
}

  /*
  *  Timer Compare interrupt
  */

ISR(TIMER1_COMPA_vect)
{
  switch (cycle_state)
  {
    case PWM_LO :
    case PHASE :
      set_state(PWM_HI);
      break;
    case PWM_HI :
      set_state(PWM_LO);
      break;
  }
}

/*
  Rotational speed encoder - opto input, on falling edge (that could be cleaner)
*/

unsigned long prevus=0;
unsigned long avgus=0;
short samples=-1;
unsigned long lastavgus=0;
short lastsamples=-1;

void rot_change()
 {
   if(samples<0)
    {
      prevus=micros();
      samples=0;
      avgus=0;
    }
   else
    {
      const unsigned long now = micros();
      unsigned long delta=0;
      if( now < prevus ) // rare wrap around, reset counters
       {
         prevus=now;
         samples=0;
         avgus=0;
         return;
       }
      else
         delta=now - prevus;

      prevus=now;
      avgus+=delta;
      samples++;

      if( samples>4000 )
       {
         avgus/=2;
         samples/=2;
       }
    }

 }

 /*
  *  Zero crossing interrupt handler
  */

int change=0;
long lastchange=0,highlow=0,lowhigh=0;

void on_change()
{
  const int p = digitalRead(optoPin);

  change++;

  long now=micros();

  if (!p) // inverted, my zero cross detector is low on ZC
  {
    highlow=now-lastchange;
    // end of cycle
    set_state(ZERO);
  } else {
    lowhigh=now-lastchange;
    // start of cycle.
    set_state(PHASE);
  }
  lastchange=now;
}

 /*
  *  Bluetooth subsystem
  */

class BlueTooth {

  int state_pin;
  int bt_connected;
  long last_low_t;

public:
  
  BlueTooth(int state_p)
  : state_pin(state_p),
    bt_connected(0),
    last_low_t(0)
  {
  }

protected:

  virtual void on_connected(void)
  {
    Serial.print("connected\n");
  }

  virtual void on_disconnected(void)
  {
    Serial.print("lost connection\n");
  }

public:

  int connected(void)
  {
    return bt_connected;
  }

  void poll(void)
  {  
    // The state pin is high if BT is connected,
    // but flashes at ~ 4.7Hz otherwise (~210ms).
    const int state = digitalRead(BtStatePin);
    const long now = millis();
    const static int disconnected_max_hi = 300;
  
    if (!state) {
      if (bt_connected)
        on_disconnected();
      bt_connected = 0; // definitely not connected
      last_low_t = now;
    } else {
      const long elapsed = now - last_low_t;
      if (elapsed > disconnected_max_hi) {
        // must be up
        if (!bt_connected)
          on_connected();
        bt_connected = 1;
      }
    }
  }
};

 /*
  * Serial command interface
  */

static int read_command(char* buff)
{
  // command protocol :
  // {s,r,i,p}xxx set the {percent,target rpm,PIDintegral coeffecient}
  
  buff++;
  
  int v = 0; 

  while (*buff) {
    const char c = *buff++;
    if ((c < '0') || (c > '9'))
      return -2; // bad number
    v *= 10;
    v += c - '0';
  }

  return v;
}

void get_serial()
{
  static char buff[16];
  static int idx = 0;

  if (!Serial.available())
    return;

  const int c = Serial.read();

  buff[idx++] = c;

  if (idx >= (sizeof(buff)-1)) {
    // too many chars
    idx = 0;
    return;
  }

  // read a line
  if ((c != '\r') && (c != '\n'))
    return;

  // null out the \n
  buff[idx-1] = '\0';
  idx = 0;

  if( *buff=='s' )
     percent=((long)read_command(buff))*(100/64);
  else if( *buff=='r' )
     rpm=read_command(buff);
  else if( *buff=='i' )
     PIDint=read_command(buff);
  else if( *buff=='p' )
     PIDprop=read_command(buff);
}

 /*
  *
  */

BlueTooth bt(BtStatePin);

void setup() 
{
  pinMode(ledPin, OUTPUT);
  pinMode(optoPin, INPUT);
  pinMode(rotPin, INPUT);
  digitalWrite(rotPin, 1 );  // internal pull up
  pinMode(optoPowerPin, OUTPUT);
  pinMode(triacPin, OUTPUT);
  pinMode(BtStatePin, INPUT);
  pinMode(BtVccPin, OUTPUT);
  pinMode(BtEnablePin, OUTPUT);

  // initialise the triac control system
  clear_timer();
  percent = 0;  // starts in percentage mode
  rpm=0;
  PIDint=0;
  PIDprop=0;
  set_state(ZERO);

  attachInterrupt(0, on_change, CHANGE);

  //attachInterrupt(1, rot_change, CHANGE);
  attachInterrupt(1, rot_change, FALLING);  // seems to be more immune to noise.

  static int test = 0;

  if (!test) {
    // switch on the zero crossing detector
    digitalWrite(optoPowerPin, 1);
    // switch on the BlueTooth device
    digitalWrite(BtVccPin, 1);
    // and enable it
    digitalWrite(BtEnablePin, 0);
  }

  Serial.begin(38400);
}

long integral=12L*64*128; // percent*64 , RPM vs target RPM error integral

long con(long a, long b, long c)
 {
   if(a<b)return b;
   else if(a>c)return c;
   else return a;
 }

void loop()
{
  bt.poll();

  get_serial(); // set percent, rpm, PIDintegral values

  static long last = 1000;
  static long last10 = 10;
  const long now = millis();

  if( last10 <= now )  // recalc percent value based on current rpm
   {
      static int bootstraps=0;
      if( rpm==0 )
       {
         percent=0;
         integral=12L*64*128;
       }
      else if( samples<=0 )
         bootstraps++;
      if( samples<=0 && bootstraps>10 ) // 0.1 seconds with no speed samples
       {
         avgus=1000000;  samples=10;  // 300 rpm
         bootstraps=0;
       }
       
      if( samples>0 )
       {
         long currpm=(60000000LL/1)*samples/avgus;
         long err=con(rpm-currpm,-32,3200);
         integral+=con(err*PIDint/16,-64,64);
         integral=con(integral,8L*64*128,99L*64*128);
         long sum=(integral>>7)+(err*PIDprop/10);  // integral>>7 because integrator is far too sensitive
         sum=con(sum,7L*64,95L*64);
         percent=sum;
         
         lastavgus=avgus;
         lastsamples=samples;
         avgus=0;
         samples=0;
         bootstraps=0;
       }
      last10=now+10;
   }

  if( last > now )
    return;

  last = now+1000;


  char buf[128];
  //sprintf(buf,"%ld, %ld, %d\r\n",highlow,lowhigh,highlow+lowhigh);
  //Serial.print(buf);
  
  sprintf(buf,"%d %%%d.%02d numus=%d, ",digitalRead(rotPin),percent>>6,(100*(percent&63))>>6,samples);
  Serial.print(buf);
  sprintf(buf,"rpm=%ld, integral=%ld, PIDint=%d, PIDprop=%d, ch=%d",rpm,integral,PIDint, PIDprop,change);
  Serial.print(buf);
  if( lastsamples>0 )
   {
     //sprintf(buf,", current rpm=%d",(60000000/1)/(avgus/samples));
     sprintf(buf,", current rpm=%d",60000000LL*lastsamples/lastavgus );
     Serial.print(buf);
   }
  sprintf(buf,"\r\n");
  Serial.print(buf);

  change=0;
  
}

// FIN