// Press brake control using stepper driven hydrualic valves
// Robogaia 6 channel encoder/counter shield set to set count to zero when the index is triggered
// 200 ppr encoders on stepper motors for closed loop.   set by index to zero when valve is centered and closed
// stepper motor only moves each direction approximately 65 degrees from center
// linear encoders on the left and right cylinders for syncing the posision and home switch at top to set them to zero
// simplified explanation is the left and right valve motor are constanly tring to match there encoder number to the requseted number.
// the next phase will be to stop via posision on the linear glass scales.
#include <SPI.h> 

#define CNTR B00100000
#define CLR B00000000

//the lines are used by 74HC138 chip to select the cable select lines
int chipSelectPinC = 10;//C  A2
int chipSelectPinB = 9; //B  A1 
int chipSelectPinA = 8; //A  A0
//press brake out pins
int rStpPls = 4; //pulse for right stepper drive
int rStpDir = 5; //direction for right stepper drive
int lStpPls = 2; //pulse for left stepper drive
int lStpDir = 3; //direction for left stepper drive
//press brake in pins
const int rCylHmBtn= 43; // right cylinder home switch
const int lCylHmBtn= 45; // left cylinder home switch
const int upSwBtn = 41; // up pedal switch
const int dnSwBtn = 39; // down pedal switch
const int htSwBtn = 49; // switch to stop press at set hight
const int stopSwBtn = 47; // switch to stop press on return home
// varibles in code
int stepdelay = 1;
int rCylHm = 0; // var right cylinder home switch
int lCylHm = 0; // var left cylinder home switch
int dnSw = 0; //  var down pedal switch
int upSw = 0; // var up pedal swithc
int htSw = 0; // var switch to stop press at set hight
int stopSw = 0; // var switch to stop press on return home
int stpDn = -67; // number of steps to open valve completly going down
int stpUp = 67; // number of steps to open valve completly going up
int lReq = 0; // requested pulse number for left stepper 
int rReq = 0; // requested pulse munber for right stepper
int lCor = 0; // correction number to alter left requested pulse number
int rCor = 0; // correction number to alter right requested pulse number
int rPos = 0; // right cylinder position
int lPos = 0; // left cyinder position
int rMtrPos = 0; // right stepper encoder position
int lMtrPos = 0; // left stepper encoder position
int rpuls = 0;  // number to watch on serial to see if code is in use
int lpuls = 0;  // number to watch on serial to see if code is in use
//*****************************************************
void setup()
//*****************************************************
{
    //Serial.begin(19200);
    // Serial.begin(9600);
//press brake out pins
pinMode (rStpDir, OUTPUT);
pinMode (rStpPls, OUTPUT);
pinMode (lStpPls, OUTPUT);
pinMode (lStpDir, OUTPUT);
//press brake in pins
pinMode (rCylHmBtn, INPUT);
pinMode (lCylHmBtn, INPUT);
pinMode (upSwBtn, INPUT);
pinMode (dnSwBtn, INPUT);
pinMode (htSwBtn, INPUT);
pinMode (stopSwBtn, INPUT); 


    pinMode(chipSelectPinC, OUTPUT);
    pinMode(chipSelectPinB, OUTPUT);
    pinMode(chipSelectPinA, OUTPUT);

    digitalWrite(chipSelectPinC, HIGH);
    digitalWrite(chipSelectPinB, HIGH);
    digitalWrite(chipSelectPinA, HIGH);

    //initialize the register in all of the 6 chips
    LS7366_Init();

    delay(100);

    //reseting the counter value inside the encoder chips to 0
    resetEncoder(1);
    resetEncoder(2);
    resetEncoder(3);
    resetEncoder(4);
    resetEncoder(5);
    resetEncoder(6);

} //end func

