/*
   This program uses an Arduino for a closed-loop control of TWO brushless DC-motors.
   Motor motion is detected by a quadrature encoder.
   Two inputs per motor named STEP and DIR allow changing the target position.
   Serial port prints current position and target position every second.
   Serial input can be used to feed a new location for the servo (no CR LF).

   Pins used: please read defines and code below

   Please note PID gains kp, ki, kd need to be tuned to each different setup.


   */
//#define DEBUG

#include <PID_v1.h>
#define encoder0PinA  12
#define encoder0PinB  8
#define encoder1PinA  7
#define encoder1PinB  6
#define PWM           9
#define PWM1          10
#define CWCCW         11
#define CWCCW1        5


double kp=1,ki=0,kd=0.0;
double input=0, output=0, setpoint=0;
double input1=0, output1=0, setpoint1=0;
PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);
PID myPID1(&input1, &output1, &setpoint1,kp,ki,kd, DIRECT);

volatile long encoder0Pos = 0;
volatile long encoder1Pos = 0;

long previousMillis = 0;        // will store last time LED was updated

long target0=0;  // destination location at any moment
long target1=0;  // destination location at any moment

//for motor control ramps 1.4
bool newStep = false;
bool oldStep = false;

// Install Pin change interrupt for a pin, can be called multiple times
void pciSetup(byte pin)
{
    *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin));  // enable pin
    PCIFR  |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
    PCICR  |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
}

void setup() {
  pinMode(encoder0PinA, INPUT); digitalWrite(encoder0PinA, HIGH);
  pinMode(encoder0PinB, INPUT); digitalWrite(encoder0PinB, HIGH);
  pciSetup(encoder0PinA);
  pciSetup(encoder0PinB);

  pinMode(encoder1PinA, INPUT); digitalWrite(encoder1PinA, HIGH);
  pinMode(encoder1PinB, INPUT); digitalWrite(encoder1PinB, HIGH);
  pciSetup(encoder1PinA);
  pciSetup(encoder1PinB);

  TCCR1B = TCCR1B & 0b11111000 | 1; // set  Hz PWM
  Serial.begin (115200);
  //Setup the pid
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-150,150);
  //pinMode(13,OUTPUT); digitalWrite(13,HIGH); // disable motor brake
  pinMode(CWCCW,OUTPUT); analogWrite(PWM,255);

  myPID1.SetMode(AUTOMATIC);
  myPID1.SetSampleTime(1);
  myPID1.SetOutputLimits(-150,150);
  //pinMode(13,OUTPUT); digitalWrite(13,HIGH); // disable motor brake
  pinMode(CWCCW1,OUTPUT); analogWrite(PWM1,255);
  // step pin interrupts on rising edge
  attachInterrupt(0, countStep      , RISING);  // step0  input on interrupt 1 - pin 2
  attachInterrupt(1, countStep1     , RISING);  // step1  input on interrupt 1 - pin 3
}

void loop(){
    input = encoder0Pos;
    setpoint=target0;
    input1 = encoder1Pos;
    setpoint1=target1;
    myPID.Compute();
    myPID1.Compute();
    // interpret received data as an integer (no CR LR): provision for manual testing over the serial port
    if(Serial.available()) target1=Serial.parseInt();
    pwmOut(output);
    pwmOut1(output1);
#ifdef DEBUG
    if(millis() % 2000 == 0) target0=target1=random(1000); // that was for self test with no input from main controller

    if( millis() % 1000 ==0 )
    {
        Serial.print(target1);
        Serial.print(":");
        Serial.print(" P=");
        Serial.print(output);
        Serial.print(" P1=");
        Serial.print(output1);
        Serial.print(" 0: ");
        Serial.print(encoder0Pos); // print out current location every second
                Serial.print(" 1: ");
        Serial.println(encoder1Pos); // print out current location every second
    }
#endif
}

void pwmOut(int out) {
  // if(abs(out)<10) digitalWrite(13,LOW); else digitalWrite(13,HIGH); // brake, compensate dead-band
   if(out<0) { digitalWrite(CWCCW,HIGH);}
   else { digitalWrite(CWCCW, LOW); }
   analogWrite(PWM, 255-abs(out) );
  }


void pwmOut1(int out) {
  // if(abs(out)<10) digitalWrite(13,LOW); else digitalWrite(13,HIGH); // brake, compensate dead-band
   if(out<0) { digitalWrite(CWCCW1,HIGH);}
   else { digitalWrite(CWCCW1, LOW); }
   analogWrite(PWM1, 255-abs(out) );
  }


const int QEM [16] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0};               // Quadrature Encoder Matrix
static unsigned char New, Old;

  ISR (PCINT0_vect) // handle pin change interrupt for D8 to D13 here
 {
  Old = New;
  New = (PINB & 1 )+ ((PINB & 16)/8); //
  encoder0Pos+= QEM [Old * 4 + New];
}

static unsigned char New2, Old2;
ISR (PCINT2_vect) // handle pin change interrupt for D0 to D7 here
 {
  Old2 = New2;
  New2 =  (PIND & 192)/64; //(PIND & 128 )/128 + ((PIND & 64)/32); //
  encoder1Pos+= QEM [Old2 * 4 + New2];
}

void countStep(){ if (PINC&B0000001) target0++;else target0--; } // pin A0 represents direction0

void countStep1(){ if (PINC&B0000010) target1++;else target1--; } // pin A1 represents direction1