You are on page 1of 4

#include <DueTimer.

h>
#include "sw.h"
#include <PID_v1.h>

///////////////////////////////// VARIAVEIS
GLOBAIS ///////////////////////////////////////////

unsigned int soma_pos = 0;

unsigned int pos = 0;


unsigned int media_pos[40];

int habilita = 0;
int i, j, k, l, m, n;
int contador_controle = 0;
int contador_serial = 0;
int contador_debug = 0;
int timeout = 0;
unsigned int quantidade_media_pos = QUANTIDADE_MEDIA_POS;

//********************************************CODIGO
ENCODER**********************************************//
//const int EncoderAPin = PIN_ENCODER_A;

//*********************************************************************************
**********************//

double Setpoint, Input, Output;


double erro, erro_ant, Output_ant;

//PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //PID de controle
de rotação

void setup()
{
analogReadResolution(12);
analogWriteResolution(12);
Serial.begin(9600); // Comunicação Serial para depurar o programa
Timer3.start(10); // interrupção a cada 1 milisegundos
Timer3.attachInterrupt(base_tempo); // attach the service routine here*/

//lcd.begin(16, 2);
//lcd.setCursor(0,0);
//lcd.print("IMT-Contr Dino");
//lcd.setCursor(0,1);
//lcd.print("rpm:");

pinMode(LED_PIN, OUTPUT);
pinMode(53, INPUT);

//myPID.SetOutputLimits(0, 4095);
//myPID.SetSampleTime(10); //frequencia de amostragem
//myPID.SetMode(AUTOMATIC);
Setpoint = 0.004;//2500;//2150; //setpoint fixo 1,73v
Input = 0;
Output = 0;
erro = 0;
erro_ant = 0;
Output_ant = 0;

//***********************codigo encoder*********************************//
//attachInterrupt(PIN_ENCODER_A, VelocidadeEixo, RISING); // interrupão
externa
//**********************************************************************//

//for(i = 0; i<=40; i++) //zera o vetor media utilizado na leitura do


encoder
//{
// media_rpm[i] = 0;
// media_setpoint[i] = 0;
// }
}

void loop()
{
//pos = analogRead(A1);
//if( digitalRead(53) == HIGH)
//{
// digitalWrite(LED_PIN, LOW);
//}
// else
//{
// digitalWrite(LED_PIN, HIGH);
// }
if(contador_controle >= 100)
{
////////////////////////////////////////////////////////MEDIA
RPM////////////////////////////////////////////////////////////
// for(j = quantidade_media_pos; j>0; j--) //
//
// {
//
// media_pos[j] = media_pos[j-1];
// }
// media_pos[0] = analogRead(A1);

// for(int k = 0; k < quantidade_media_pos; k++) //somatoria dos valores


// {
// soma_pos = soma_pos + media_pos[k];
// }
//pos = soma_pos/quantidade_media_pos;
//soma_pos = 0;

//
//

//
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////
pos = analogRead(A1);
//if(pos >= 1000)
//{
// digitalWrite(LED_PIN, LOW);
//}
//else
// {
// digitalWrite(LED_PIN, HIGH);
//}
Input = -0.0000006*pos + 0.0042;
avanco_fase(); //calculos de controle para amostragem a cada 1ms
analogWrite(11, int(Output)+2867);
contador_controle = 0;
}

if(contador_serial > 10000)


{
enviar_serial();
contador_serial = 0;
}

if(contador_debug > 5000)


{

contador_debug = 0;
}

void enviar_serial(void)
{
Serial.print ("pos: ");
Serial.print(pos); //rotação
Serial.print ('\t');

Serial.print ("Setpoint:");
Serial.print(Input); //Setpoint
Serial.print ('\t');

Serial.print ("U: ");


Serial.println(Output); //Setpoint
//Serial.print (z,DEC); /// leitura da determinação de atitude somente com
acelerometro
//Serial.println (var_pro,DEC); /// leitura do dado estimado com Filtro de
Kalman
//Serial.print ('\t');
//Serial.println (esforco_proporcional,DEC);
//Serial.print ('\t');
//Serial.println (esforco_integrador,DEC);
//Serial.print ('\t');
//Serial.println (esforco_derivativo,DEC);
// Serial.println (esforco_controle,DEC);
// Serial.println (PWM2_M2,DEC);
//Serial.print ('\t');
//Serial.println (PWM1_M2,DEC);
//Serial.print ('\n');
}

void avanco_fase()
{
//erro = Setpoint - Input;

//Output = 0.548*Output_ant + 21216*erro -19985*erro_ant;


//Output = Output*227.55;
//if(Output >= 4095.0) Output = 4095.0;
//if(Output <= -2867.0) Output = -2867.0;

//Output_ant = Output;
//erro_ant = erro;

void base_tempo()
{

//digitalWrite(LED_PIN, digitalRead(LED_PIN) ^ 1);


contador_controle++;
contador_serial++;
contador_debug++;
}

You might also like