/* Example of driving servomotor and reading encoder signals */

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

/* 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 DC Motor driver */
#define enA 13      /* PWM output, also visible as LED */
#define in1 8       /* H bridge selection input 1 */
#define in2 9       /* H bridge selection input 2 */
#define minPercent -100.0
#define maxPercent 100.0

/* Used to to initiate SPI communication to LS7366R chip (Counter click) */
#define chipSelectPin 10

/* Size of buffer used to store received characters */
#define numChars 32

/* Intervals in milliseconds for user-defined timed loops */
#define printInterval 1000           

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

/* Global variable used for motor control */
double percentSpeed;

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

/* 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.println("Enter PWM duty cycle as a percentage (positive for forward, negative for reverse");
  
  /* 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);

}

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

  // Print out value to serial monitor at interval specified by printInterval variable
  if (currentMillis - prevMillisPrint >= printInterval) {
    // save the last time you printed output
    prevMillisPrint = currentMillis;
    printLoop();
  }

  // Check if new data has been input via serial monitor
  recvWithEndMarker();
  if(convertNewNumber())  // Update value read from serial line
  {
     percentSpeed=dataNumber;
     driveMotorPercent(percentSpeed);  // Send new speed value to motor driver
  }
}

void driveMotorPercent(double percentSpeed)
/* Output PWM and H bridge signals based on positive or negative duty cycle % */
{
      percentSpeed = constrain(percentSpeed, -100, 100);   // Value must be in range -100 to +100
      int regVal = map(percentSpeed, -100, 100, -255, 255);   // Scale value to range -255 to +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));         // depending on whether speed is positive or negative
}

void printLoop()
/* Print count and control information */
{
   /* Sample counter chip and output position and requested speed */
   long encoderCountFromLS7366R = readEncoderCountFromLS7366R();
   
   Serial.print("Count from LS7366R = ");
   Serial.print(encoderCountFromLS7366R); 
   Serial.print(" Percent speed = ");
   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)
/* Initialiseds 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;
    }
}