File content as of revision 0:c4c11b0ac1b9:

#define MAPLE_MINI
#include "mbed.h"
#include "pines.h"
#include <motoresDC.h>
#include <HMC5883L.h>

HMC5883L brujula;
InterruptIn boton(PB_8);
//motores       D27   D31    D30    D26   D29    D28     
MotoresDC carro(PWM_L, CTRL1_L, CTRL2_L, PWM_R, CTRL1_R, CTRL2_R);

Timer t;
unsigned long prev_time;
unsigned long interval = 33;
double maxOut = 0.5;
double minOut = -0.5;
volatile double setpoint = 0.0;
double iTerm = 0;
double lastInput;
double kp = 1.1;
double ki = 0.01;
double kd = 0.0;

PwmOut led(PB_1);

void toggle()
{
    if(setpoint == 0.0)
    {
        setpoint = 0.5;    
    }
    else
    {
        setpoint = 0.0;
    }
    
}
// functions 
void init_p()
{
    led = 1;
    carro.detener();
    boton.fall(toggle);
    brujula.init();
    wait_ms(1000);
    led = 0;
    wait_ms(500);
    led = 1;
    t.start();
    wait_ms(2000);
    
    led = 0;
}

int main() {
    init_p();
    while(1) {
        unsigned long now = t.read_ms();
        if((now - prev_time) > interval)
        {
            prev_time = now;
            
            double input = brujula.getHeading()/ 180.0 -1.0;
            // PID
            double error = setpoint - input;
            if(abs(error) < 0.1) led = 1;
            else led = 0;
            
            iTerm += ki * error;
            if(iTerm > maxOut) iTerm = maxOut;
            else if(iTerm < minOut) iTerm = minOut;
            
            double dInput = input - lastInput;
            
            double output = kp* error + iTerm + kd * dInput;
            if(output > maxOut) output = maxOut;
            else if(output < minOut) output = minOut;
            carro.pivotar(output);   
            lastInput = input;
        }
    }
}