Diff: main.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "math.h"
+#include "mechanam.h"
+#include "Reset.h"
+
+Serial Twelite(D5,D4,115200);
+
+Mechanam gx_d_l(D7,D12);//1 #1/3N
+Mechanam fx_d_l(A3,D10);//2 #3/2
+Mechanam gx_d_r(D9,D3);//3 #1/1
+Mechanam fx_d_r(D2,D8);//4 #16/1
+
+Mbed_MD elev(A2,D11);
+
+DigitalOut fire(D6);
+
+char buffer[12];
+double x;
+double y;
+double x_t;
+double y_t;
+int8_t SEL;
+int8_t SEL_T;
+bool error_fa=false;
+bool check=false;
+bool Yadama=false;
+bool Yadama_check=false;
+Timer error_time;
+
+Reset_Mbed Reset_t;
+
+void receive()
+{
+    for (int i=0; i<=11; i++) {
+        buffer[i]=Twelite.getc();
+        if (error_fa==true and buffer[0]!='[')return;
+    }
+    if (buffer[0]=='[' and buffer[11]=='\0' and int16_t(buffer[9])<=1 and int16_t(buffer[10])<=1) {
+        int16_t x_get=(buffer[1]<<8)+buffer[2];
+        int16_t y_get=(buffer[3]<<8)+buffer[4];
+        int16_t turnx_get=(buffer[5]<<8)+buffer[6];
+        int16_t turny_get=(buffer[7]<<8)+buffer[8];
+        SEL=buffer[9];
+        SEL_T=buffer[10];
+        x=(double)(x_get)/1000.0;
+        y=(double)(y_get)/1000.0;
+        x_t=(double)(turnx_get)/1000.0;
+        y_t=(double)(turny_get)/1000.0;
+        check=true;
+        error_fa=false;
+        error_time.reset();
+    } else {
+        check=false;
+        error_fa=true;
+    }
+}
+
+int main()
+{
+    Twelite.attach(receive,Serial::RxIrq);
+
+    while(1) {
+        // ----------------ここから矢玉の処理----------------
+        if ( (SEL_T==1) and Yadama==false) Yadama=true;
+        if ( (SEL_T==0) and Yadama==true) Yadama_check=true;
+        while (Yadama_check==true) {
+            fx_d_r.free();
+            fx_d_l.free();
+            gx_d_r.free();
+            gx_d_l.free();
+
+            fire=(SEL==1 ? 1:0);
+
+            if (-0.10<y_t and y_t<0.10)y_t=0;
+            elev.drive(-y_t);
+
+            printf("mode:Y SEL=%d,Y=%3.2f\n\r",SEL,-y_t);
+            
+            if (error_fa==true and error_time.read()==0)error_time.start();
+            if (error_time.read()>=0.5f)Twelite.putc('e');
+            Reset_t.Reset(error_time.read(),1.0);
+            
+            if ( (SEL_T==1) and Yadama==true)Yadama=false;
+            if ( (SEL_T==0) and Yadama==false)Yadama_check=false;
+        }
+        // ----------------ここまで矢玉の処理----------------
+
+        if(error_fa==true and error_time.read()==0)error_time.start();
+        
+        // --------------ここからメカナムの処理---------------
+
+        if (check==true) {
+            elev.free();
+            fx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
+            fx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
+            gx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
+            gx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
+            printf("mode:M x=%3.2f y=%3.2f x_t=%3.2f\n\r",x,y,x_t);
+            check=false;
+            error_fa=true;
+        } else {
+            error_fa=true;
+        }
+
+        if (error_time.read()>=0.5f)Twelite.putc('e');
+        Reset_t.Reset(error_time.read(),1.0);
+        // --------------ここまでメカナムの処理---------------
+    }
+}
\ No newline at end of file