/* Example of driving servomotor using PID closed loop control */

#include <SPI.h>     /* Needed to communicate with LS7366R (Counter Click) */
#include <PID_v1.h>

/* Serial input aspects are based closely upon: 
   http://forum.arduino.cc/index.php?topic=396450
   Example 4 - Receive a number as text and convert it to an int
   Modified to read a float */

/* LS7366R aspects very loosely based on concepts used in controlling
   the Robogaia 3-axis encoder shield though implementation is very different
   https://www.robogaia.com/3-axis-encoder-conter-arduino-shield.html */

/* Pins used for L298 driver */
const int enA = 13;      /* PWM output, also visible as LED */
const int in1 = 8;       /* H bridge selection input 1 */
const int in2 = 9;       /* H bridge selection input 2 */
const float minPercent = -100.0;
const float maxPercent = 100.0;

/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
const int chipSelectPin = 10;

/* Size of buffer used to store received characters */
enum {numChars = 32};

/* Intervals in milliseconds for user-defined timed loops */
const int printInterval = 1000;           
const int controlInterval = 20;

/* Global variables used in serial input */ 
char receivedChars[numChars];   // an array to store the received data
float dataNumber = 0;             
boolean newData = false;

/* Global variables used for motor control and encoder reading */
double percentSpeed = 0;
double encoderPosnMeasured = 0; 
double positionSetPoint = 0; 

/* PID */
double Kp = 0.1;
double Ki = 0.1;
double Kd = 0.05;

PID myPID(&encoderPosnMeasured, &percentSpeed, &positionSetPoint, Kp, Ki, Kd, DIRECT);

/* Global variables used for loop timing */
unsigned long prevMillisPrint = 0;        /* stores last time values were printed */
unsigned long prevMillisControl = 0;

/* Overlapping regions of memory used to convert four bytes to a long integer */
union fourBytesToLong
{
  long result;
  unsigned char bytes [4];
};

void setup() 
{
  Serial.begin(9600);
  Serial.print("Kp"); Serial.print(Kp);
  Serial.print("Ki"); Serial.print(Ki);
  Serial.print("Kd"); Serial.println(Kd);
  Serial.println("Enter desired motor position: ");
  
  /* Set up and initialise pin used for selecting LS7366R counter: hi=inactive */
  pinMode(chipSelectPin, OUTPUT);   
  digitalWrite(chipSelectPin, HIGH);

  SetUpLS7366RCounter();

  delay(100);

  /* Configure control pins for L298 H bridge */
  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);

  /* Set initial rotation direction */
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);

  delay(100);
  positionSetPoint = 0;
  encoderPosnMeasured=readEncoderCountFromLS7366R();
  myPID.SetOutputLimits(-100,100);
  myPID.SetMode(AUTOMATIC);
}

void loop() 
{
  unsigned long currentMillis = millis();

  // Call control loop at frequency controInterval
  if (currentMillis - prevMillisControl >= controlInterval) {
    // save the last time the control loop was called
    prevMillisControl = currentMillis;
    controlLoop();
  }

  // Call print loop at frequency of printInterval
  if (currentMillis - prevMillisPrint >= printInterval) {
    // save the last time you printed output
    prevMillisPrint = currentMillis;
    printLoop();
  }
  
  recvWithEndMarker();// Update value read from serial line
  // If a valid number has been read this is set to the current required position
  if(convertNewNumber()){
     positionSetPoint=dataNumber;
  }
}

void controlLoop()
{
   // Get the current position from the encoder
  encoderPosnMeasured=readEncoderCountFromLS7366R();  // Get current motor position
  myPID.Compute();          // Use the PID library to compute new value for motor input
  driveMotorPercent(percentSpeed);    // Send value to motor
}


void driveMotorPercent(double percentSpeed)
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
{
      percentSpeed = constrain(percentSpeed, -100, 100);
      int regVal = map(percentSpeed, -100, 100, -255, 255);
      analogWrite(enA, (int)abs(regVal));   // Write value to speed control pin
      digitalWrite(in1, regVal>0);          // Set the value of direction control pins to true or false
      digitalWrite(in2, !(regVal>0));
}

