Revision 10:53a92f2ab1c4, committed 2019-02-21

Comitter:

Ryosei

Date:

Thu Feb 21 10:06:09 2019 +0000

Parent:

9:6901b1dfa688

Commit message:

unkounko

Changed in this revision

--- a/Input/Switch/Switch.cpp	Mon Feb 18 05:40:03 2019 +0000
+++ b/Input/Switch/Switch.cpp	Thu Feb 21 10:06:09 2019 +0000
@@ -2,12 +2,12 @@
 #include <stdint.h>
 
 DigitalIn limitSw[] = {
-    DigitalIn(LIMITSW2_PIN),
-    DigitalIn(LIMITSW3_PIN),
+    DigitalIn(LIMITSW0_PIN),
+    DigitalIn(LIMITSW1_PIN),
+//    DigitalIn(LIMITSW2_PIN),
+//    DigitalIn(LIMITSW3_PIN),
 };
 
-
-
 namespace SWITCH
 {
     void Switch::Initialize(void) {
@@ -21,3 +21,4 @@
 
 
 
+

--- a/Input/Switch/Switch.h	Mon Feb 18 05:40:03 2019 +0000
+++ b/Input/Switch/Switch.h	Thu Feb 21 10:06:09 2019 +0000
@@ -4,16 +4,15 @@
 #include "mbed.h"
 
 extern DigitalIn limitSw[];
-extern DigitalOut AIR[];
 
 namespace SWITCH
 {
-    #define AIR1_PIN A7
-    #define AIR2_PIN A6
-    #define LIMITSW2_PIN A5
-    #define LIMITSW3_PIN A4
+    #define LIMITSW0_PIN A7
+    #define LIMITSW1_PIN A6
+//    #define LIMITSW2_PIN A5
+//    #define LIMITSW3_PIN A4
 
-    #define MOUNTING_LIMITSW_NUM 4
+    #define MOUNTING_LIMITSW_NUM 2
 
     class Switch
     {
@@ -21,11 +20,6 @@
             static void Initialize(void);
             static bool checkPushed(int mySwitch);
     };
-
 }
 
-
-        
-
-
-#endif
+#endif
\ No newline at end of file

--- a/System/Process/Process.cpp	Mon Feb 18 05:40:03 2019 +0000
+++ b/System/Process/Process.cpp	Thu Feb 21 10:06:09 2019 +0000
@@ -26,11 +26,88 @@
 #define TIRE_1 0
 #define TIRE_2 1
 #define TIRE_3 2
-#define ARM 3
-#define AIR_1 0
-#define AIR_2 1
-#define ARM_roll 0
+#define ARM    3
+
+#define ARMlim 0
+
+int Air0=0;
+int Air1=1;
+
+#define a 0
+#define b 1
+#define c 2
+
+int a_array[15][15] = {
+    {   -30,   -20,   -10,     0,     0,     0,   0,   0,   0,    0,    0,    0,   10,   20,   30 },
+    {   -40,   -30,   -20,   -10,     0,     0,   0,   0,   0,    0,    0,   10,   20,   30,   40  },
+    {   -50,   -40,   -30,   -20,   -10,     0,   0,   0,   0,    0,   10,   20,   30,   40,   50  },
+    {   -60,   -50,   -40,   -30,   -20,   -10,   0,   0,   0,   10,   20,   30,   40,   50,   60  },
+    {   -70,   -60,   -50,   -40,   -30,   -20, -10,   0,  10,   20,   30,   40,   50,   60,   70  },
+    {   -80,   -70,   -60,   -50,   -40,   -30, -20,   0,  20,   30,   40,   50,   60,   70,   80  },
+    {   -90,   -80,   -70,   -60,   -50,   -40, -30,   0,  30,   40,   50,   60,   70,   80,   90  },
+    {   -100,   -90,   -80,   -70,   -60,   -50, -40,   0,  40,   50,   60,   70,   80,   90,  100  },
+    {   -90,   -80,   -70,   -60,   -50,   -40, -30,   0,  30,   40,   50,   60,   70,   80,   90  },
+    {   -80,   -70,   -60,   -50,   -40,   -30, -20,   0,  20,   30,   40,   50,   60,   70,   80  },
+    {   -70,   -60,   -50,   -40,   -30,   -20, -10,   0,  10,   20,   30,   40,   50,   60,   70  },
+    {   -60,   -50,   -40,   -30,   -20,   -10,   0,   0,   0,   10,   20,   30,   40,   50,   60  },
+    {   -50,   -40,   -30,   -20,   -10,     0,   0,   0,   0,    0,   10,   20,   30,   40,   50  },
+    {   -40,   -30,   -20,   -10,     0,     0,   0,   0,   0,    0,    0,   10,   20,   30,   40  },
+    {   -30,   -20,   -10,     0,     0,     0,   0,   0,   0,    0,    0,    0,   10,   20,   30  }
+};
 
+int b_array[15][15] = {
+    {  100,   92,   84,   76,   68,   60,   52,  100,   36,   28,   20,   12,   4,    0,   0   },
+    {   92,   84,   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0   },
+    {   84,   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,   -4  },
+    {   76,   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12 },
+    {   68,   60,   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,   -4,   -12,   -20 },
+    {   60,   52,   44,   36,   28,   20,   12,    4,    0,   0,   0,   -4,   -12,   -20,   -28 },
+    {   52,   44,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36 },
+    {  100,   36,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44 },
+    {   85,   28,   20,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52 },
+    {   70,   20,   12,    4,   0,   0,   0,   -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60 },
+    {   55,   12,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68 },
+    {   40,    4,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76 },
+    {   25,   0,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76,   -84 },
+    {   10,   0,   0,    -4,   -12,   -20,   -28,   -36,   -44,   -52,   -60,   -68,   -76,   -84,   -92 },
+    {    0,   0,   -4,   -12,   -20,   -28,  -36,   -100,   -52,   -60,   -68,   -76,   -84,   -92,  -100 }
+};
+
+
+int c_array[15][15] = {
+    {   0,   0,    4,    12,   20,   28,   36,  100,   52,   60,   68,   76,   84,   92,  100  },
+    {   0,   0,    0,     4,   12,   20,   28,   36,   44,   52,   60,   68,   76,   84,   92  },
+    {   -4,   0,    0,     0,    4,   12,   20,   28,   36,   44,   52,   60,   68,   76,   84  },
+    {   -12,   -4,    0,     0,   0,    4,   12,   20,   28,   36,   44,   52,   60,   68,   76  },
+    {   -20,   -12,   -4,     0,   0,   0,    4,   12,   20,   28,   36,   44,   52,   60,   68  },
+    {   -28,   -20,   -12,    -4,   0,   0,    0,    4,   12,   20,   28,   36,   44,   52,   60  },
+    {   -36,   -28,   -20,   -12,    -4,   0,    0,   0,    4,   12,   20,   28,   36,   44,   52  },
+    {   -44,   -36,   -28,   -20,   -12,    -4,    0,   0,    0,   4,   12,   20,   28,   36,   44  },
+    {   -52,   -44,   -36,   -28,   -20,   -12,     4,   0,    0,   0,   4,   12,    20,   28,   36  },
+    {   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,    0,   0,   0,   4,   12,   20,   28  },
+    {   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,   0,   4,   12,   20  },
+    {   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,   0,   4,   12  },
+    {   -84,   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,   -4,   0,   0,   0,    4  },
+    {   -92,   -84,   -76,   -68,   -60,   -52,   -44,   -36,   -28,   -20,   -12,    -4,   0,   0,    0  },
+    {  -100,   -92,   -84,   -76,   -76,   -60,  -100,  -100,   -36,   -28,   -20,   -12,  -4,   0,    0  }
+};
+
+int pwm_array[15] = {   30,   25,   20,   15,   10,   5,   0,   0,   0,   -5,   -10,   -15,   -20,   -25,   -30 };
+
+#define usiro 0
+#define mae 0
+uint8_t motorData[5];
+uint8_t pwmData[5];
+
+int conlx;
+int conly;
+int conrx;
+int conry;
+int conba;
+int conbb;
+int conbx;
+int conby;
+int homes=0;
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
 //_____________________
 
@@ -41,90 +118,150 @@
 Serial pc(SERIAL_TX,SERIAL_RX);
 XBEE::ControllerData *controller;
 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
-DigitalOut Air[]={
-    DigitalOut(A7),
-    DigitalOut(A6),
-
+//////////////////////////////////
+//関数、タイマーの宣言
+DigitalOut Air[]= {
+    DigitalOut(A5),
+    DigitalOut(A4),
 };
 Timer Airtime;
-void AirOut(int pin,int mode){
+Timer home;
+void AirOut(int pin,int mode)
+{
     Air[pin]=mode;
 }
+//
+//////////////////////////////////
 using namespace SWITCH;
 
-void SystemProcess(void) {  
+void SystemProcess(void)
+{
     while(true) {
         controller = XBEE::Controller::GetData();
 //____________________________
-/*------------------------ HOW TO WRITE ------------------------/
+        /*------------------------ HOW TO WRITE ------------------------/
+
+            ここにメインのプログラムを書く
+
+            ・コントローラから受け取ったデータをもとに動作のプログラムを書く
+             (コントローラのデータは controller-> で取る)
 
-    ここにメインのプログラムを書く
+                if(controller->Button.RIGHT) {
+                    motor[TIRE_L].dir = FOR;
+                    motor[TIRE_R].dir = BACK;
+                    motor[TIRE_L].pwm = 12.3;
+                    motor[TIRE_R].pwm = 12.3;
+                }
+
+             motor[0].dirは     FOR   (正転)
+                                BACK  (逆転)
+                                BRAKE (ブレーキ)
+                                FREE  (フリー)
+
+             motor[0].pwmは     0.0(%) ~ 100.0(%)
+
+             controllerは       XBee.hの構造体の中身
+
+             (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
+
+            ・リミットスイッチの値をもとに動作のプログラムを書く
 
-    ・コントローラから受け取ったデータをもとに動作のプログラムを書く
-     (コントローラのデータは controller-> で取る)
+                if(Switch::CheckPushed(ARM_L))
+                {
+                    if(controller->Button.L)
+                    {
+                        motor[ARM].dir = FOR;
+                        motor[ARM].pwm = 80.0;
+                    }
+                    if(motor[ARM].dir == BACK)
+                    {
+                        motor[ARM].dir = BRAKE;
+                    }
+                }
+
+             →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
 
-        if(controller->Button.RIGHT) {
-            motor[TIRE_L].dir = FOR;
-            motor[TIRE_R].dir = BACK;
-            motor[TIRE_L].pwm = 12.3;
-            motor[TIRE_R].pwm = 12.3;
+            ・他にもやりたいことがあったら自由にどうぞ
+
+            ps.わからないことがあったら聞いてください
+
+        /------------------------ HOW TO WRITE ------------------------*/
+//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
+        
+        float precision; //精密モード変数
+        float s=Airtime.read();//エアータイム
+        float hometime=home.read();//ホームタイム
+        float goaisatsu_time=0.2;
+        //////////////////////////////////////////////////////////////////////////////////
+        //ご挨拶モード
+        
+        if(controller->Button.HOME){
+            home.start();
+            homes=1;
         }
 
-     motor[0].dirは     FOR   (正転)
-                        BACK  (逆転)
-                        BRAKE (ブレーキ)
-                        FREE  (フリー)
-
-     motor[0].pwmは     0.0(%) ~ 100.0(%)
-
-     controllerは       XBee.hの構造体の中身
-
-     (AnalogL・Rを使いたかったら、頑張って考える or 聞いてください)
-
-    ・リミットスイッチの値をもとに動作のプログラムを書く
+        if((hometime<=goaisatsu_time)&&(!(homes==0))){
+            homes=2;
+        }else if(hometime<=2*goaisatsu_time){
+            homes=1;
+        }else if(hometime<=3*goaisatsu_time){
+            homes=2;
+        }else if(hometime<4*goaisatsu_time){
+            homes=1;
+        }else if(hometime<5*goaisatsu_time){
+            homes=0;
+            home.stop();
+            home.reset();
+        }else{
+            homes=0;
+            home.stop();
+            home.reset();
+        }
+        
+        //ご挨拶モード
+        ////////////////////////////////////////////////////////////////////////////////////
 
-        if(Switch::CheckPushed(ARM_L))
-        {
-            if(controller->Button.L)
-            {
-                motor[ARM].dir = FOR;
-                motor[ARM].pwm = 80.0;
-            }
-            if(motor[ARM].dir == BACK)
-            {
-                motor[ARM].dir = BRAKE;
-            }
-        }
-
-     →関数 Switch::CheckPushed の引数はリミットスイッチの名前 (limitSw[0]みたいな), 返り値はbool型 (true or false)
-
-    ・他にもやりたいことがあったら自由にどうぞ
-
-    ps.わからないことがあったら聞いてください
-
-/------------------------ HOW TO WRITE ------------------------*/
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓
-        float precision;
-        float s=Airtime.read();
-        pc.printf("time= %d",Airtime.read());
-        if(controller->Button.ZR){
+        ////////////////////////////////////////////////////////////////////////////////////
+        //精密モード
+        if(controller->Button.ZR) {
             precision=0.5;
-        }else{
+        } else {
             precision=1.0;
         }
+        //精密モード
+        ////////////////////////////////////////////////////////////////////////////////////
+
+        ////////////////////////////////////////////////////////////////////////////////////
+        //アーム機構
+        //motor[ARM].pwm=100;
         
         if(controller->Button.X){
-            motor[ARM].dir=FOR;
-            motor[ARM].pwm=80;
-        }else if((controller->Button.Y)&&(Switch::checkPushed(ARM_roll))){
-            motor[ARM].dir=BACK;
-            motor[ARM].pwm=80;
+            if(Switch::checkPushed(ARMlim)){
+                motor[ARM].dir = BRAKE;
+                motor[ARM].pwm = 100;
+            }else{
+                motor[ARM].dir = BACK;
+                motor[ARM].pwm = 100;
+            }
+        }else if(controller->Button.Y){
+            motor[ARM].dir = FOR;
+            motor[ARM].pwm = 100;
         }else{
-            motor[ARM].dir=BRAKE;
-            motor[ARM].pwm=100;
+            motor[ARM].dir = BRAKE;
+            motor[ARM].pwm = 100;
         }
         
         
+        
+        
+        
+      
+        //アーム機構
+        ////////////////////////////////////////////////////////////////////////////////////
+
+        ////////////////////////////////////////////////////////////////////////////////////
+        //十字キー足回り（宮川ver）
+        
         if(controller->Button.L) {
             motor[TIRE_1].dir = FOR;
             motor[TIRE_2].dir = FOR;
@@ -132,70 +269,85 @@
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100*precision;
             motor[TIRE_3].pwm = 100*precision;
-        }else if(controller->Button.R){
+        } else if(controller->Button.R) {
             motor[TIRE_1].dir = BACK;
             motor[TIRE_2].dir = BACK;
             motor[TIRE_3].dir = BACK;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100*precision;
             motor[TIRE_3].pwm = 100*precision;
-        }else if(controller->Button.RIGHT) {
+        } else if(controller->Button.RIGHT) {
             motor[TIRE_1].dir = BACK;
             motor[TIRE_2].dir = FOR;
             motor[TIRE_3].dir = FOR;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 60*precision;
             motor[TIRE_3].pwm = 60*precision;
-        }else if(controller->Button.LEFT){
+        } else if(controller->Button.LEFT) {
             motor[TIRE_1].dir = FOR;
             motor[TIRE_2].dir = BACK;
             motor[TIRE_3].dir = BACK;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 60*precision;
             motor[TIRE_3].pwm = 60*precision;
-        }else if(controller->Button.DOWN){
+        } else if(controller->Button.DOWN) {
             motor[TIRE_1].dir = BRAKE;
             motor[TIRE_2].dir = FOR;
             motor[TIRE_3].dir = BACK;
-                        motor[TIRE_1].pwm = 100;
+            motor[TIRE_1].pwm = 100;
             motor[TIRE_2].pwm = 100*precision;
             motor[TIRE_3].pwm = 100*precision;
-        }else if(controller->Button.UP){
+        } else if(controller->Button.UP) {
             motor[TIRE_1].dir = BRAKE;
             motor[TIRE_2].dir = BACK;
             motor[TIRE_3].dir = FOR;
             motor[TIRE_1].pwm = 100;
             motor[TIRE_2].pwm = 100*precision;
             motor[TIRE_3].pwm = 100*precision;
-        }else if((controller->Button.UP) && (controller->Button.RIGHT)){
+        } else if((controller->Button.UP) && (controller->Button.RIGHT)) {
             motor[TIRE_1].dir = BACK;
             motor[TIRE_2].dir = BRAKE;
             motor[TIRE_3].dir = FOR;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100;
-            motor[TIRE_3].pwm = 100*precision;          
-        }else if((controller->Button.UP) && (controller->Button.LEFT)){
+            motor[TIRE_3].pwm = 100*precision;
+        } else if((controller->Button.UP) && (controller->Button.LEFT)) {
             motor[TIRE_1].dir = FOR;
             motor[TIRE_2].dir = BACK;
             motor[TIRE_3].dir = BRAKE;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100*precision;
-            motor[TIRE_3].pwm = 100; 
-        }else if((controller->Button.DOWN) && (controller->Button.RIGHT)){
+            motor[TIRE_3].pwm = 100;
+        } else if((controller->Button.DOWN) && (controller->Button.RIGHT)) {
             motor[TIRE_1].dir = BACK;
             motor[TIRE_2].dir = FOR;
             motor[TIRE_3].dir = BRAKE;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100*precision;
-            motor[TIRE_3].pwm = 100; 
-        }else if((controller->Button.DOWN) && (controller->Button.LEFT)){
+            motor[TIRE_3].pwm = 100;
+        } else if((controller->Button.DOWN) && (controller->Button.LEFT)) {
             motor[TIRE_1].dir = FOR;
             motor[TIRE_2].dir = BRAKE;
             motor[TIRE_3].dir = BACK;
             motor[TIRE_1].pwm = 100*precision;
             motor[TIRE_2].pwm = 100;
-            motor[TIRE_3].pwm = 100*precision;   
-        }else{
+            motor[TIRE_3].pwm = 100*precision;
+        } else if(homes==1) {
+            motor[TIRE_1].dir = FOR;
+            motor[TIRE_2].dir = FOR;
+            motor[TIRE_3].dir = FOR;
+            motor[TIRE_1].pwm = 70*precision;
+            motor[TIRE_2].pwm = 70*precision;
+            motor[TIRE_3].pwm = 70*precision;
+        } else if(homes==2) {
+            motor[TIRE_1].dir = BACK;
+            motor[TIRE_2].dir = BACK;
+            motor[TIRE_3].dir = BACK;
+            motor[TIRE_1].pwm = 70*precision;
+            motor[TIRE_2].pwm = 70*precision;
+            motor[TIRE_3].pwm = 70*precision;
+        }
+         else {
             motor[TIRE_1].dir=BRAKE;
             motor[TIRE_2].dir=BRAKE;
             motor[TIRE_3].dir=BRAKE;
@@ -203,26 +355,152 @@
             motor[TIRE_2].pwm=100;
             motor[TIRE_3].pwm=100;
         }
-        
-        if(controller->Button.A){
-            AirOut(AIR_1,1);
-        }else if(controller->Button.B){
-            AirOut(AIR_1,0);
+        ///////////////////////////////////
+
+        if(controller->Button.A) {
+            AirOut(Air0,1);
+        } else if(controller->Button.B) {
+            AirOut(Air0,0);
         }
-        
-        if(controller->Button.ZL){
+
+        if(controller->Button.ZL) {
             Airtime.start();
-            AirOut(AIR_2,0);
+            AirOut(Air1,0);
         }
-        if(s>1){
+        if(s>1) {
             Airtime.stop();
             Airtime.reset();
-            AirOut(AIR_2,1);
+            AirOut(Air1,1);
         }
 
 
+        /////////////////////////////////////////////////////////////////////////////////////
+        //アナログスティック足回り（新村ver）
+        
+        conlx = controller->AnalogL.X;
+        conly = controller->AnalogL.Y;
+        conrx = controller->AnalogR.X;
+        conry = controller->AnalogR.Y;
+        if((!(controller->Button.RIGHT))&&(!(controller->Button.DOWN))&&
+                (!(controller->Button.LEFT))&&(!(controller->Button.UP))&&
+                (!(controller->Button.L))&&(!(controller->Button.R))&&
+                (!(controller->Button.HOME))) {
+            if(((conlx >6) && (conly <8)) ||((conrx >6)&&(conry <8))) {
+                motor[a].dir = BRAKE;
+                motor[b].dir = BRAKE;
+                motor[c].dir = BRAKE;
+                motor[a].pwm = 100.0;
+                motor[b].pwm = 100.0;
+                motor[c].pwm = 100.0;
+            }
+
+            if(a_array[conly][conlx]<0) {
+                motor[a].pwm = -1*a_array[conly][conlx]*precision;
+                motor[a].dir = FOR;
+            } else if(a_array[conly][conlx] > 0) {
+                motor[a].pwm = a_array[conly][conlx]*precision;
+                motor[a].dir = BACK;
+            } else if(a_array[conly][conlx] == 0) {
+                if(conry < 6) {
+                    motor[a].dir = BACK;
+                    motor[b].dir = BACK;
+                    motor[c].dir = BACK;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else if(conry > 8) {
+                    motor[a].dir = FOR;
+                    motor[b].dir = FOR;
+                    motor[c].dir = FOR;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else {
+                    motor[a].dir = BRAKE;
+                    motor[b].dir = BRAKE;
+                    motor[c].dir = BRAKE;
+                    motor[a].pwm = 100.0;
+                    motor[b].pwm = 100.0;
+                    motor[c].pwm = 100.0;
+                }
+            }
+
+            if(b_array[conly][conlx] <0 ) {
+                motor[b].pwm = -1*b_array[conly][conlx]*precision;
+                motor[b].dir = FOR;
+            } else if(b_array[conly][conlx] > 0) {
+                motor[b].pwm = b_array[conly][conlx]*precision;
+                motor[b].dir = BACK;
+            } else if(a_array[conly][conlx] == 0 ) {
+                if(conrx < 6 ) {
+                    motor[a].dir = BACK;
+                    motor[b].dir = BACK;
+                    motor[c].dir = BACK;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else if(conrx > 8) {
+                    motor[a].dir = FOR*precision;
+                    motor[b].dir = FOR*precision;
+                    motor[c].dir = FOR*precision;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else {
+                    motor[a].dir = BRAKE;
+                    motor[b].dir = BRAKE;
+                    motor[c].dir = BRAKE;
+                    motor[a].pwm = 100.0;
+                    motor[b].pwm = 100.0;
+                    motor[c].pwm = 100.0;
+                }
+            }
+
+
+            if(c_array[conly][conlx] <0 ) {
+                motor[c].pwm =-1*c_array[conly][conlx]*precision;
+                motor[c].dir = BACK;
+            } else if(c_array[conly][conlx] > 0) {
+                motor[c].pwm = c_array[conly][conlx]*precision;
+                motor[c].dir = FOR;
+            } else if(a_array[conly][conlx] == 0 ) {
+                if(conrx < 6 ) {
+                    motor[a].dir = BACK;
+                    motor[b].dir = BACK;
+                    motor[c].dir = BACK;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else if(conrx > 8) {
+                    motor[a].dir = FOR;
+                    motor[b].dir = FOR;
+                    motor[c].dir = FOR;
+                    motor[a].pwm = 100.0*precision;
+                    motor[b].pwm = 100.0*precision;
+                    motor[c].pwm = 100.0*precision;
+                } else {
+                    motor[a].dir = BRAKE;
+                    motor[b].dir = BRAKE;
+                    motor[c].dir = BRAKE;
+                    motor[a].pwm = 100.0;
+                    motor[b].pwm = 100.0;
+                    motor[c].pwm = 100.0;
+                }
+                //アナログスティック足回り（新村ver）
+                /////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+            }
+            }
+        
+
+
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑
-//____________________________
+
 
         MOTOR::Motor::Update(motor);
     }