/*
Example sketch for demonstration of the DC motor custom chip.

The DC motor custom chip emulates a full H-bridge driver (e.g. L298N), DC motor and quadrature encoder assembly.

Physical parameters of the DC motor (and power supply) can be adjusted in the dc-motor.chip.c file.

Pinout:
- chip inputs V+/V- are IO-level commands from the microcontroller, possibly pulse-width-modulated, for the V+/V- motor terminals.
- chip outputs ENC_A/ENC_B is the quadrature encoder output
- chip output vel is an analog measure of shaft velocity

Notes: 
Motor behavior is simulated at constant (arbitrary) time intervals of 10µs, therefore small signal porches might be missed.

*/

#include <Arduino.h>


volatile int motorPos = 0;

#define ppr 100 // pulses per revolution

#define PWM_FREQ 5000
#define PIN_MOTOR_P 25
#define PIN_MOTOR_N 26

#define PWM_CHANNEL_0 0
#define PWM_CHANNEL_1 1

#define PIN_ENCODER_A 34
#define PIN_ENCODER_B 35

#define MIN(a,b) (((a)<(b)) ? (a) : (b))
#define MAX(a,b) (((a)>(b)) ? (a) : (b))

void IRAM_ATTR pinChangeInterrupt();
void setSpeed(int speed);


const float kp = 1.8; /* proportional factor */
const float ti = 8.0; /* time constant of integral term */
const float td = 0.3; /* time constant of derivative term */


void setup() {
  Serial.begin(115200);
 
  /* motor outputs */
  pinMode(PIN_MOTOR_P, OUTPUT);
  pinMode(PIN_MOTOR_N, OUTPUT);

  /* 5 kHz PWM, 8-bit resolution */
  ledcAttach(PIN_MOTOR_P, PWM_FREQ, 8);
  ledcAttach(PIN_MOTOR_N, PWM_FREQ, 8);

  /* quadrature encoder inputs */
  pinMode(PIN_ENCODER_A, INPUT);
  pinMode(PIN_ENCODER_B, INPUT);

  attachInterrupt(PIN_ENCODER_A, pinChangeInterrupt, CHANGE);

  /* potentiometer input */
  analogReadResolution(10);

  Serial.printf("setPos, motorPos, err, der, int, cmd\n");
}


void loop() {
  static float integral = 0;
  static float prev_err = 0;

  setSpeed(255);

  return; 

  /* Read user input */
  int setpos = analogRead(2);
  
  /* PID regulation */
  float err = setpos - motorPos;
  
  float dt = 50.0/1000.0; // approx. 50ms
  integral += err * dt;
  integral = MAX(-1000, MIN(1000, integral)); /* basic anti-windup */
 
  float derivative = (err - prev_err) / dt;
  
  float cmd = kp * (err + integral / ti + derivative * td);

  /* Actuate motor */
  setSpeed((int)cmd);

  /* Print out data for display (use the Serial plotter to graph out data) */
  Serial.printf("%d, %d, %3.3f, %3.3f, %3.3f, %3.3f\n", 
      setpos, motorPos, err, derivative, integral, cmd);

  delay(50);
}


/**
 * @brief Set motor speed
 * @param speed (-255: full speed CCW ... 0: stop ... +255: full speed CW)
 */
void setSpeed(int speed) {
  speed = MAX(-255, MIN(255, speed));

  if (speed > 0) {
    ledcWrite(PWM_CHANNEL_0, 255);
    ledcWrite(PWM_CHANNEL_1, 255 - speed);
  } else {
    ledcWrite(PWM_CHANNEL_0, 255 + speed);
    ledcWrite(PWM_CHANNEL_1, 255);
  }
}


/**
 * @brief Read quadrature encoder signals, and update shaft position
 */
void IRAM_ATTR pinChangeInterrupt() {
  int a = digitalRead(PIN_ENCODER_A);
  int b = digitalRead(PIN_ENCODER_B);

  motorPos += (a ^ b) ? 1 : -1;
}