//main loop
//*****************************************************
void loop()
  //*****************************************************
  {
    //long encoder1Value;
    long encoder2Value;
    long encoder3Value;
    long encoder4Value;
    long encoder5Value;
    //long encoder6Value;

    
    encoder5Value = getEncoderValue(5);
    Serial.print("l cyl = ");
    lPos = encoder5Value; // left cylinder 
    Serial.print(lPos);
    
    encoder4Value = getEncoderValue(4);
    Serial.print(" r cyl = ");
    rPos = encoder4Value; // right cylinder
    Serial.print(rPos);
    
    encoder2Value = getEncoderValue(2);
    Serial.print(" l stpr = "); 
    lMtrPos = encoder2Value; // left motor
    Serial.print(lMtrPos);

    encoder3Value = getEncoderValue(3);
    Serial.print(" r stpr = ");
    rMtrPos = encoder3Value; // right motor
    Serial.print(rMtrPos);
 
    Serial.print (" l rq #");
    Serial.print (lReq);

    Serial.print (" l cr #");
    Serial.print (lCor);

    Serial.print (" r rq #");
    Serial.print (rReq);

    Serial.print (" r cr #");
    Serial.print (rCor);

    Serial.print (" r pulse");
    Serial.print (rpuls); //number to watch on serial to check code is working

    Serial.print (" l pulse");
    Serial.print (lpuls); //number to watch on serial to check code is working
    
    Serial.print("\r\n");

 // press brake sw set
// when switch is on it makes the requested number =(stpDn is positive) or (stpUp is negative) - the correction amount)
// example 65-2=63  or -65-2=67
// the request number can never be more than the stpDn or stpUp # (67)

    dnSw = digitalRead(dnSwBtn);
    upSw = digitalRead(upSwBtn);
    htSw = digitalRead(htSwBtn);
    stopSw = digitalRead(stopSwBtn);
    
     if (dnSw == LOW && upSw == LOW){
      
       lReq = (0);
       rReq = (0);
    }
    
    if (dnSw == HIGH && htSw == LOW){
      lReq = stpDn + lCor;
      rReq = stpDn + rCor; 
        if (lPos == rPos){
          lCor = 0 ;
          rCor = 0 ;
                }
       if (lPos > rPos && rReq == stpDn){
        lCor = constrain(lCor, 0 , 66);
        lCor ++; // if the left cylinder is ahead of the right and its not at max step down position increase the corection number
       }
       if (lPos > rPos && rReq != stpDn){
        rCor = constrain(rCor, 0 , 66);
        rCor--;
       }
       
       if (rPos > lPos && lReq == stpDn){
        rCor = constrain(rCor, 0 , 66);
        rCor ++; // if the right cylinder is ahead of the left and its not at max step down position increase the r corection number
       }
       if (rPos > lPos && lReq != stpDn){
        lCor = constrain(lCor, 0 , 66);
        lCor--; // else increase the left 
       }    
    }

     if (upSw == HIGH && stopSw == LOW){
      lReq = stpUp - lCor;
      rReq = stpUp - rCor;
                if (lPos == rPos){
                lCor = 0;
                rCor = 0;
                } 
                if (lPos > rPos && rReq == stpUp){
                lCor = constrain(lCor, 0 , 66);
                lCor ++; // if the left cylinder is ahead of the right and its not at max step down position increase the corection number
                }
                if (lPos > rPos && rReq != stpUp){
                rCor = constrain(rCor, 0 , 66);
                rCor--;
                }
                if (rPos > lPos && lReq == stpUp){
                rCor = constrain(rCor, 0 , 66);
                rCor ++; // if the right cylinder is ahead of the left and its not at max step down position increase the r corection number
                }
                if (rPos > lPos && lReq != stpUp){
                lCor = constrain(lCor, 0 , 66);
                lCor--; // else increase the left 
                }
     }
               
    
    

 
    // if right motor position less than right motor requested number advance 1 step
    if (rMtrPos < rReq){
      digitalWrite (rStpDir, HIGH);
      //delay(stepdelay);
      digitalWrite(rStpPls, HIGH); 
      delay(stepdelay); 
      digitalWrite(rStpPls, LOW); 
      delay(stepdelay);
      rpuls ++; //number to watch on serial to check code is working
    }

    // if right motor position grater than right motor requested number retard 1 step    
    if (rMtrPos > rReq){
      digitalWrite (rStpDir, LOW);
      //delay(stepdelay);
      digitalWrite(rStpPls, HIGH); 
      delay(stepdelay); 
      digitalWrite(rStpPls, LOW); 
      delay(stepdelay);
      rpuls --; //number to watch on serial to check code is working
    }


    // if left motoer position less than left motor requested number advance 1 step    
    if (lMtrPos < lReq){
      digitalWrite (lStpDir, HIGH);
      //delay(stepdelay);
      digitalWrite(lStpPls, HIGH); 
      delay(stepdelay); 
      digitalWrite(lStpPls, LOW); 
      delay(stepdelay);
      lpuls ++; //number to watch on serial to check code is working
    }

    // if left motoer position grater than left motor requested number retard 1 step    
    if (lMtrPos > lReq){
      digitalWrite (lStpDir, LOW);
      //delay(stepdelay);
      digitalWrite(lStpPls, HIGH); 
      delay(stepdelay); 
      digitalWrite(lStpPls, LOW); 
      delay(stepdelay);
      lpuls --; //number to watch on serial to check code is working
    }



  

  } //end func






