// Rotina para acionar um pequeno motor CC a partir de um chopper de 4 quadrantes,
// ler a posição/velocidade angular a partir de um encoder em quadratura 
// e calcular o sinal de controle para controle da velocidade em malha fechada;
//
// UFOP, fevereiro de 2024;


#include "TimerOne.h" // biblioteca para criar PWM mais avancado

// Define uma macro que permite executar um trecho de código periodicamente a
// cada intervalo de tempo "t" milisegundos:
#define runEvery(t) for (static uint16_t _lasttime; (uint16_t)((uint16_t)millis() - _lasttime) >= (t); _lasttime += (t))

// Pinos para acionar chopper (PWM):
#define MOTOR_PWM1 9        // Pino 1 do PWM  [Pinos 9 ou 10 (Uno, mini-Pro) para PWM com Timer1]
#define MOTOR_PWM2 10       // Pino 2 do PWM  [Pinos 9 ou 10 (Uno, mini-Pro) para PWM com Timer1]
#define POT A0       // Pino potenciômetro

//************ DECLARACAO DE CONSTANTES / VARIAVEIS *********************
const unsigned Ts = 10;     // Período de amostragem [ms]
const float VFONTE = 11.0; // Tensão máxima de saida da ponte H [v];
uint16_t PWM_FREQ = 1e3;  // Frequencia do PWM [Hz]

// Para o encoder:
const unsigned NENC = 1;           // Numero de encoderes a serem usados
const float ENC_RES = 32;          // Resolução do encoder
volatile byte abOld;               // Old state
volatile byte changedCount;        // Change-indication
volatile long pulsosEncoder[NENC]; // current rotary pulsosEncoder
volatile long encoderAnt;
byte MASCARA = 0x0c;               // Pinso a ser usados na PCINT2

bool LIGAMOTOR = true; // flag para ligar/desligar o motor
bool MA = false; // flag para colocar em malha aberta (true) ou fechada (false);

// Para criação de sinais de referencia
uint8_t tipo_entrada; 
uint32_t dt_ref = 1000; // periodo entre mudanca de estado do sinal de referencia (1s)
float RAMPA = 5.0;      // inclinacao da rampa
float s, S;             // patamares para onda quadrada
bool decresce = true;   // flag para criação de um sinal rampa

// Declarando variaveis para os sinais de interesse:
float ref = 0, r = 0; // sinal de referencia inicial
float y = 0; // sinal de saida inicial (variavel controlada): velocidade angular [rad/s]
float u = 0, u_ant = 0; // cond. iniciais dos sinais de controle - u(k) e u(k-1);
float e = 0,  e_ant1 = 0, e_ant2 = 0; // cond. iniciais dos sinais de erro - e(k), e(k-1) e e(k-2);

int periodo_PWM;


// =================  ROTINA PARA O CONTROLADOR: =================================================
// A princípio, basta mudar somente esta rotina para colocar o seu controlador

void controlador(float r, float y) {   
  // Controlador PID
  float kp = -0.003740, ki = -0.001167, kd = 0.000834;  // Ts=0.003, Tau=0.030
  e = r-y;
  u = u_ant + (kp+ki+kd)*e - (kp+2*kd)*e_ant1 + kd*e_ant2;
  u = (u>VFONTE) ? VFONTE : u;    // saturando o sinal de controle caso MAIOR que a tensao da fonte.
  u = (u<-VFONTE) ? -VFONTE : u;  // saturando o sinal de controle caso MENOR que a tensao da fonte.
  // Atualizando vaores dos sinais para proxima iteracao:
  e_ant2 = e_ant1;
  e_ant1 = e;
  u_ant = u;
}

// ==============================================================================================

void setup() {
  Serial.begin(115200);          // Inicializa comunicacao serial

  // Configuracao do PWM
  pinMode(MOTOR_PWM1, OUTPUT);     // Configura pino do PWM como de saida
  pinMode(MOTOR_PWM2, OUTPUT);     // Configura pino do PWM como de saida
  digitalWrite(MOTOR_PWM1, LOW);
  digitalWrite(MOTOR_PWM2, LOW);
  periodo_PWM = 1e6/PWM_FREQ;
  Timer1.initialize(1.0e6/float(PWM_FREQ)); // Configura timer do PWM (período em micro-segundos)
  Timer1.pwm(MOTOR_PWM1, 0);       // Inicia PWM com ciclo de trabalho 0;
  Timer1.pwm(MOTOR_PWM2, 0);       // Inicia PWM com ciclo de trabalho 0;

  // Configuracao do encoder
  setaEncoders(INPUT_PULLUP, true, NENC);
}


