main.cpp

00001 #include "mbed.h"
00002 #include "haseMD.h"
00003 #include "lpf.h"
00004 
00005 HaseMD md[4] = { HaseMD(PA_6),HaseMD(PA_7), HaseMD(PA_9),HaseMD(PB_10) };
00006 //DigitalIn DS[3] = { DigitalIn(PC_12), DigitalIn(PC_10), DigitalIn(PD_2) };
00007 Timer tim;
00008 Serial pc(USBTX,USBRX,115200);
00009 lpf mdlpf(1.0/300,0.005);
00010 //DigitalIn fallut(PB_6);
00011 //DigitalIn otw(PC_7);
00012 
00013 int main()
00014 {
00015     tim.start();
00016     double pwm = 0.0,put;
00017     
00018     while (true) {
00019         
00020         pwm = sin(tim/3.0);
00021         for(int i=0; i < 4; i++)
00022         {
00023             md[i].setSpeed(pwm);
00024             
00025         }
00026         mdlpf.path_value(pwm);
00027 //        pwm += ((pwm < 0.1) - (pwm > 0.9)) * 0.1;
00028 //        put = 0.5 + pwm / 2.0;
00029         put = pwm * 0.9 / 2 + 0.5;
00030         pc.printf("%f \n\r",put);
00031     }
00032 }
00033