//*****************************************************  
long getEncoderValue(int encoder)
//*****************************************************
{
    unsigned int count1Value, count2Value, count3Value, count4Value;
    long result;

    selectEncoder(encoder);

    SPI.transfer(0x60); // Request count
    count1Value = SPI.transfer(0x00); // Read highest order byte
    count2Value = SPI.transfer(0x00);
    count3Value = SPI.transfer(0x00);
    count4Value = SPI.transfer(0x00); // Read lowest order byte

    deselectEncoders();

    result = ((long) count1Value << 24) + ((long) count2Value << 16) + ((long) count3Value << 8) + (long) count4Value;

    return result;
} //end func

//*****************************************************
void resetEncoder(int encoder)
//*****************************************************
  {
    selectEncoder(encoder);
    SPI.transfer(CLR | CNTR);
    deselectEncoders();

  } //end func

//this will control the 74HC138 to set the lines low for SPI cable select
//*************************************************
void selectEncoder(int encoder)
//*************************************************
{
   switch (encoder) 
   {
    case 1:
      digitalWrite(chipSelectPinC, LOW); //C A2  //0 A=LOW B=LOW C=LOW
      digitalWrite(chipSelectPinB, LOW); //B A1 
      digitalWrite(chipSelectPinA, LOW); //A A0
      break;
    case 2:
      digitalWrite(chipSelectPinC, LOW); //C   A2//1 A=HIGH B=LOW C=LOW
      digitalWrite(chipSelectPinB, LOW); //B   A1
      digitalWrite(chipSelectPinA, HIGH); //A  A0
      break;
    case 3:
      digitalWrite(chipSelectPinC, LOW); //C   A2  A=LOW B=HIGH C=LOW
      digitalWrite(chipSelectPinB, HIGH); //B  A1
      digitalWrite(chipSelectPinA, LOW); //A   A0
      break;
    case 4:
      digitalWrite(chipSelectPinC, LOW); //C  A2  A=LOW B=HIGH C=HIGH
      digitalWrite(chipSelectPinB, HIGH); //B  A1
      digitalWrite(chipSelectPinA, HIGH); //A  A0
      break;
    case 5:
      digitalWrite(chipSelectPinC, HIGH); //C  //4 A2  A=HIGH B=LOW C=LOW 
      digitalWrite(chipSelectPinB, LOW); //B       A1
      digitalWrite(chipSelectPinA, LOW); //A       A0
      break;
    case 6:
      digitalWrite(chipSelectPinC, HIGH); //C  //5  A2 A=HIGH B=LOW C=HIGH 
      digitalWrite(chipSelectPinB, LOW); //B        A1
      digitalWrite(chipSelectPinA, HIGH); //A       A0 
      break;
  } //end switch

} //end func

//*************************************************
void deselectEncoders()
//*************************************************
{

    //select the 8th output low which puts all the cable select lines for the 6 encoder chips high and disables the communication
    digitalWrite(chipSelectPinC, HIGH);
    digitalWrite(chipSelectPinB, HIGH);
    digitalWrite(chipSelectPinA, HIGH);

} //end func

// LS7366 Initialization and configuration
//*************************************************
void LS7366_Init(void)
//*************************************************
{

    int a = 0;
    // SPI initialization
    SPI.begin();
     SPI.setClockDivider(SPI_CLOCK_DIV16);      // SPI at 1Mhz (on 16Mhz clock)
    delay(10);

    //initialize the 6 
    for (a = 1; a < 7; a++) 
  {
      selectEncoder(a);
      SPI.transfer(0x88); //Instruction to write to MDRO
      //SPI.transfer(0x20); //byte to select index_reset CNTR
      SPI.transfer(0x80);//byte to select 0x00=asychronous index 0x80= sychronous index
      deselectEncoders();
    }
} //end func