Tarea 3 (Evaluacion + Bonus)

Dependencies:   mbed

main.cpp

Committer:
contechno
Date:
2019-12-07
Revision:
0:f5dbc5db9794

File content as of revision 0:f5dbc5db9794:

#include "mbed.h"
#include "string.h" 

#define REFERENCEVOLTAGE 3.3    // Max Output voltage from DAC

AnalogIn y(PTB3); // Analog Input
AnalogOut u(PTE30); // Analog Output
Serial HC06(PTE0,PTE1);
PwmOut led1(LED_RED);

int Kp, Ki, Kd, Sp;
float pid,o,ai,ad,ap,med,err;
float err_v;
char buffer[6];

/* Funciones */
void cleanBuffer(char *buffer, int count) { 
    for(int i=0; i < count; i++) {
        buffer[i] = '\0';
    }
}

void PID(int Sp, int Kp, int Ki, int Kd) { 
    
    med = y.read()*999; // med = Ist-Wert (medido)
    err = (Sp-med); // err = Soll-Wert (Sp) - Ist-Wert (med) = Fehler/Reglerabweichung
    ap = Kp * err * 0.01f;  // Proportional
    ai = (Ki * err * 0.01f) + ai;   // Integral (aktueller Fehler wird immer weiter aufsummiert)
    ad = Kd * (err - err_v) * 0.01f; // Differential (groesse der Fehlerabweichung (= Geschwindigkeit der Aenderung)
    pid = (ap + ai + ad);
    
    if(pid <= 0) {
        pid = 0;
    }
    if(pid > 999) {
        pid = 999;
    }  
    
    // Normalize PID-Output
    err_v = err;
    o = pid / 999;
    u.write(o); // Write PID-value to Analog Output
    
    if(err > -2 && err < 2) {
        led1 = 0.9;
    }
    else {
        led1 = 1;
    }
    
    HC06.printf("SetPoint: %d \r\n",Sp);
    HC06.printf("Entrada: %0.2f \r\n",med);
    HC06.printf("Error: %0.2f \r\n",err);
    HC06.printf("Salida: %0.2f \r\n",pid);
    HC06.printf("DAC-Output: %0.2f V \r\n",o);
    HC06.printf("Voltaje: %0.2f V \r\n",o*REFERENCEVOLTAGE); // Resulting voltage (0V ... 3.3V)
    HC06.printf("\r\n");
    
    wait_ms(500);
}



int main() {
    
    led1 = 1;
    HC06.baud(9600);
    
    int readptr = 0;
    char c;
    
    
    HC06.printf("\r\nEnter SetPoint value (Sp): ");
    while( (c = HC06.getc()) != '\n') { // Get characters and write them to buffer[]-Chararray
        buffer[readptr++] = c; 
    }
    Sp = atoi(buffer); // Convert string (= Char[]-Array) to integer
    readptr = 0; // Reset String-Characterindex
    cleanBuffer(buffer,6); // Set all string-characters to \0 that no false chars remain for next use of buffer
    
    
    HC06.printf("\r\nEnter Proportional-Factor value (Kp): ");
    while( (c = HC06.getc()) != '\n') {
        buffer[readptr++] = c; 
    }
    Kp = atoi(buffer);
    readptr = 0;
    cleanBuffer(buffer,6);
    
    
    HC06.printf("\r\nEnter Integral-Factor value (Ki): ");
    while( (c = HC06.getc()) != '\n') {
        buffer[readptr++] = c; 
    }
    Ki = atoi(buffer);
    readptr = 0;
    cleanBuffer(buffer,6);
    
    
    HC06.printf("\r\nEnter Differential-Factor value (Kd): ");
    while( (c = HC06.getc()) != '\n') {
        buffer[readptr++] = c; 
    }
    Kd = atoi(buffer);
    readptr = 0;
    cleanBuffer(buffer,6);
    
    HC06.printf("\r\nStarting PID-controlling to Sp = %d with Kp = %d / Ki = %d / Kd = %d\r\n", Sp, Kp, Ki, Kd);
    
    while(1){
        if (HC06.readable()) { // Wait for any key sent to get new Sp value
            HC06.getc(); // Erase first key from buffer (dummy signal-key typed to indicate the following new Sp-input)
            
            HC06.printf("\r\n\r\n\r\nEnter a new SetPoint value (Sp): ");
            while( (c = HC06.getc()) != '\n') {
                buffer[readptr++] = c;
            }
            Sp = atoi(buffer);
            readptr = 0;
            cleanBuffer(buffer,6);
            
            HC06.printf("\r\nSuccessfully changed Setpoint-Value to Sp = %d (= %0.2f V)\r\n\r\n", Sp, (Sp*REFERENCEVOLTAGE/999));
        }
        
        PID(Sp, Kp, Ki, Kd);
    }
}