/*
*  Nano, Canbus MCP2515, serial LCD, optical rotary encoder (quadrature)
*
*  this can drive 1 to 4 ELtek Flatpack 48V 2kW PS
*  with a constant voltage control
*  
*  Output voltage and current limit is set via encoder.
*  push button to toggle from CV to CC
*  
*  The serial numbers are unique for each PS and must be set for your units
*
*  The outputs of the PSs and the voltage setpoint are available for plotting via Arduino IDE plotter
*/

#include <SPI.h>
#include "mcp_can.h"
#include <mcp_can_dfs.h>
#include <LiquidCrystal_PCF8574.h>
#include <Wire.h>

LiquidCrystal_PCF8574 lcd(0x27);

unsigned char login[][8] = {
 {0x16, 0x36, 0x71, 0x07, 0x03, 0x76, 0x00, 0x00},  
 {0x18, 0x10, 0x50, 0x01, 0x76, 0x09, 0x00, 0x00},
 {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0},  
 {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}};

#define NPS 2

float v[4],i[4];
volatile int ps_alive[4],nps, icount;

MCP_CAN CAN(10); // Set CS pin as D10

#define VOLTAGE 4800
#define CURRENT 450
uint8_t outset[8] = {CURRENT & 0xff, (CURRENT >> 8) & 0xff, VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,0x3E, 0x17};

float pot,cpot,tamps;
char lcdbuf[90],*lp,lpc;

void get_biquad(float,int,int);
void biquad_filter(float *,int);
float aa[5][8],d1[5][8],d2[5][8];
float w0[5][8],w1[5][8],w2[5][8];
int bqnp[8];

void ps_check_send(int , unsigned char *, uint32_t , unsigned int , unsigned int );

volatile int encoderPos = 0,b_mode, opto_v, opto_i;
volatile boolean PastA = 0;
volatile boolean PastB = 0;
volatile long last_press;

void doEncoderA()
{
 //PastB ? encoderPos-- :  encoderPos++;
 if (PastB)
   {
     if (b_mode == 0)
       opto_v --;
     else
       opto_i --;
   }
 else
   {
     if (b_mode == 0)
       opto_v ++;
     else
       opto_i ++;
   }
if (opto_v < 0) opto_v = 0;
if (opto_i < 0) opto_i = 0;
if (opto_v > 100) opto_v = 100;
if (opto_i > 100) opto_i = 100;
}

void doEncoderB()
{
 PastB = !PastB;
}


void get_biquad(float fc, int np, int idx)
 {  
 float s,r;
 int i;  
 float a = tan(M_PI*fc);
 float a2 = a*a;
 bqnp[idx] = np;
 for(i=0; i<np; ++i)
   {
   r =  sin(3.141592926 * (2.0*i+1.0)/(4.0*(float)np));
   s = a2 + 2.0*a*r + 1.0;
   aa[i][idx] = a2/s;
   d1[i][idx] = 2.0*(1-a2)/s;
   d2[i][idx] = -(a2 - 2.0*a*r + 1.0)/s;
   w1[i][idx]=w2[i][idx]=w0[i][idx]=0.0;
   }
 }

void biquad_filter(float *val, int idx)
{
 float s;
 int i;
 s = *val;
 for(i=0; i<bqnp[idx]; ++i)
   {
   w0[i][idx] = d1[i][idx]*w1[i][idx] + d2[i][idx]*w2[i][idx] + s;
   s = aa[i][idx]*(w0[i][idx] + 2.0*w1[i][idx] + w2[i][idx]);
   w2[i][idx] = w1[i][idx];
   w1[i][idx] = w0[i][idx];
   }
 *val = s;
}

 
void setup()
{
 int n;

 // optical encoder setup
 opto_v = 0;
 opto_i = 0;
 last_press = millis();
 pinMode(2, INPUT);
 pinMode(3, INPUT);
 pinMode(8,INPUT_PULLUP);
 PastA = (boolean)digitalRead(2); //initial value of channel A;
 PastB = (boolean)digitalRead(3); //and channel B
 // encoder A channel on interrupt 0 (Arduino's pin 2)
 attachInterrupt(0, doEncoderA, RISING);
 // encoder B channel pin on interrupt 1 (Arduino's pin 3)
 attachInterrupt(1, doEncoderB, CHANGE);
 b_mode = 0;                     // = 1 for change current, = 0 for change voltage
 
 Serial.begin(115200);
 lcd.begin(20,4);
 lcd.print("  ELTEK CV/CC ");
 pinMode(3,INPUT);
 pinMode(12,INPUT);
   
START_INIT:
if(CAN_OK == CAN.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ))
   {
       //Serial.println("CAN BUS Shield init ok!");
   }
