改良版位置補正プログラム動作未確認

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
la00noix
Date:
Thu Mar 07 02:54:55 2019 +0000
Parent:
7:44ce34007499
Child:
9:63924280272d
Commit message:
a

Changed in this revision

can/can.cpp Show annotated file Show diff for this revision Revisions of this file
can/can.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
movement/movement.cpp Show annotated file Show diff for this revision Revisions of this file
movement/movement.h Show annotated file Show diff for this revision Revisions of this file
--- a/can/can.cpp	Sat Mar 02 08:27:05 2019 +0000
+++ b/can/can.cpp	Thu Mar 07 02:54:55 2019 +0000
@@ -11,11 +11,14 @@
 DigitalOut cansend_led(LED1);  //canread -> on
 DigitalOut canread_led(LED2);  //cansend -> on
 
+int t1_r=0, T1=0; //動作番号(受け取った値、送信する値(int型))
+
 void can_readsend()
 {
     CANMessage msg;
 
     if(can1.read(msg)) {
+
         canread_led = 1;
 
         if(msg.id == 1) { //from main
@@ -27,9 +30,11 @@
             id1_value[4] = (msg.data[0]>>3)%4;  //state of right joystick(1~3)
             id1_value[5] = (msg.data[0]>>2)%2;  //left joystick neutral position(0 or 1)
             id1_value[6] = (msg.data[0]>>1)%2;  //decide right/left(0 or 1)
-
+            t1_r = msg.data[3];                 //value of t(0~7)
+            
             debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d [6]=%d\n\r"
                          ,id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5],id1_value[6]);
+            debug_printf("t1_r=%d T1=%d can_ashileddata[1]=%d\n\r",t1_r,T1,can_ashileddata[1]);
         }
 
         if(msg.id == 3) {
@@ -50,13 +55,20 @@
         canread_led = 0;
     }
 
-    if(can1.write(CANMessage(7,can_ashileddata,1))) {  //IDを7にして送信
+
+    can_ashileddata[1] = T1;  //動作番号(id節約のため、can_ashileddata[]と一緒に送る)
+
+    if(can1.write(CANMessage(4,can_ashileddata,2))) {  //IDを4にして送信
         cansend_led = 1;
     } else {
         cansend_led = 0;
     }
 
+    if(t1_r > T1) {
+        T1 = t1_r;
+    }
 }
+
 void can_start()
 {
 
--- a/can/can.h	Sat Mar 02 08:27:05 2019 +0000
+++ b/can/can.h	Thu Mar 07 02:54:55 2019 +0000
@@ -1,6 +1,8 @@
 #ifndef HARUROBO2019_CAN
 #define HARUROBO2019_CAN
 
+extern int T1;
+
 void can_readsend();
 
 void can_start();
--- a/main.cpp	Sat Mar 02 08:27:05 2019 +0000
+++ b/main.cpp	Thu Mar 07 02:54:55 2019 +0000
@@ -45,65 +45,59 @@
                 switch(id1_value[6]) {
                     case 0:
                         //-----right mode-------------------------------------------------------------------------------------------------------------//
+                        if(T1 == 0) {
 
+                            //スタート位置からみかんの木まで移動
+                            wait(0.5);
+                            T1++;
+
+                        }
+                        if(T1 == 1) {
+                            while(1) {
+                                wait(0.5);
+                                if(T1 == 2) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 2) {
+
+                            //みかんの木から三宝置き場まで移動
+                            T1++;
+                        }
+                        if(T1 == 3) {
+                            while(1) {
+                                wait(0.5);
+                                if(T1 == 4) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 4) {
+
+                            //三宝置き場からりんごの木まで移動
+                            wait(0.5);
+                            T1++;
+                        }
+                        if(T1 == 5) {
+                            while(1) {
+
+                                wait(0.5);
+                                if(T1 == 6) {
+                                    break;
+                                }
+                            }
+                        }
+                        if(T1 == 6) {
+
+                            //りんごの木からお供え台まで移動
+                            wait(0.5);
+                            T1++;
+                        }
                         break;
                     case 1:
                         //-----left mode--------------------------------------------------------------------------------------------------------------//
 
-                        if(go_waitmode == 0) {
-
-                            //can_start();
-                            //set_cond(2,1,-700,1,-700);
-                            gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
-                            purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
-                            purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0);
-                            gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0);
-                            MaxonControl(0,0,0,0);
-                            set_cond(2,0,-1243,1,1080);
-                            pos_correction(-1610,1250,0,1,0);
-                            wait(0.5);
-
-                            gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0);
-                            purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
-                            purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
-                            gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
-                            gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
-                            MaxonControl(0,0,0,0);
-                            pos_correction(-2962,4500,-90,1,1);
-
-                            set_cond(0,0,-2462,0,0);
-                            gogo_straight(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
-                            MaxonControl(0,0,0,0);
-                            pos_correction(-2850,4500,-90,1,1);
-                            wait(0.5);
-
-                            gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
-                            purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90);
-                            purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90);
-                            purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
-                            gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
-                            set_cond(2,1,-3500,1,6324);
-                            gogo_straight(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
-                            MaxonControl(0,0,0,0);
-                            pos_correction(-2700,6647,-90,1,1);
-                            wait(0.5);
-
-                            gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
-                            gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
-                            purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180);
-                            gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180);
-                            set_cond(2,1,1243,1,4000);
-                            gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
-                            MaxonControl(0,0,0,0);
-                            pos_correction(-400,4500,-180,1,1);
-
-                            go_waitmode = 1;
-
-                        } else if(go_waitmode == 1) {
-
-                            MaxonControl(0,0,0,0);
-
-                        }
                         break;
                 }
 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
--- a/movement/movement.cpp	Sat Mar 02 08:27:05 2019 +0000
+++ b/movement/movement.cpp	Thu Mar 07 02:54:55 2019 +0000
@@ -11,7 +11,7 @@
 
 #define PI 3.141592
 
-char can_ashileddata[1]= {0};
+char can_ashileddata[2]= {0};
 int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3;
 
 Ec EC1(p22,p21,NC,500,0.05);
--- a/movement/movement.h	Sat Mar 02 08:27:05 2019 +0000
+++ b/movement/movement.h	Thu Mar 07 02:54:55 2019 +0000
@@ -1,7 +1,7 @@
 #ifndef HARUROBO2019_MOVEMENT
 #define HARUROBO2019_MOVEMENT
 
-extern char can_ashileddata[1];
+extern char can_ashileddata[2];
 
 extern int16_t m1,m2,m3,m4;