
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 8:2ba338b4590e, committed 2019-03-07
- Comitter:
- la00noix
- Date:
- Thu Mar 07 02:54:55 2019 +0000
- Parent:
- 7:44ce34007499
- Child:
- 9:63924280272d
- Commit message:
- a
Changed in this revision
--- 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;