3/6
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 9:337fe0747940, committed 2019-03-06
- Comitter:
- yuki0701
- Date:
- Wed Mar 06 03:51:08 2019 +0000
- Parent:
- 8:4bdfac5b52e1
- Commit message:
- a
Changed in this revision
diff -r 4bdfac5b52e1 -r 337fe0747940 can/can.cpp --- 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; }
diff -r 4bdfac5b52e1 -r 337fe0747940 main.cpp --- 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) {
diff -r 4bdfac5b52e1 -r 337fe0747940 movement/movement.cpp --- 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とは限らない))(反時計回りが正) //ジャイロの出力は角度だが三角関数はラジアンとして計算する
diff -r 4bdfac5b52e1 -r 337fe0747940 movement/movement.h --- 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,