Diff: main.cpp

Revision:

3:f09bb38d0909

Parent:

1:81190825251c

Child:

4:d0a512e71296

--- a/main.cpp	Mon Jan 27 09:19:06 2020 +0000
+++ b/main.cpp	Thu Mar 04 10:51:22 2021 +0000
@@ -1,11 +1,17 @@
 #include "mbed.h"
+//#include <math.h>
 #include "kohiMD.h"
 #include "lpf.h"
+#include "lib.h"
 
-KohiMD md[4] = { KohiMD(PA_6),KohiMD(PA_7), KohiMD(PA_9),KohiMD(PB_10) };
+//using namespace std::chrono;
+
+Lib lib;
+//KohiMD md[4] = {KohiMD(PA_15), KohiMD(PA_6), KohiMD(PA_7), KohiMD(PB_6)};
+//DigitalOut output[4] = {DigitalOut(PB_6),DigitalOut(PC_7),DigitalOut(PB_5),DigitalOut(PA_10)};
 //DigitalIn DS[3] = { DigitalIn(PC_12), DigitalIn(PC_10), DigitalIn(PD_2) };
-Timer tim;
-Serial pc(USBTX,USBRX,115200);
+Timer t;
+//Serial pc(USBTX,USBRX,115200);
 lpf mdlpf(1.0/300,0.005);
 //DigitalIn fallut(PB_6);
 //DigitalIn otw(PC_7);
@@ -14,27 +20,48 @@
 
 int main()
 {
-    tim.start();
+    t.start();
+    int p1,p2,b1=0,b2=0,co=0;
     double pwm = 0.0,put;
-    
+    for(int i=0; i < 4; i++)
+    {
+//        output[i] = 1;
+    }
     while (true) {
         
-        pwm = sin(tim/2.0);
+//        pwm = sin(duration_cast<milliseconds>(t.elapsed_time()).count() / 1000.0);
+
+        b1 = b2-b;
+        b2 = b;
+        if(b1 == 1)
+        {
+            co++;
+        }
+        pwm = (co % 10) / 10.0 - 0.5;
+
+
+
+
         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);
+//            if(b) pwm = 0; 
+//            md[i].setSpeed(pwm);
                     }
         led = b;
-        mdlpf.path_value(pwm);
+//        mdlpf.path_value(pwm);
 //        pwm += ((pwm < 0.1) - (pwm > 0.9)) * 0.1;
         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);
+        lib.go(0.0);
+        p1 = pwm * 100;
+        p2 = put * 100;
+        printf("%d %d %d %d %d\r\n",p1,p2,(int)b,b1,b2);
+//        printf("The time taken was %llu milliseconds\n", duration_cast<milliseconds>(t.elapsed_time()).count());
     }
 }