Diff: main.cpp

Revision:

1:81190825251c

Parent:

0:2762f6b27c5f

Child:

3:f09bb38d0909

--- a/main.cpp	Sat Jan 25 09:13:29 2020 +0000
+++ b/main.cpp	Mon Jan 27 09:17:57 2020 +0000
@@ -1,14 +1,16 @@
 #include "mbed.h"
-#include "haseMD.h"
+#include "kohiMD.h"
 #include "lpf.h"
 
-HaseMD md[4] = { HaseMD(PA_6),HaseMD(PA_7), HaseMD(PA_9),HaseMD(PB_10) };
+KohiMD md[4] = { KohiMD(PA_6),KohiMD(PA_7), KohiMD(PA_9),KohiMD(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);
+DigitalIn b(USER_BUTTON);
+DigitalOut led(LED1);
 
 int main()
 {
@@ -17,17 +19,22 @@
     
     while (true) {
         
-        pwm = sin(tim/3.0);
+        pwm = sin(tim/2.0);
         for(int i=0; i < 4; i++)
         {
+            if(pwm > 0.5) pwm = 0.5;
+            if(pwm < -0.5) pwm = -0.5;
+            if(b) pwm = 0; 
             md[i].setSpeed(pwm);
-            
-        }
+                    }
+        led = b;
         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);
+        put = 0.5 + pwm / 2.0;
+        if(put > 0.9) put = 0.9;
+        if(put < 0.1) put = 0.1;
+//        put = pwm * 0.9 / 2 + 0.5;
+        //pc.printf("%f \n\r",put);
     }
 }