else
   {
   Serial.println("CAN BUS Shield init fail");
   Serial.println("Init CAN BUS Shield again");
   delay(1000);
   goto START_INIT;
   }
icount = 0;  
CAN.setMode(0);    
delay(200);
nps = NPS;
for (n=0; n < nps; n++)
 {
   ps_alive[n] = 0;
   v[n]=i[n]=0.0;
 }
for (n=0;n < (nps*2); n++)
 get_biquad(0.2,2,n);
 

lp = lcdbuf;
lpc=0;
memset(lp,' ',80);
}

void loop()
{

   unsigned char len = 0;
   unsigned char buf[8] ;
   int rv, was_msg,n, n_alive;
   float a;
   unsigned int ui,uc;
   uint32_t canId;

   // look for button press on D8, toggle b_mode. Debounce with 1000 milisecond delay
   if (digitalRead(8) == 0 && (millis() - last_press) > 1000)          // D8 is input with internal pullup. zero = button press
     {
       last_press = millis();
       if (b_mode == 1)
         b_mode = 0;
       else
         b_mode = 1;
     }

   
   // update LCD a char at a time
   lcd.write(lcdbuf[lpc]);
   lpc++;
   if (lpc > 79)
     {
       lpc=0;
       lcd.home();
       delay(100);
       icount++;
       if (icount > 5)
         {
           icount=0;
           for (n = 0; n < nps; n++)
             {
               CAN.sendMsgBuf(0x05004800 + 4 * (n+1), 1, 8, login[n]);
             }
         }
     }

   // get pot settings
   
   a = (float)opto_v/100.0;      // pot is connected to 3.3V so need this scale for 0 - 1 range. 5V pin is used by canbus adaptor
   pot = 43.0 + (57.4 - 43.0)*a;          // set demand from 43 to 57V. flatpack has output limits smaller than this range.
   if (pot < 43.0)       pot = 43.0;
   if (pot > 57.4)       pot = 57.4;        // programming more than that makes the canbus program command be ignored so current limit changes will also be ignored
   ui = int(pot * 100.0);
   
   
   a = (float)opto_i/100.0;      // pot is connected to 3.3V so need this scale for 0 - 1 range. 5V pin is used by canbus adaptor
   cpot = (float)nps*45.0*a;                 // set current limit
   if (cpot < 0.0)        cpot = 0.0;
   n_alive=0;
   for (n=0; n < nps; n++)
     if (ps_alive[n] == 1)
       n_alive++;
   if (n_alive == 0)
     uc = int( cpot * 10.0/(float)nps );         // none alive
   else
     uc = int( cpot * 10.0/(float)n_alive );      // scale current setpoint to (no. of ps alive)/(no. of ps)
   
   
   // check for CAN message and parse
   was_msg = 0;  
   rv = CAN.checkReceive();
   if(rv == CAN_MSGAVAIL )
   {
     was_msg = 1;
     CAN.readMsgBuf(&len, buf);
     canId = CAN.getCanId();

     for(n=0; n < nps; n++)
         ps_check_send(n, buf,canId, ui, uc);  
     }


   do_lcd();
   
   if (was_msg == 1 )
     {
     // for graphing
     Serial.print (tamps); Serial.print(" ");
     Serial.print (pot); Serial.print(" ");
     Serial.print (cpot); Serial.print(" ");
     for (n=0; n < nps; n++)
       {
       Serial.print (v[n]); Serial.print(" ");Serial.print(i[n]);Serial.print(" ");
       }
     Serial.println(" 0 60 ");
     }
}  


void ps_check_send(int ps_n, unsigned char buf[], uint32_t canId, unsigned int ui, unsigned int uc)
 {
   int ps_no = ps_n + 1;
   uint32_t c1,c2,c3;
   c1 = 0x05004004 + 0x10000 * ps_no;
   c2 = 0x05004008 + 0x10000 * ps_no;
   c3 = 0x0500400C + 0x10000 * ps_no;
   
   if (canId == c1 || canId == c2 )      
         {
         i[ps_n] = buf[2]*255*0.1+buf[1]*0.1;
         v[ps_n] = buf[4]*255*0.01+buf[3]*0.01;
         biquad_filter(&v[ps_n],ps_n*2);
         biquad_filter(&i[ps_n],1 + ps_n*2);
         //CAN.sendMsgBuf(0x05004800 + 4 * ps_no, 1, 8, login[ps_n]);
         // make changes to control word with new Vout
         outset[0] = uc & 0xff;    
         outset[1] = (uc >> 8) & 0xff;
         outset[2] = outset[4] = ui & 0xff;    
         outset[3] = outset[5] = (ui >> 8) & 0xff;  
         CAN.sendMsgBuf(0x05FF4000 + 4 * ps_no, 1, 8, outset);
         ps_alive[ps_n] = 1;
         }
   if (canId == c3)      
     {
       ps_alive[ps_n] = 0;
       i[ps_n] = v[ps_n] = 0.0;
     }
 }