void loop() {

  runEvery(30){leSerial();} // A cada 30 milisegundos, olha se tem comunicacao
                            // serial chegando

  runEvery(dt_ref){ajustaRef();} // A cada 'dt_ref' ms, muda (ajusta) o sinal de referencia

  runEvery(Ts)  // Código principal: a cada: período de amostragem (Ts) milisegundos é executado
  {
    y = leVelocidade2();  // Le (calcula) a variavel de saida - velociade angular (variavel controlada)
    r = map3(analogRead(POT), 0, 1023, -500, 500);
    controlador(r,y);    // executa a rotina do controlador propriamente dito (r-referencia; y-saida);
    Aciona_PWM(LIGAMOTOR? (MA ? r/50.0 : u) : 0); // Aciona o motor com ciclo de trabalho determinado por r, u ou 0:
    // - Caso motor esteja desligado (LIGAMOTOR = false), envia 0 valor a determinar o ciclo de trabalho do PWM; 
    // - Caso contrário:
    //       - Caso esteja em Malha Aberta (MA = true), envia 'r' como valor a determinar o ciclo de trabalho do PWM;
    //       - Caso esteja em Malha Fechada (MA = false), envia 'u' como valor a determinar o ciclo de trabalho do PWM; 
    
    enviaSerial(); // Evia sinais via comunicacao serial para o computador (ASCII)
  } 
} 
//--------


void leSerial()  // Rotina para receber cominicacao serial (ASCII) e tratar dados recebidos
  {
    // LEITURA DOS DADOS SERIAIS
    if ( Serial.available() > 0)
    {
      char ch = Serial.read();
      switch (ch) {
        case 'r':  // Caso valor passado comece com a letra r:
          ref = float(lenumero(ch)); // le o valor numerico depois de 'r' na variavel 'r';
          tipo_entrada = 1;        // ajusta sinal como valor constante
          break;
        case 's':  // Caso valor passado comece com a letra s:
          s = float(lenumero(ch)); // le o valor numerico depois de 's' na variavel 's';
          tipo_entrada = 2;        // ajusta sinal como "onda quadrada" (patamar inferior) 
          break;
        case 'S':  // Caso valor passado comece com a letra s:
          S = float(lenumero(ch)); // le o valor numerico depois de 'S' na variavel 'S';
          tipo_entrada = 2;        // ajusta sinal como "onda quadrada" (patamar superior) 
          break;
        case 't':  // Caso valor passado comece com a letra s:
          dt_ref = lenumero(ch); // ajusta periodo entre mudança de estado do sinal de referencia
          break;
        case 'a': // Caso valor passado comece com a letra a:
          MA = true;  // Coloca em malha aberta (referencia passa a ser sinal para o PWM)
          break;
        case 'f': // Caso valor passado comece com a letra f:
          MA = false;  // Coloca em malha fechada (sinal de controle 'u' passa a ser sinal para o PWM)
          break;
        case 'l': // Caso valor passado comece com a letra l:
          LIGAMOTOR = true;   // Coloca motor no estado ligado
          break;
        case 'd': // Caso valor passado comece com a letra d:
          LIGAMOTOR = false;   // Coloca motor no estado DESligado
          break;
      } // Fim do switch
    } // Fim da leitura serial
}


// Calcula a velocidade de giro do motor em rad/s a partir dos pulsos do encoder.
float leVelocidade(){
   float v =  2.0*PI*(float)pulsosEncoder[0]/Ts*1000.0/(float)ENC_RES/4.0; // retorna velocidade angular [rad/s]
   pulsosEncoder[0] = 0;
   return v;
}

// Calcula a velocidade de giro do motor em rad/s a partir dos pulsos do encoder.
float leVelocidade2(){
   long encoderAgora = pulsosEncoder[0];
   long delta_t = encoderAgora-encoderAnt;
   encoderAnt = encoderAgora;
   float v =  2.0*PI*(float)delta_t/Ts*1000.0/(float)ENC_RES/4.0; // retorna velocidade angular [rad/s]
   return v;
}

// Rotina para enviar os sinais via comunicacao serial:
void enviaSerial() {
  Serial.print(r);  // Sinal de referencia
  Serial.print(',');
  Serial.print(y);  // Sinal de saida (vel angular)
  Serial.print(',');
  Serial.print(u); // Sinal de controle ('tensao' alim. motor)
  Serial.print(',');
  Serial.print(periodo_PWM); 
  Serial.println();

}

void Aciona_PWM(float saida)
{
    if (saida < 0)
    {
      Timer1.setPwmDuty(MOTOR_PWM1, map2(saida, 0, -VFONTE, 1, 1024));    // aciona motor em um sentido
      Timer1.setPwmDuty(MOTOR_PWM2, 0); 
    }
    else
    {
      Timer1.setPwmDuty(MOTOR_PWM1, 0);                                 // aciona motor em outro sentido
      Timer1.setPwmDuty(MOTOR_PWM2, map2(saida, 0, VFONTE, 1, 1024));
    }
}