void printLoop()
/* Print count and control information */
{  
   double error;
   Serial.print("Actual position: ");
   Serial.print(encoderPosnMeasured);
   Serial.print("\t");
   Serial.print("Desired position: ");
   Serial.print(positionSetPoint);
   Serial.print("\t");
   error = positionSetPoint - encoderPosnMeasured; 
   Serial.print("Error: ");
   Serial.print(error); 
   Serial.print("\t");
   //   Serial.print(percentSpeed);
   Serial.print("\r\n");
}

 
long readEncoderCountFromLS7366R()
/* Reads the LS7366R chip to obtain up/down count from encoder.  Reads four
   bytes separately then concverts them to a long integer using a union */
{
    fourBytesToLong converter; /* Union of four bytes and a long integer */
    
    digitalWrite(chipSelectPin,LOW); /* Make LS7366R active */
    
    SPI.transfer(0x60); // Request count
    converter.bytes[3] = SPI.transfer(0x00); /* Read highest order byte */
    converter.bytes[2] = SPI.transfer(0x00); 
    converter.bytes[1] = SPI.transfer(0x00); 
    converter.bytes[0] = SPI.transfer(0x00); /* Read lowest order byte */
    
    digitalWrite(chipSelectPin,HIGH); /* Make LS7366R inactive */
   
    return converter.result;
}

void SetUpLS7366RCounter(void)
/* Initialises LS7366R hardware counter on Counter Click board to read quadrature signals */
{
    /* Control registers in LS7366R - see LS7366R datasheet for this and subsequent control words */
    unsigned char IR = 0x00, MRD0=0x00;
    
    // SPI initialization
    SPI.begin();
    //SPI.setClockDivider(SPI_CLOCK_DIV16);      // SPI at 1Mhz (on 16Mhz clock)
    delay(10);

   /* Configure as free-running 4x quadrature counter */
   digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
   /* Instruction register IR */
   IR |= 0x80;   /* Write to register (B7=1, B6=0) */
   IR |= 0x08;   /* Select register MDR0: B5=0, B4=0, B3=1 */
   SPI.transfer(IR); /* Write to instruction register */ 
   /* Mode register 0 */
   MRD0 |= 0x03;    /* 4x quadrature count: B0=1, B1=1 */
   /* B2=B3=0: free running.  B4=B5=0: disable index. */
   /* B6=0: asynchronous index.  B7: Filter division factor = 1. */
   SPI.transfer(MRD0);
   digitalWrite(chipSelectPin,HIGH); 

   /* Clear the counter i.e. set it to zero */
   IR = 0x00; /* Clear the instructino register IR */
   digitalWrite(chipSelectPin,LOW); /* Select chip and initialise transfer */
   IR |= 0x20; /* Select CNTR: B5=1,B4=0,B3=0; CLR register: B7=0,B6=0 */
   SPI.transfer(IR); /* Write to instruction register */ 
   digitalWrite(chipSelectPin,HIGH); 
}

void recvWithEndMarker() 
/* Receive data from serial port finishing with "newline" character. 
   Based on http://forum.arduino.cc/index.php?topic=396450 Example 4 */
{
    static byte ndx = 0;
    char endMarker = '\n';
    char rc;
    
    if (Serial.available() > 0) {     // If data is available read value from serial monitor
        rc = Serial.read();

        if (rc != endMarker) {        // Store character in buffer if not end marker
            receivedChars[ndx] = rc;
            ndx++;
            if (ndx >= numChars) {
                ndx = numChars - 1;
            }
        }
        else {      // Add end of string character and change flag to indicate new data is available
            receivedChars[ndx] = '\0'; // terminate the string
            ndx = 0;
            newData = true;
        }
    }
}

bool convertNewNumber() 
/* Converts character string to floating point number only if there are new
       data to convert, otherwise returns false */
{
    if (newData) {
        dataNumber = 0.0;             
        dataNumber = atof(receivedChars);   
        newData = false;
        return true;
    }
    else
    {
       return false;
    }
}