void do_lcd()
 {
   char fbuf[10],hbuf[10],gbuf[10];
   int cmode,n,p[4];

   p[0] = 0; p[1] = 40; p[2] = 20; p[3] = 60;

   tamps = 0.0;
   for(n=0; n < nps; n++)
     tamps += i[n];
     
   if ( (cpot - tamps) < 1.0)
     cmode = 1;
   else
     cmode = 0;

   dtostrf(pot,4,1,gbuf);
   dtostrf(cpot,5,1,fbuf);
   dtostrf(tamps,5,1,hbuf);
   if (cmode == 1)
       sprintf(&lcdbuf[16],"CC  ");
   else
       sprintf(&lcdbuf[16],"CV  ");
   sprintf(&lcdbuf[35],"%4s ",gbuf);
   sprintf(&lcdbuf[54],"%5s ",fbuf);
   sprintf(&lcdbuf[74],"%5sA",hbuf);

 if (b_mode == 0)
   {
     lcdbuf[39] = 'v';
     lcdbuf[59] = 'A';
   }
 else
   {
     lcdbuf[39] = 'V';
     lcdbuf[59] = 'a';
   }

   
   for (n=0; n < 4; n++)
     {
       if (ps_alive[n] == 1)
         {
           dtostrf(v[n],4,1,fbuf);  
           dtostrf(i[n],4,1,hbuf);
           sprintf(&lcdbuf[p[n]],"%d %4sV %4sA",n+1,fbuf,hbuf);
         }
       else
         {
           if (n < nps)
             sprintf(&lcdbuf[p[n]],"%d  0.0V  0.0A",n+1);
           else
             sprintf(&lcdbuf[p[n]],"%d            ",n+1);
         }
     lcdbuf[p[n]+ 13] = ' ';    
     }

 }
 

#include <SPI.h>
#include "mcp_can.h"
#include <mcp_can_dfs.h>
// first unit
unsigned char login[8] = {0x16, 0x36, 0x71, 0x07, 0x03, 0x76, 0x00, 0x00}; //this is for logging into your flatpack. Must use your serial number.

const int SPI_CS_PIN = 10;
MCP_CAN CAN(SPI_CS_PIN); // Set CS pin

#define VOLTAGE 4800
#define CURRENT 450

uint8_t outset[8] = {CURRENT & 0xff, (CURRENT >> 8) & 0xff, VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,VOLTAGE & 0xFF, (VOLTAGE >> 8) & 0xFF ,0x3E, 0x17};
volatile int got_cb_data = 0;
// 0x1130 = 44.00V
// 0x12C0 = 48.00V
// 0x1680 = 57.60V
// low byte first  {0x29, 0x15, 0x00, 0x30, 0x11};
unsigned char setdefaultvolt[5] = {0x29, 0x15, 0x00, 0xc0, 0x12};
float potvolts;

void setup()
{
   Serial.begin(115200);
   pinMode(3,INPUT);
   pinMode(12,INPUT);
START_INIT:
   if(CAN_OK == CAN.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ))
   {
       Serial.println("CAN BUS Shield init ok!");
   }
else
   {
   Serial.println("CAN BUS Shield init fail");
   Serial.println("Init CAN BUS Shield again");
   delay(1000);
   goto START_INIT;
   }

CAN.setMode(0);    
delay(200);
CAN.sendMsgBuf(0x05004804, 1, 8, login);    // id = 1 so XX = 04    
delay(200);
CAN.sendMsgBuf(0x05019C04, 1, 5, setdefaultvolt);  
}


void loop()
{

}  

CAN.sendMsgBuf(0x05004804, 1, 8, login);    // id = 1 so XX = 04    

// check for CAN message and parse
  was_msg = 0;  
  rv = CAN.checkReceive();
  if(rv == CAN_MSGAVAIL )
  {
    was_msg = 1;
    CAN.readMsgBuf(&len, buf);
    canId = CAN.getCanId();

    for(n=0; n < nps; n++)
        ps_check_send(n, buf,canId, ui, uc);  
    }