// Rotina para criar sinais de referencia:
// caso 1: sinal constante;
// caso 2: sinal na forma de uma onda quadrada;
// caso 3: sinal na forma de rampa;
void ajustaRef()
{
      switch (tipo_entrada)
      {
        case 1:
          break;
        case 2: // ONDA QUADRADA
          r = (r == S) ? s : S;
          break;
        case 3: // RAMPA
          if ((r >= RAMPA) && (!decresce))
            decresce = 1;
          if ((r <= -RAMPA) && (decresce))
            decresce = 0;
          r = r + (decresce ? -10 : 10);
          break;
      }
  }

long int lenumero(char caracter)
{
  long int valor = 0;
  int sinal = 1;
  while (caracter != 10)                    // verifica se eh um digito entre 0 e 9
  {
    caracter = Serial.read();
    if (caracter >= '0' && caracter <= '9')   // verifica se eh um digito entre 0 e 9
      valor = (valor * 10) + (caracter - '0');  // se sim, acumula valor
    else if ( caracter == '-')
      sinal = -1;
    else                                      // em caso de nao ser um numero ou simbolo de menos termina o valor
    {
      valor = valor * sinal ;                   // seta a variavel valor com o valor acumulado
      // valor = 0;                             // reseta valor para 0 para a proxima sequencia de digitos
      sinal = 1;
    }
  }
    return (valor); //WARN: Modifiquei o lugar desse return (ver se vai continuar funcionando)
}

long map2(float x, float in_min, float in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
float map3(int x, int in_min, int in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// FUNCOES PARA TRATAMENTO DO ENCODER EM QUADRATURA:

void setaEncoders(byte ioMode, byte enablePCI, byte NENC) {
  switch (NENC) {
      case 1:
        MASCARA = 0x0c;
        pinMode( 2, ioMode); //  2, PD2   PCINT 18
        pinMode( 3, ioMode); //  3, PD3   PCINT 19
        break;
      case 2:
        MASCARA = 0x3c;
        pinMode( 2, ioMode); //  2, PD2   PCINT 18
        pinMode( 3, ioMode); //  3, PD3   PCINT 19
        pinMode( 4, ioMode); //  4, PD4   PCINT 20
        pinMode( 5, ioMode); //  5, PD5   PCINT 21
        break;
      case 3:
        MASCARA = 0xfc;
        pinMode( 2, ioMode); //  2, PD2   PCINT 18
        pinMode( 3, ioMode); //  3, PD3   PCINT 19
        pinMode( 4, ioMode); //  4, PD4   PCINT 20
        pinMode( 5, ioMode); //  5, PD5   PCINT 21
        pinMode( 6, ioMode); //  6, PD6   PCINT 22
        pinMode( 7, ioMode); //  7, PD7   PCINT 23
        break;
      case 4:
        MASCARA = 0xff;
        pinMode( 0, ioMode); //  2, PD0   PCINT 16
        pinMode( 1, ioMode); //  3, PD2   PCINT 17
        pinMode( 2, ioMode); //  2, PD2   PCINT 18
        pinMode( 3, ioMode); //  3, PD3   PCINT 19
        pinMode( 4, ioMode); //  4, PD4   PCINT 20
        pinMode( 5, ioMode); //  5, PD5   PCINT 21
        pinMode( 6, ioMode); //  6, PD6   PCINT 22
        pinMode( 7, ioMode); //  7, PD7   PCINT 23
        break;
      default:
        MASCARA = 0x03;
        pinMode( 2, ioMode); //  2, PD2   PCINT 18
        pinMode( 3, ioMode); //  3, PD3   PCINT 19
        break;
  }
  if (enablePCI) {
    PCMSK2 |= MASCARA; // Seleciona quais pinos monitorar no PCINT2
    PCIFR  |= 0x04; // Limpa as interrupcoes por mudança no pino (PC - Pin Change)
    PCICR  |= 0x04; // Habilita interrupcoes PC
  }
  abOld = changedCount = 0;
  for(byte i=0; i<NENC; ++i){
    pulsosEncoder[i] = 0;
  }
}

// Trata interrupcoes externas (para o encoder):

ISR(PCINT2_vect) {
  byte abNew = PIND & MASCARA;      // Le os pinos D(0,1,2,3,4,5,6,7) e filtra pela mascara                    
  byte changes = abNew ^ abOld;     // Usa XOR para marcar bits alterados           
  char wayflag = 2 * abNew ^ abNew; // Usa XOR para obter bits de mudanca de estado 
  byte test2;
  for (byte i = 0; i < NENC; ++i) {
    // Para PD2-7, os 2 bits inferiores não são relevantes. Mova as mudanças para
    // fim do loop se os 2 bits inferiores carregam informações do encoder.
    wayflag >>= 2;        // Descarta dois bits de estado inferiores
    changes >>= 2;        // Descarte dois bits de mudança inferiores
    test2 = changes & 3;  // Obtem os 2 bits de interesse
      if (test2 == 1) {
        pulsosEncoder[i] -= 1 - (wayflag & 2);
        changedCount = true;
      } else if (test2 == 2) {
        pulsosEncoder[i] += 1 - (wayflag & 2);
        changedCount = true;
      }
  }
  abOld = abNew;   // Salva novo estado
}