Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

main.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "mbed.h"
e2011220 0:ee7e9405e1c7 2 #include "math.h"
e2011220 0:ee7e9405e1c7 3 #include "mechanam.h"
e2011220 0:ee7e9405e1c7 4 #include "Reset.h"
e2011220 0:ee7e9405e1c7 5
e2011220 0:ee7e9405e1c7 6 Serial Twelite(D5,D4,115200);
e2011220 0:ee7e9405e1c7 7
e2011220 0:ee7e9405e1c7 8 Mechanam gx_d_l(D7,D12);//1 #1/3N
e2011220 0:ee7e9405e1c7 9 Mechanam fx_d_l(A3,D10);//2 #3/2
e2011220 0:ee7e9405e1c7 10 Mechanam gx_d_r(D9,D3);//3 #1/1
e2011220 0:ee7e9405e1c7 11 Mechanam fx_d_r(D2,D8);//4 #16/1
e2011220 0:ee7e9405e1c7 12
e2011220 0:ee7e9405e1c7 13 Mbed_MD elev(A2,D11);
e2011220 0:ee7e9405e1c7 14
e2011220 0:ee7e9405e1c7 15 DigitalOut fire(D6);
e2011220 0:ee7e9405e1c7 16
e2011220 0:ee7e9405e1c7 17 char buffer[12];
e2011220 0:ee7e9405e1c7 18 double x;
e2011220 0:ee7e9405e1c7 19 double y;
e2011220 0:ee7e9405e1c7 20 double x_t;
e2011220 0:ee7e9405e1c7 21 double y_t;
e2011220 0:ee7e9405e1c7 22 int8_t SEL;
e2011220 0:ee7e9405e1c7 23 int8_t SEL_T;
e2011220 0:ee7e9405e1c7 24 bool error_fa=false;
e2011220 0:ee7e9405e1c7 25 bool check=false;
e2011220 0:ee7e9405e1c7 26 bool Yadama=false;
e2011220 0:ee7e9405e1c7 27 bool Yadama_check=false;
e2011220 0:ee7e9405e1c7 28 Timer error_time;
e2011220 0:ee7e9405e1c7 29
e2011220 0:ee7e9405e1c7 30 Reset_Mbed Reset_t;
e2011220 0:ee7e9405e1c7 31
e2011220 0:ee7e9405e1c7 32 void receive()
e2011220 0:ee7e9405e1c7 33 {
e2011220 0:ee7e9405e1c7 34 for (int i=0; i<=11; i++) {
e2011220 0:ee7e9405e1c7 35 buffer[i]=Twelite.getc();
e2011220 0:ee7e9405e1c7 36 if (error_fa==true and buffer[0]!='[')return;
e2011220 0:ee7e9405e1c7 37 }
e2011220 0:ee7e9405e1c7 38 if (buffer[0]=='[' and buffer[11]=='\0' and int16_t(buffer[9])<=1 and int16_t(buffer[10])<=1) {
e2011220 0:ee7e9405e1c7 39 int16_t x_get=(buffer[1]<<8)+buffer[2];
e2011220 0:ee7e9405e1c7 40 int16_t y_get=(buffer[3]<<8)+buffer[4];
e2011220 0:ee7e9405e1c7 41 int16_t turnx_get=(buffer[5]<<8)+buffer[6];
e2011220 0:ee7e9405e1c7 42 int16_t turny_get=(buffer[7]<<8)+buffer[8];
e2011220 0:ee7e9405e1c7 43 SEL=buffer[9];
e2011220 0:ee7e9405e1c7 44 SEL_T=buffer[10];
e2011220 0:ee7e9405e1c7 45 x=(double)(x_get)/1000.0;
e2011220 0:ee7e9405e1c7 46 y=(double)(y_get)/1000.0;
e2011220 0:ee7e9405e1c7 47 x_t=(double)(turnx_get)/1000.0;
e2011220 0:ee7e9405e1c7 48 y_t=(double)(turny_get)/1000.0;
e2011220 0:ee7e9405e1c7 49 check=true;
e2011220 0:ee7e9405e1c7 50 error_fa=false;
e2011220 0:ee7e9405e1c7 51 error_time.reset();
e2011220 0:ee7e9405e1c7 52 } else {
e2011220 0:ee7e9405e1c7 53 check=false;
e2011220 0:ee7e9405e1c7 54 error_fa=true;
e2011220 0:ee7e9405e1c7 55 }
e2011220 0:ee7e9405e1c7 56 }
e2011220 0:ee7e9405e1c7 57
e2011220 0:ee7e9405e1c7 58 int main()
e2011220 0:ee7e9405e1c7 59 {
e2011220 0:ee7e9405e1c7 60 Twelite.attach(receive,Serial::RxIrq);
e2011220 0:ee7e9405e1c7 61
e2011220 0:ee7e9405e1c7 62 while(1) {
e2011220 0:ee7e9405e1c7 63 // ----------------ここから矢玉の処理----------------
e2011220 0:ee7e9405e1c7 64 if ( (SEL_T==1) and Yadama==false) Yadama=true;
e2011220 0:ee7e9405e1c7 65 if ( (SEL_T==0) and Yadama==true) Yadama_check=true;
e2011220 0:ee7e9405e1c7 66 while (Yadama_check==true) {
e2011220 0:ee7e9405e1c7 67 fx_d_r.free();
e2011220 0:ee7e9405e1c7 68 fx_d_l.free();
e2011220 0:ee7e9405e1c7 69 gx_d_r.free();
e2011220 0:ee7e9405e1c7 70 gx_d_l.free();
e2011220 0:ee7e9405e1c7 71
e2011220 0:ee7e9405e1c7 72 fire=(SEL==1 ? 1:0);
e2011220 0:ee7e9405e1c7 73
e2011220 0:ee7e9405e1c7 74 if (-0.10<y_t and y_t<0.10)y_t=0;
e2011220 0:ee7e9405e1c7 75 elev.drive(-y_t);
e2011220 0:ee7e9405e1c7 76
e2011220 0:ee7e9405e1c7 77 printf("mode:Y SEL=%d,Y=%3.2f\n\r",SEL,-y_t);
e2011220 0:ee7e9405e1c7 78
e2011220 0:ee7e9405e1c7 79 if (error_fa==true and error_time.read()==0)error_time.start();
e2011220 0:ee7e9405e1c7 80 if (error_time.read()>=0.5f)Twelite.putc('e');
e2011220 0:ee7e9405e1c7 81 Reset_t.Reset(error_time.read(),1.0);
e2011220 0:ee7e9405e1c7 82
e2011220 0:ee7e9405e1c7 83 if ( (SEL_T==1) and Yadama==true)Yadama=false;
e2011220 0:ee7e9405e1c7 84 if ( (SEL_T==0) and Yadama==false)Yadama_check=false;
e2011220 0:ee7e9405e1c7 85 }
e2011220 0:ee7e9405e1c7 86 // ----------------ここまで矢玉の処理----------------
e2011220 0:ee7e9405e1c7 87
e2011220 0:ee7e9405e1c7 88 if(error_fa==true and error_time.read()==0)error_time.start();
e2011220 0:ee7e9405e1c7 89
e2011220 0:ee7e9405e1c7 90 // --------------ここからメカナムの処理---------------
e2011220 0:ee7e9405e1c7 91
e2011220 0:ee7e9405e1c7 92 if (check==true) {
e2011220 0:ee7e9405e1c7 93 elev.free();
e2011220 0:ee7e9405e1c7 94 fx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
e2011220 0:ee7e9405e1c7 95 fx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'f');
e2011220 0:ee7e9405e1c7 96 gx_d_l.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
e2011220 0:ee7e9405e1c7 97 gx_d_r.drive(x,y,x_t,2.8,1.5,1.0,1.0,1.0,'g');
e2011220 0:ee7e9405e1c7 98 printf("mode:M x=%3.2f y=%3.2f x_t=%3.2f\n\r",x,y,x_t);
e2011220 0:ee7e9405e1c7 99 check=false;
e2011220 0:ee7e9405e1c7 100 error_fa=true;
e2011220 0:ee7e9405e1c7 101 } else {
e2011220 0:ee7e9405e1c7 102 error_fa=true;
e2011220 0:ee7e9405e1c7 103 }
e2011220 0:ee7e9405e1c7 104
e2011220 0:ee7e9405e1c7 105 if (error_time.read()>=0.5f)Twelite.putc('e');
e2011220 0:ee7e9405e1c7 106 Reset_t.Reset(error_time.read(),1.0);
e2011220 0:ee7e9405e1c7 107 // --------------ここまでメカナムの処理---------------
e2011220 0:ee7e9405e1c7 108 }
e2011220 0:ee7e9405e1c7 109 }