File content as of revision 4:d0a512e71296:

#include "mbed.h"
#include "kohiMD.h"
#include "lpf.h"
#include "lib.h"
Lib lib;
//KohiMD md[4] = {KohiMD(PA_15), KohiMD(PA_6), KohiMD(PA_7), KohiMD(PB_6)};
Timer t;
lpf mdlpf(1.0/300,0.005);
//DigitalIn b(USER_BUTTON);
PwmOut led(LED1);
 
int main()
{
    t.start();
    int p1,p2,b1=0,b2=0,co=0;
    float pwm = 0.0,put;
    for(int i=0; i < 4; i++)
    while (true) {
//        b1 = b2-b;
//        b2 = b;
//        if(b1 == 1)
//        {
//            co++;
//        }
//        pwm = (co % 10) / 10.0 - 0.5;
        pwm = (float)sin(t.read());
        for(int i=0; i < 4; i++)
        {
            if(pwm > 0.5) pwm = 0.5;
            if(pwm < -0.5) pwm = -0.5;
                    }
        led = put;
        put = 0.5 + pwm / 2.0;
        if(put > 0.9) put = 0.9;
        if(put < 0.1) put = 0.1;
        lib.go(pwm);
        p1 = pwm * 100;
        p2 = put * 100;
        printf("%d %d \r\n",p1,p2);
//        printf("The time taken was %llu milliseconds\n", duration_cast<milliseconds>(t.elapsed_time()).count());
    }
}