3/6

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

Files at this revision

API Documentation at this revision

Comitter:
yuki0701
Date:
Wed Mar 06 03:51:08 2019 +0000
Parent:
8:4bdfac5b52e1
Commit message:
a

Changed in this revision

can/can.cpp 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	Tue Mar 05 04:29:49 2019 +0000
+++ b/can/can.cpp	Wed Mar 06 03:51:08 2019 +0000
@@ -11,17 +11,16 @@
 DigitalOut cansend_led(LED1);  //canread -> on
 DigitalOut canread_led(LED2);  //cansend -> on
 
-char t1[1]; //動作番号(送信する値(char型))
-int t1_r, T1; //動作番号(受け取った値、送信する値(int型))
+int t1_r=0, T1=0; //動作番号(受け取った値、送信する値(int型))
 
 void can_readsend()
 {
-    t1[0] = T1;
-    printf("t1[0] = %d\n\r",t1[0]);
+    can_ashileddata[1] = T1; //動作番号(id節約のため、can_ashileddata[]と一緒に送る)
+  //printf("t1[0] = %d\n\r",t1[0]);
 
     CANMessage msg;
-
-    if(can1.write(CANMessage(9,t1,4))) {
+    
+    if(can1.write(CANMessage(4,can_ashileddata,2))) {  //IDを7にして送信
         cansend_led = 1;
     } else {
         cansend_led = 0;
@@ -58,8 +57,8 @@
            // debug_printf("usw_data4 = %f\n\r",usw_data4);
         }
 
-        if(msg.id == 8) {
-            t1_r = msg.data[0];
+        if(msg.id == 1) {
+            t1_r = msg.data[3];
             //printf("t1_r = %d\n\r",t1_r);
         }
 
@@ -67,13 +66,7 @@
         canread_led = 0;
     }
 
-    if(can1.write(CANMessage(7,can_ashileddata,1))) {  //IDを7にして送信
-        cansend_led = 1;
-    } else {
-        cansend_led = 0;
-    }
-
-    if(t1_r > T1) {
+    if(t1_r > T1){
         T1 = t1_r;
     }
 
--- a/main.cpp	Tue Mar 05 04:29:49 2019 +0000
+++ b/main.cpp	Wed Mar 06 03:51:08 2019 +0000
@@ -41,18 +41,19 @@
 #ifdef HARUROBO_TEST_MODE
     T1 = 0;
     printf("start\n\r");
-    printf("T1 = %d\n\r",T1);
+    //printf("T1 = %d\n\r",T1);
     if(T1 == 0) {
 
         //スタート位置からみかんの木まで移動
-        printf("t = 0");
+        printf("T1 = 0\n\r");
         T1++;
         //printf("t = %d\n\r",T1);
     }
 
     if(T1 == 1) {
         while(1) {
-            //printf("wait\n\r");
+            printf("T1 = 1\n\r");
+            wait(0.5);
             if(T1 == 2) {
                 break;
             }
@@ -62,13 +63,15 @@
     if(T1 == 2) {
 
         //みかんの木から三宝置き場まで移動
-        printf("t = 2");
+        printf("T1 = 2\n\r");
         T1++;
     }
 
     if(T1 == 3) {
         while(1) {
+            printf("T1 = 3\n\r");
             if(T1 == 4) {
+                
                 break;
             }
         }
@@ -118,19 +121,20 @@
                         if(go_waitmode == 0) {
 
 
-                            
+
                             //can_start();
                             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);
+                            purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
+                            gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
                             MaxonControl(0,0,0,0);
-                            set_cond(2,0,-1243,1,800);
-                            pos_correction(-1610,1250,0,1,0);
+                            set_cond(2,0,-1243,1,1076);
+                            pos_correction(-1672,1400,0,0,0);
+                            enc_correction(1,1);
                             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);
+                            gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
+                            purecurve(3,1,1,-1672,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);
@@ -140,7 +144,8 @@
                             set_cond(0,0,-2462,0,0);
                             gogo_straight(0,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);
+                            pos_correction(-2850,4500,-90,0,1);
+                            enc_correction(1,0);
                             wait(0.5);
 
                             gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
@@ -151,19 +156,20 @@
                             set_cond(2,1,-3500,1,6324);
                             gogo_straight(0,0,-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);
+                            pos_correction(-2700,6647,-90,0,0);
+                            enc_correction(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);
+                            purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
+                            gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
                             set_cond(2,1,1243,1,4000);
-                            gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+                            gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
                             MaxonControl(0,0,0,0);
-                            pos_correction(-400,4500,-180,1,1);
+                            pos_correction(-519,4500,-180,0,0);
                             MaxonControl(0,0,0,0);
-                            
+
                             go_waitmode = 1;
 
                         } else if(go_waitmode == 1) {
--- a/movement/movement.cpp	Tue Mar 05 04:29:49 2019 +0000
+++ b/movement/movement.cpp	Wed Mar 06 03:51:08 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);
@@ -172,7 +172,7 @@
 //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
 
     double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
-    double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
+    double D1=42,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
 
     now_angle=gyro.getAngle();  //ジャイロの値読み込み
 
@@ -284,6 +284,18 @@
     }*/
 }
 
+void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き
+//x_select,y_select → (0:上書きしない/1:上書きする)
+
+    if(x_select == 1){
+        info.nowX.enc = info.nowX.usw;
+    }
+    if(y_select == 1){
+        info.nowY.enc = info.nowY.usw;
+    }
+    
+}
+
 //ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
--- a/movement/movement.h	Tue Mar 05 04:29:49 2019 +0000
+++ b/movement/movement.h	Wed Mar 06 03:51:08 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;
 
@@ -25,6 +25,8 @@
 
 void calc_xy(double tgt_angle, double u, double v);
 
+void enc_correction(int x_select,int y_select);
+
 void purecurve(int type,double u, double v,          //正面を変えずに円弧or楕円を描いて曲がる
                double point_x1,double point_y1,
                double point_x2,double point_y2,