
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 1:3c11e07da92a, committed 2019-02-25
- Comitter:
- yuki0701
- Date:
- Mon Feb 25 06:18:09 2019 +0000
- Parent:
- 0:c61c6e4775ca
- Child:
- 2:7cba05e70367
- Commit message:
- a;
Changed in this revision
--- a/can/can.cpp Wed Feb 13 03:02:19 2019 +0000 +++ b/can/can.cpp Mon Feb 25 06:18:09 2019 +0000 @@ -31,32 +31,32 @@ } if(msg.id == 3){ - usw_data1 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); + usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //printf("usw_data1 = %d:%d,%lf\n\r",msg.data[0],msg.data[1],usw_data1); //debug_printf("%f\n\r",usw_data1); } - if(msg.id == 4){ - usw_data2 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); + /* if(msg.id == 4){ + usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //printf("usw_data2 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data2); - } + }*/ if(msg.id == 5){ - usw_data3 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); + usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //printf("usw_data3 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data3); //debug_printf("%f\n\r",usw_data3); } - if(msg.id == 6){ - usw_data4 = 0.01 * (short)((msg.data[0]<<8) | msg.data[1]); + /*if(msg.id == 6){ + usw_data4 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //printf("usw_data4 = %d:%d,%d\n\r",msg.data[0],msg.data[1],usw_data4); - } + }*/ }else{ canread_led = 0; } - if(can1.write(CANMessage(7,ashi_data,1))) { //IDを7にして送信 + if(can1.write(CANMessage(7,ashi_data,4))) { //IDを7にして送信 cansend_led = 1; }else{ cansend_led = 0;
--- a/main.cpp Wed Feb 13 03:02:19 2019 +0000 +++ b/main.cpp Mon Feb 25 06:18:09 2019 +0000 @@ -42,6 +42,8 @@ while(1){ + id1_value[0] = 0; + switch(id1_value[0]){ //-----auto mode----------------------------------------------------------------------------------------------------------------------// @@ -51,20 +53,53 @@ #ifdef HARUROBO_TEST_MODE if(go_waitmode == 0){ - - purecurve(1,1,1, - 0,0, - 1000,1000, - 9, - 1000,5,0.1,10,0.1,500,0); - gogo_straight(1,1, - 1000,1000, - 1000,2000, - 1000,1000, - 5,0.1,10,0.1,600,0); - MaxonControl(0,0,0,0); - - go_waitmode = 1; + + can_start(); + // set_cond(2,1,-700,1,-700); + gogo_straight(1,1,-2962,3500,-2962,3000,200,1000,5,0.1,10,0.1,600,0); + purecurve(7,1,1,-2962,3000,-2317,2500,9,1000,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,1500,1000,200,5,0.1,10,0.1,600,0); + MaxonControl(0,0,0,0); + set_cond(2,0,-1243,1,1076); + pos_correction(-1672,1500,0,0,0); + wait(0.5); + + gogo_straight(1,1,-1672,1500,-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); + MaxonControl(0,0,0,0); + pos_correction(-2962,4500,90,1,1); + + 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,0,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(1,0,0,0,7000); + gogo_straight(1,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,0); + 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,-1243,6647,1000,1000,5,0.1,10,0.1,800,90); + purecurve(8,1,1,-1243,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,0,0,1,4000); + 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(-519,4500,180,0,0); + + go_waitmode = 1; }else if(go_waitmode == 1){ @@ -105,6 +140,7 @@ calc_xy(0,1,1); ashi_led(); MaxonControl(0,0,0,0); + printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y); break;
--- a/movement/movement.cpp Wed Feb 13 03:02:19 2019 +0000 +++ b/movement/movement.cpp Mon Feb 25 06:18:09 2019 +0000 @@ -63,6 +63,8 @@ ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 EC1.setDiameter_mm(48); EC2.setDiameter_mm(48); //測定輪半径//後で測定 + info.nowX.enc = -2962; //初期位置の設定 + info.nowY.enc = 3500; } void calOmega() //角速度計算関数 @@ -109,6 +111,7 @@ } if(now_x > -1 && now_x < 1){ + ashi_data[2] = 1; }else{ ashi_data[2] = 0;
--- a/movement/movement.h Wed Feb 13 03:02:19 2019 +0000 +++ b/movement/movement.h Mon Feb 25 06:18:09 2019 +0000 @@ -43,6 +43,6 @@ double r_out_max, double target_angle); -void pos_correction(); +void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v); #endif \ No newline at end of file