Controle P de atuador de marcha lenta

Dependencies:   mbed

main.cpp

Committer:
Marcelocostanzo
Date:
2019-04-08
Revision:
2:aee0cbc8b7dd
Parent:
0:b5a9e0614efd

File content as of revision 2:aee0cbc8b7dd:

#include "mbed.h"

#define setpoint 1000 //alvo de rotação
#define KP 0.0007     //ganho proporcional
#define close 0       //sentido dos passos
#define open 1        //sentido dos passos

//----Configurando pinos de saida-----------
DigitalOut DIR_STEP(D6);
DigitalOut STEP(D7);

//-----Configurando pino de interrupção externa-----------
InterruptIn crank_sensor(D8);

//-----Inclusão da função timer------------------
Timer t1;

//Serial pc(USBTX, USBRX);

//------Variavel global-------------------
float time_rpm = 0;

//----------Rotina de para ler o tempo entre bordas de descida--------------
void timer_read()
{
    t1.stop();
    time_rpm = t1.read();
    t1.reset();
    t1.start();   
}

//-------Rotina para referenciar o motor de passso-----------------------------------
void ref()
{
    for(int i = 0; i < 300; i++)//curso de 250 passos, 300 para garantia de referenciamento
    {
        DIR_STEP = close;
        STEP = 1;
        wait_us(10);
        STEP = 0;
        wait_ms(5);   
    }    
}

int main() 
{
    ref();//chama a rotina para refernciar
    
    //----------variaveis locais----------
    float rpm = 0, error = 0, delay = 0;
    int steps_counter = 0; 
    
    //--------habilita a interrupção externa e aponta para o endereço da rotina de medição de tempo--------------
    crank_sensor.fall(&timer_read);
    
    while(1) 
    {
        rpm = 1 / time_rpm; //converte tempo em frequencia
        
        error = setpoint - rpm; //calcula o erro entre alvo e feedback
        
        //-----Rotina caso o erro seja positivo---------
        if(error > 30)
        {
            DIR_STEP = close;
            STEP = 1;
            wait_us(10);
            steps_counter--;
            STEP = 0;
            delay = 1 / (error * KP);
            wait_ms(delay);
        }
        
        //-----Rotina caso o erro seja negativo---------
        if(error < -30)
        {   
            error = error * -1.0f;
            DIR_STEP = open;
            STEP = 1;
            wait_us(10);
            steps_counter++;
            STEP = 0;
            delay = 1 / (error * KP);
            wait_ms(delay);
        }
        
        //-----Rotina caso o erro seja neutro---------
        else
        {
            STEP = 0;
        }
        
        //pc.printf("\n\rerror %f",error);
        //pc.printf("\n\rdelay %f",delay);
    }
}