hirromu takaki / Mbed OS 06_MD_practice

quad

Dependencies:   haseMD lpf

main.cpp

Committer:
THtakahiro702286
Date:
2020-01-25
Revision:
0:2762f6b27c5f

File content as of revision 0:2762f6b27c5f:

#include "mbed.h"
#include "haseMD.h"
#include "lpf.h"

HaseMD md[4] = { HaseMD(PA_6),HaseMD(PA_7), HaseMD(PA_9),HaseMD(PB_10) };
//DigitalIn DS[3] = { DigitalIn(PC_12), DigitalIn(PC_10), DigitalIn(PD_2) };
Timer tim;
Serial pc(USBTX,USBRX,115200);
lpf mdlpf(1.0/300,0.005);
//DigitalIn fallut(PB_6);
//DigitalIn otw(PC_7);

int main()
{
    tim.start();
    double pwm = 0.0,put;
    
    while (true) {
        
        pwm = sin(tim/3.0);
        for(int i=0; i < 4; i++)
        {
            md[i].setSpeed(pwm);
            
        }
        mdlpf.path_value(pwm);
//        pwm += ((pwm < 0.1) - (pwm > 0.9)) * 0.1;
//        put = 0.5 + pwm / 2.0;
        put = pwm * 0.9 / 2 + 0.5;
        pc.printf("%f \n\r",put);
    }
}