/*******************************************************
 * APP6.23 - Torque Control Software
 * Dual DC motor speed controller
 * Processor: 328P
 * Hardware Version: 1.0
 * Version Date: 27.05.2016
 ********************************************************/

//#define PWM_FREQ_1KHZ    0
//#define PWM_FREQ_5KHZ    0
#define PWM_FREQ_10KHZ    0

#define clear_M1_D2()     PORTD &= ~(1 << 4)
#define set_M1_D2()       PORTD |= (1 << 4)

#define clear_M2_D1()     PORTD &= ~(1 << 7)
#define set_M2_D1()       PORTD |= (1 << 7)

#define clear_M2_D2()     PORTB &= ~(1 << 0)
#define set_M2_D2()       PORTB |= (1 << 0)

unsigned int M1_direction;    // FORWARD or REVERSE for the motor 1
unsigned int M1_start_stop;    // START or STOP for the motor 1
unsigned int M1_fault_in;        // Fault input from the motor 1
unsigned int M1_torque_set_pnt;  // Torque setpoint for the motor 1

unsigned int M2_direction;   // FORWARD or REVERSE for the motor 2
unsigned int M2_start_stop;    // START or STOP for the motor 2
unsigned int M2_fault_in;        // Fault input from the motor 2
unsigned int M2_torque_set_pnt;  // Torque setpoint for the motor 2

void setup()
{
  Serial.begin(9600);
  /*
  * Motor 1 Controls:
  * -----------------
  * D1:          Tied externally
  * D2:          PORTD.4
  * IN1:         PORTD.5
  * IN2:         PORTD.6
  * Start-Stop:  PORTC.2
  * Direction:   PORTC.3
  * Fault:       PORTD.2
  *
  */

  // D4, D5, D6 are outputs
  DDRD |= (1 << 4);
  DDRD |= (1 << 5);
  DDRD |= (1 << 6);

  /*
   * Motor 2 Controls:
   * -----------------
   * D1:          PORTD.7
   * D2:          PORTB.0
   * IN1:         PORTB.1
   * IN2:         PORTB.2
   * Start-Stop:  PORTC.4
   * Direction:   PORTC.5
   * Fault:       PORTD.3
   *
   */

  // D7, B0, B1 and B2 are outputs
  DDRD |= (1 << 7);
  DDRB |= (1 << 0);
  DDRB |= (1 << 1);
  DDRB |= (1 << 2);


  // Set M2_D1 and clear M2_D2
  // This will disable M2
  set_M2_D1();
  clear_M2_D2();

  // Clear M2_D2 so that it
  // will disable M1
  clear_M1_D2();

  // PWM setup for Motor 1:
  TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00);
  TCCR0B = _BV(CS01);
  OCR0A = 0;
  OCR0B = 0;

  // PWM setup for Motor 2:
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);
#ifdef PWM_FREQ_1KHZ
  TCCR1B = _BV(CS11) | _BV(WGM13);
  OCR1A = 0;
  OCR1B = 0;
  ICR1 = 999;
#endif
#ifdef PWM_FREQ_5KHZ
  TCCR1B = _BV(CS10) | _BV(WGM13);
  OCR1A = 0;
  OCR1B = 0;
  ICR1 = 1599;
#endif
#ifdef PWM_FREQ_10KHZ
  TCCR1B = _BV(CS10) | _BV(WGM13);
  OCR1A = 0;
  OCR1B = 0;
  ICR1 = 795;
#endif
}


void loop()
{
  UpdateInputs();
  UpdateAnalogs();
  HardwarePWM();
}

