Revision 19:340fc4370258, committed 2019-10-09

Comitter:

Tom0108

Date:

Wed Oct 09 09:00:01 2019 +0000

Parent:

18:d6be6154f9aa

Child:

20:3f68738c4019

Commit message:

10/09

Changed in this revision

--- a/main.cpp	Thu Sep 26 09:56:47 2019 +0000
+++ b/main.cpp	Wed Oct 09 09:00:01 2019 +0000
@@ -58,6 +58,8 @@
 // 足回り
 int lx,ly,rx;
 
+int rod_hit=0;
+
 /*----------------------------------- main -----------------------------------*/
 int main()
 {
@@ -105,7 +107,7 @@
         else PALETTE(BLUE);
 
         //リミット
-        for(int i=0; i<7; i++) i2c.In(In_Data,i);
+        for(int i=0; i<9; i++) i2c.In(In_Data,i);
 
         lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
         ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
@@ -543,9 +545,6 @@
         }
 
         /* --------------電磁弁系-------------------- */
-        /*タオル掛け*/
-        if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
-        else i2c.Out_Set(Out_Data,2,0);
 
         /*タオルを引っ張るやつ*/
         if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
@@ -561,6 +560,12 @@
             i2c.Out_Set(Out_Data,5,1);
         } else i2c.Out_Set(Out_Data,5,0);
 
+        if(!rod_hit) PALETTE(CYAN);
+
+        /*タオル掛け*/
+        if(sbdbt.L2) i2c.Out_Set(Out_Data,2,1);
+        else i2c.Out_Set(Out_Data,2,0);
+
         //-------------- 出力 --------------//
         //第二引数には使う最大の個数
         i2c.Out(Out_Data,9);
@@ -576,6 +581,8 @@
         short_lim = (In_Data+6)->in_data;
         slide_start  =  (In_Data+3)->in_data;
         slide_stop   =  (In_Data+0)->in_data;
+        rod_hit = (In_Data+8)->in_data;
+
 
         /* ----------------ボタン系--------------- */
         //角度リセット
@@ -645,8 +652,8 @@
 //        pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw);
 //            pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
 //        pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY);
-//for(int i=0; i<7; i++)
-        pc.printf("%d %d ",grab, grab_f);
+        for(int i=0; i<9; i++)
+            pc.printf("%d ",(In_Data+i)->in_data);
         pc.printf("\n\r");
     }     // while(1)
 }       // int main()

--- a/main.h	Thu Sep 26 09:56:47 2019 +0000
+++ b/main.h	Wed Oct 09 09:00:01 2019 +0000
@@ -40,38 +40,6 @@
 i2c.Out_Set(Out_Data, s+6, color&1<<s)\
 
 /* 型定義 --------------------------------------------------------------------*/
-typedef enum {
-    test_motor,
-    test_motor2,
-    test_motor3,
-    test_motor4,
-    test_motor5,
-    test_motor6,
-    test_motor7,
-    test_motor8,
-    Motor_MAX
-} _Motor_;
-
-
-typedef enum {
-    limit,
-    limit1,
-    limit2,
-    limit3,
-    limit4,
-    limit5,
-    limit6,
-    limit7,
-    IN_MAX
-} _limit_;
-typedef enum {
-    out1,
-    out2,
-    out3,
-    out4,
-    ExTowel,
-    OUT_MAX
-} _OUT_;
 
 typedef enum {
     BLACK,
@@ -87,7 +55,7 @@
 
 } _COLOR_;
 
-MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
+MD_I2C_Data_TypeDef MD_Data[] = {
     { 8, 0, 0, 0, {0}},     //足回り
     { 9, 1, 0, 0, {0}},
     {10, 0, 0, 0, {0}},
@@ -107,6 +75,7 @@
     {3,1,0},        //        (下)
     {3,2,0},         // 腕(シーツ兼シャツ）
     {3,3,0},         // ゾーン切り替え
+    {0,0,0},         // 竿検知
 };
 
 OUT_I2C_Data_TypeDef Out_Data[] = {
@@ -120,33 +89,33 @@
     {41,1,0},        // r
     {41,2,0},        // b;
 };
-    /* 変数宣言 ------------------------------------------------------------------*/
+/* 変数宣言 ------------------------------------------------------------------*/
 
-    /* 関数宣言 ------------------------------------------------------------------*/
+/* 関数宣言 ------------------------------------------------------------------*/
 //メカナム基本動作
-    void mecanum_Move(int lx, int ly, int rx);
+void mecanum_Move(int lx, int ly, int rx);
 
 //角度補正
 //n_angle: 現在の角度
 //t_angle: 目標の角度
-    void AngleCorrection(double n_angle, double t_angle);
+void AngleCorrection(double n_angle, double t_angle);
 
-    /*クラス宣言-------------------------------------------------------------------*/
-    Y_I2C i2c(PB_9, PB_8);           // I2C
-    SBDBT sbdbt(PA_0, PA_1, 9600);   // SDBDT
-    BNO055 bno(PC_9, PA_8);
-    DigitalOut LED[]= {PA_10,PB_4,PB_5};
-    Serial pc(USBTX, USBRX, 115200);
+/*クラス宣言-------------------------------------------------------------------*/
+Y_I2C i2c(PB_9, PB_8);           // I2C
+SBDBT sbdbt(PA_0, PA_1, 9600);   // SDBDT
+BNO055 bno(PC_9, PA_8);
+DigitalOut LED[]= {PA_10,PB_4,PB_5};
+Serial pc(USBTX, USBRX, 115200);
 
 //半自動の時に使う
-    Timer tim;
+Timer tim;
 
 //角度補正の時に使う
-    Timer drift_tim;
+Timer drift_tim;
 
 //上下移動の保持に使う
-    Timer keep_tim[2];
+Timer keep_tim[2];
 
 #endif /* _MAIN_H_ */
 
-    /******************************** END OF FILE *********************************/
+/******************************** END OF FILE *********************************/