春ロボ1班(元F3RC4班+)
3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 13:f9867c3b97b0, committed 2019-03-19
- Comitter:
- yuki0701
- Date:
- Tue Mar 19 17:03:27 2019 +0000
- Parent:
- 12:762061580168
- Commit message:
- a
Changed in this revision
| 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 |
diff -r 762061580168 -r f9867c3b97b0 main.cpp
--- a/main.cpp Tue Mar 19 14:31:24 2019 +0000
+++ b/main.cpp Tue Mar 19 17:03:27 2019 +0000
@@ -144,7 +144,7 @@
}
}
if(T1 == 6) { //りんごの木からお供え台まで移動
-
+
set_cond(1,0,0,0,6962);
gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90);
gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90);
@@ -307,9 +307,248 @@
break;
//-----manual mode--------------------------------------------------------------------------------------------------------------------//
case 2:
+ switch(id1_value[6]) {
+ case 0:
+ //-----right mode-------------------------------------------------------------------------------------------------------------//
+ //未修正
- ManualOut(250,100,500,200);
- go_waitmode = 0;
+ flag = 0;
+ UserLoopSetting_enc_right();
+ if(T1 == 0) { //スタート位置からみかんの木まで移動
+
+ gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
+ gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
+ set_cond(2,1,1050,1,1076);
+ gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
+ MaxonControl(0,0,0,0);
+ pos_correction(1672,1600,0,0,0,25);
+ enc_correction(1,1);
+ gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
+ MaxonControl(0,0,0,0);
+
+ //printf("t1 = 0\n\r");
+ //wait(1);
+ T1++;
+ }
+ if(T1 == 1) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ //calc_xy(0,0,0);
+ if(T1 == 2) {
+ // enc_correction(1,1);
+ break;
+ }
+ }
+ }
+ if(T1 == 2) { //みかんの木から三宝置き場まで移動
+
+ gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90);
+ gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90);
+ gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90);
+ MaxonControl(0,0,0,0);
+ set_cond(2,1,2462,0,6000);
+ pos_correction(2962,4837.5,90,0,0,10);
+ gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90);
+ MaxonControl(0,0,0,0);
+ enc_correction(1,1);
+
+ //printf("t1 = 2\n\r");
+ //wait(1);
+ T1++;
+ }
+ if(T1 == 3) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ //calc_xy(90,0,0);
+ if(T1 == 4) {
+ // enc_correction(1,1);
+ break;
+ }
+ }
+ }
+ if(T1 == 4) { //三宝置き場からりんごの木まで移動
+
+ gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90);
+ purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90);
+ gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90);
+ MaxonControl(0,0,0,0);
+ set_cond(1,0,0,1,6038);
+ pos_correction(2500,6610,90,1,0,8);
+ set_cond(2,0,3162,1,6038);
+ gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90);
+ MaxonControl(0,0,0,0);
+ enc_correction(1,1);
+
+ // printf("t1 = 4\n\r");
+ // wait(1);
+ T1++;
+ }
+ if(T1 == 5) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ //calc_xy(90,0,0);
+ if(T1 == 6) {
+ // enc_correction(1,1);
+ break;
+ }
+ }
+ }
+ if(T1 == 6) { //りんごの木からお供え台まで移動
+
+ set_cond(1,0,0,0,6962);
+ gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90);
+ gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90);
+ gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90);
+ enc_correction(0,1);
+ set_cond(2,1,19,0,6962); //要修正
+ purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正
+ enc_correction(1,0);
+ set_cond(0,1,19,0,0);
+ gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180);
+ gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180);
+ set_cond(2,0,1000,1,4000);
+ gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180);
+ MaxonControl(0,0,0,0);
+ pos_correction(620,4370,180,0,0,30);
+ enc_correction(1,1);
+ MaxonControl(0,0,0,0);
+
+ // printf("t1 = 6\n\r");
+ // wait(1);
+ T1++;
+ }
+ if(T1 == 7) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ //printf("ashi finished\n\r");
+ //MaxonControl(0,0,0,0);
+ if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
+ if(id1_value[6] != flag)break;
+ }
+ }
+ break;
+
+ case 1:
+ //-----left mode--------------------------------------------------------------------------------------------------------------//
+ flag = 1;
+ printf("start\n\r");
+ UserLoopSetting_enc_left();
+ if(T1 == 0) { //スタート位置からみかんの木まで移動
+
+ gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
+ gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
+ set_cond(2,0,-1050,1,1076);
+ gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
+ MaxonControl(0,0,0,0);
+ pos_correction(-1672,1600,0,0,0,25);
+ enc_correction(1,1);
+ gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
+ MaxonControl(0,0,0,0);
+
+ T1++;
+ }
+
+ if(T1 == 1) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ if(T1 == 2) {
+ break;
+ }
+ }
+ }
+ if(T1 == 2) { //みかんの木から三宝置き場まで移動
+ gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
+ purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,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,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
+ gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
+ MaxonControl(0,0,0,0);
+ set_cond(2,0,-2462,0,6000);
+ pos_correction(-2962,4837.5,-90,0,0,10);
+ gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
+ MaxonControl(0,0,0,0);
+ enc_correction(1,1);
+
+ T1++;
+ }
+ if(T1 == 3) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ if(T1 == 4) {
+ break;
+ }
+ }
+ }
+ if(T1 == 4) { //三宝置き場からりんごの木まで移動
+ gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
+ purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
+ purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
+ purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
+ gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
+ MaxonControl(0,0,0,0);
+ set_cond(1,0,0,1,6038);
+ pos_correction(-2500,6610,-90,1,0,8);
+ set_cond(2,1,-3162,1,6038);
+ gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
+ MaxonControl(0,0,0,0);
+ enc_correction(1,1);
+
+ T1++;
+ }
+ if(T1 == 5) {
+ while(1) {
+ ManualOut(250,100,500,200);
+ if(T1 == 6) {
+ break;
+ }
+ }
+ }
+ if(T1 == 6) { //りんごの木からお供え台まで移動
+ set_cond(1,0,0,0,6962);
+ gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
+ gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
+ gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
+ enc_correction(0,1);
+ set_cond(2,0,-19,0,6962);
+ purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
+ enc_correction(1,0);
+ set_cond(0,0,-19,0,0);
+ gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
+ gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
+ set_cond(2,1,-1000,1,4000);
+ gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
+ MaxonControl(0,0,0,0);
+ pos_correction(-620,4370,-180,0,0,30);
+ enc_correction(1,1);
+ MaxonControl(0,0,0,0);
+
+ T1++;
+ }
+ if(T1 == 7) {
+ while(1) {
+ //printf("ashi finished\n\r");
+ ManualOut(250,100,500,200);
+ //MaxonControl(0,0,0,0);
+
+ if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
+ if(id1_value[6] != flag)break;
+ }
+ }
+
+
+
+ break;
+ }
+ // ManualOut(250,100,500,200);
+ //go_waitmode = 0;
break;
}
diff -r 762061580168 -r f9867c3b97b0 movement/movement.cpp
--- a/movement/movement.cpp Tue Mar 19 14:31:24 2019 +0000
+++ b/movement/movement.cpp Tue Mar 19 17:03:27 2019 +0000
@@ -34,6 +34,7 @@
double xy_type,pm_typeX,pm_typeY,x_base,y_base;
int flag;
+int RL_mode;
///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言/////////////////
@@ -73,11 +74,13 @@
{
info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド)
info.nowY.enc = 3500;
+ RL_mode = 0;
}
void UserLoopSetting_enc_left()
{
info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド)
info.nowY.enc = 3500;
+ RL_mode = 1;
}
void calOmega() //角速度計算関数
@@ -124,16 +127,30 @@
can_ashileddata0_1 = 0;
}
- if(now_x > -1 && now_x < 1) {
- can_ashileddata0_2 = 1;
- } else {
- can_ashileddata0_2 = 0;
- }
+ if(RL_mode == 0) {
+ if(now_x > 3110 && now_x < 3114) {
+ can_ashileddata0_2 = 1;
+ } else {
+ can_ashileddata0_2 = 0;
+ }
- if(now_y > -1 && now_y < 1) {
- can_ashileddata0_3 = 1;
- } else {
- can_ashileddata0_3 = 0;
+ if(now_y > 3498 && now_y < 3502) {
+ can_ashileddata0_3 = 1;
+ } else {
+ can_ashileddata0_3 = 0;
+ }
+ } else if(RL_mode == 1) {
+ if(now_x > -3114 && now_x < -3110) {
+ can_ashileddata0_2 = 1;
+ } else {
+ can_ashileddata0_2 = 0;
+ }
+
+ if(now_y > 3498 && now_y < 3502) {
+ can_ashileddata0_3 = 1;
+ } else {
+ can_ashileddata0_3 = 0;
+ }
}
can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4);
@@ -272,10 +289,10 @@
//エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する
calc_xy_enc();
-
- if(usw_flag == 1){
- u = 1;
- v = 1;
+
+ if(usw_flag == 1) {
+ u = 1;
+ v = 1;
}
if(u != 1 || v != 1) {
@@ -542,7 +559,7 @@
{
//距離に比例的に補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする/初期速度が目標速度を下回らないようにする）
- double first_speed, first_speed50 = 150,last_speed = 50, Max_speed = 300;
+ double first_speed, first_speed50 = 150,last_speed = 50, Max_speed = 300;
//first_speed50:５センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限
double r; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
double out;
@@ -556,20 +573,20 @@
first_speed = (first_speed50 - last_speed) * r / 50 + last_speed;
- if(r < 20){ //目標地点までの距離が１センチ以上２センチ未満のとき
+ if(r < 20) { //目標地点までの距離が１センチ以上２センチ未満のとき
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,30,30,5,0.1,10,0.1,500,tgt_angle);
- }else if(r < 30){ ////目標地点までの距離が２センチ以上３センチ未満のとき
+ } else if(r < 30) { ////目標地点までの距離が２センチ以上３センチ未満のとき
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,40,40,5,0.1,10,0.1,500,tgt_angle);
- }else if(first_speed > Max_speed){
+ } else if(first_speed > Max_speed) {
tgt_xx = ((Max_speed - last_speed)*now_x + (first_speed - Max_speed)*tgt_x)/(first_speed-last_speed);
tgt_yy = ((Max_speed - last_speed)*now_y + (first_speed - Max_speed)*tgt_y)/(first_speed-last_speed);
gogo_straight(u,v,now_x,now_y,tgt_xx,tgt_yy,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle);
gogo_straight(u,v,tgt_xx,tgt_yy,tgt_x,tgt_y,Max_speed,last_speed,5,0.1,10,0.1,500,tgt_angle);
- }else{
+ } else {
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed,last_speed,5,0.1,10,0.1,500,tgt_angle);
}
- //gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
+ //gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
MaxonControl(0,0,0,0);