
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 3:8a0faa3b08c3, committed 2019-03-01
- Comitter:
- yuki0701
- Date:
- Fri Mar 01 00:48:07 2019 +0000
- Parent:
- 2:7cba05e70367
- Child:
- 4:317c53a674fa
- Commit message:
- a;
Changed in this revision
--- a/can/can.cpp Tue Feb 26 05:10:27 2019 +0000 +++ b/can/can.cpp Fri Mar 01 00:48:07 2019 +0000 @@ -14,12 +14,12 @@ void can_readsend() { CANMessage msg; - + if(can1.read(msg)) { canread_led = 1; - - if(msg.id == 1){ //from main - + + if(msg.id == 1) { //from main + id1_value[0] = msg.data[0]; //decide mode(1~3) id1_value[1] = msg.data[1]; //angle of left joystick(0~359) id1_value[2] = msg.data[2]; @@ -29,54 +29,56 @@ //debug_printf("[0]=%d [1]=%d [2]=%d [3]=%d [4]=%d [5]=%d\n\r",id1_value[0],id1_value[1],id1_value[2],id1_value[3],id1_value[4],id1_value[5]); } - - if(msg.id == 3){ + + /*if(msg.id == 3) { usw_data1 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //debug_printf("usw_data1 = %f\n\r",usw_data1); - } - - /*if(msg.id == 4){ - usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); - //debug_printf("usw_data2 = %f\n\r",usw_data2); }*/ - - /*if(msg.id == 5){ - usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); - //debug_printf("usw_data3 = %f\n\r",usw_data3); - }*/ - - /*if(msg.id == 6){ + + /* if(msg.id == 4){ + usw_data2 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); + //debug_printf("usw_data2 = %f\n\r",usw_data2); + }*/ + + /* if(msg.id == 5){ + usw_data3 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); + //debug_printf("usw_data3 = %f\n\r",usw_data3); + }*/ + + if(msg.id == 6) { usw_data4 = 0.1 * (short)((msg.data[0]<<8) | msg.data[1]); //debug_printf("usw_data4 = %f\n\r",usw_data4); - }*/ - - }else{ + } + + } else { canread_led = 0; } - + if(can1.write(CANMessage(7,ashi_data,4))) { //IDを7にして送信 cansend_led = 1; - }else{ + } else { cansend_led = 0; } + +} +void can_start() +{ + + while(1) { + + CANMessage msg; + + debug_printf("wait\n\r"); + wait(0.1); + if(can1.read(msg)) { + break; + } + } } -void can_start(){ +void UserLoopSetting_can() +{ - while(1){ - - CANMessage msg; - - debug_printf("wait\n\r"); - wait(0.1); - if(can1.read(msg)){ - break; - } - } -} - -void UserLoopSetting_can(){ - can1.frequency(1000000); can_ticker.attach(&can_readsend,0.01); //遅かったら早める } \ No newline at end of file
--- a/main.cpp Tue Feb 26 05:10:27 2019 +0000 +++ b/main.cpp Fri Mar 01 00:48:07 2019 +0000 @@ -62,40 +62,40 @@ 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); + pos_correction(-1672,1500,0,0,1); 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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);
--- a/maxonsetting/maxonsetting.cpp Tue Feb 26 05:10:27 2019 +0000 +++ b/maxonsetting/maxonsetting.cpp Fri Mar 01 00:48:07 2019 +0000 @@ -23,10 +23,10 @@ //-----mbed----------------------------------------------------------------------------// SPI spi(p5,p6,p7); -DigitalOut ss_md1(p15); //エスコンの設定 -DigitalOut ss_md2(p16); -DigitalOut ss_md3(p17); -DigitalOut ss_md4(p18); +DigitalOut ss_md1(p17); //エスコンの設定 +DigitalOut ss_md2(p18); +DigitalOut ss_md3(p15); +DigitalOut ss_md4(p16); DigitalOut md_enable(p25); //DigitalIn md_ch_enable(p10); // check enable switch is open or close
--- a/movement/movement.cpp Tue Feb 26 05:10:27 2019 +0000 +++ b/movement/movement.cpp Fri Mar 01 00:48:07 2019 +0000 @@ -13,8 +13,8 @@ char ashi_data[4]={0}; -Ec EC1(p8,p26,NC,500,0.05); -Ec EC2(p21,p22,NC,500,0.05); +Ec EC2(p26,p8,NC,500,0.05); +Ec EC1(p22,p21,NC,500,0.05); Ticker ec_ticker; //ec角速度計算用ticker R1370P gyro(p28,p27); @@ -235,12 +235,9 @@ calc_xy_enc(); - if(u != 1){ + if(u != 1 || v != 1){ calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 } - if(v != 1){ - calc_xy_usw(target_angle); - } now_x = u * info.nowX.enc + (1-u) * info.nowX.usw; now_y = v * info.nowY.enc + (1-v) * info.nowY.usw; @@ -450,7 +447,7 @@ calc_xy(tgt_angle, u, v); while(1){ //機体の位置を目標領域(目標座標+許容誤差)に収める - gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,100,0,5,0.1,10,0.1,500,tgt_angle); + gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,100,5,0.1,10,0.1,500,tgt_angle); MaxonControl(0,0,0,0); calc_xy(tgt_angle, u, v); @@ -475,7 +472,7 @@ MaxonControl(out,out,out,out); } - if(tgt_angle - 2 < now_angle && now_angle < tgt_angle + 2) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 } MaxonControl(0,0,0,0);