// Subroutines
void UpdateInputs()
// Update Digital inputs
{
  // Direction for Motor 1
  if ((PINC & (1 << 2)) == 0)
    M1_direction = 1;
  else
    M1_direction = 0;

  // Start-Stop for Motor 1
  if ((PINC & (1 << 3)) == 0)
    M1_start_stop = 1;
  else
    M1_start_stop = 0;

  // Fault Input for Motor 1
  if ((PIND & (1 << 2)) == 0)
    M1_fault_in = 0;
  else
    M1_fault_in = 0;

  // Direction for Motor 2
  if ((PINC & (1 << 5)) == 0)
    M2_direction = 1;
  else
    M2_direction = 0;

  // Start-Stop for Motor 2
  if ((PINC & (1 << 4)) == 0)
    M2_start_stop = 1;
  else
    M2_start_stop = 0;

  // Fault Input for Motor 2
  if ((PIND & (1 << 3)) == 0)
    M2_fault_in = 0;
  else
    M2_fault_in = 0;
}

void UpdateAnalogs()
{
  static unsigned char working = 0;
  static unsigned char adcPin = 0;
  static unsigned long tmp = 0;

  // Check if the ADC module is free and not working
  if (!working)
  {
    // Select the queued ADC channel
    ADMUX =   bit (REFS0) | (adcPin & 0x07);
    // Start a conversion
    bitSet (ADCSRA, ADSC);
    // Set the flag since the ADC is busy now.
    working = true;
  }

  // The ADC clears the bit when done
  // Check if the ADC is still working and busy
  if (bit_is_clear(ADCSRA, ADSC))
  {
    // Read analog inputs
    // The result is from 0 to 1023
    if (adcPin == 0)
    {
      tmp = (unsigned long)((unsigned long)(ADC) * (unsigned long)(25));
      tmp = tmp >> 8;
      M1_torque_set_pnt = (unsigned int)(tmp);
      Serial.println(M1_torque_set_pnt);
    }
    else
    {
#ifdef PWM_FREQ_1KHZ
      tmp = (unsigned long)((unsigned long)(ADC) * (unsigned long)(102));
      tmp = tmp >> 10;
      M2_torque_set_pnt = (unsigned int)(tmp);
#endif
#ifdef PWM_FREQ_5KHZ
      tmp = (unsigned long)((unsigned long)(ADC) * (unsigned long)(150));
      tmp = tmp >> 10;
      M2_torque_set_pnt = (unsigned int)(tmp);
#endif
#ifdef PWM_FREQ_10KHZ
      tmp = (unsigned long)((unsigned long)(ADC) * (unsigned long)(80));
      tmp = tmp >> 8;
      M2_torque_set_pnt = (unsigned int)(tmp);
#endif
    }
    // ADC is no longer busy, clear the flag
    working = false;
    // Toggle between ADC channels A0 and A1
    if (adcPin == 0)
      adcPin = 1;
    else
      adcPin = 0;
  }
}

void HardwarePWM()
{
  // If the motor is in START condition
  if (M1_start_stop == 1)
  {
    // Set M1_D2 so that it
    // will enable M1
    set_M1_D2();

    // If the direction is FORWARD
    if (M1_direction == 1)
    {
      OCR0A = M1_torque_set_pnt;
      OCR0B = 0;
    }
    else // if the direction is REVERSE
    {
      OCR0A = 0;
      OCR0B = M1_torque_set_pnt;
    }
  }
  // If the motor is in STOP condition
  else
  {
    // Clear M1_D2 so that it
    // will disable M1
    clear_M1_D2();

    OCR0A = 0;
    OCR0B = 0;
  }


  // If the motor is in START condition
  if (M2_start_stop == 1)
  {
    // Clear M2_D1 and set M2_D2
    // This will enable M2
    clear_M2_D1();
    set_M2_D2();

    // If the direction is FORWARD
    if (M2_direction == 1)
    {
      OCR1A = M2_torque_set_pnt;
      OCR1B = 0;
    }
    else // if the direction is REVERSE
    {
      OCR1A = 0;
      OCR1B = M2_torque_set_pnt;
    }
  }
  // If the motor is in STOP condition
  else
  {
    // Set M2_D1 and clear M2_D2
    // This will disable M2
    set_M2_D1();
    clear_M2_D2();

    OCR1A = 0;
    OCR1B = 0;
  }
}