春ロボ1班(元F3RC4班+)
/
harurobo_ashi_3_13_13h_24m
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Revision 10:c741191360de, committed 2019-03-13
- Comitter:
- yuki0701
- Date:
- Wed Mar 13 04:23:18 2019 +0000
- Parent:
- 9:7667dcfc7ce5
- Commit message:
- a
Changed in this revision
--- a/main.cpp Wed Mar 13 03:24:49 2019 +0000
+++ b/main.cpp Wed Mar 13 04:23:18 2019 +0000
@@ -46,6 +46,101 @@
case 0:
//-----right mode-------------------------------------------------------------------------------------------------------------//
flag = 0;
+ UserLoopSetting_enc_right();
+ if(T1 == 0) { //スタート位置からみかんの木まで移動
+ gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0);
+ purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0);
+ purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0);
+ set_cond(2,1,1050,1,1076);
+ gogo_straight(0,0,1672,2000,1672,1400,1000,200,5,0.1,10,0.1,600,0);
+ MaxonControl(0,0,0,0);
+ pos_correction(1672,1400,0,0,0);
+ enc_correction(1,1);
+ wait(0.5);
+ T1++;
+ }
+ if(T1 == 1) {
+ while(1) {
+ wait(0.1);
+ if(T1 == 2) {
+ break;
+ }
+ }
+ }
+ if(T1 == 2) { //みかんの木から三宝置き場まで移動
+ gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0);
+ purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0);
+ purecurve(1,1,1,2317,2500,2962,3000,9,1000,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);
+ set_cond(2,1,2462,0,6000);
+ gogo_straight(0,0,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,0);
+ enc_correction(1,1);
+ wait(0.5);
+ T1++;
+ }
+ if(T1 == 3) {
+ while(1) {
+ wait(0.1);
+ if(T1 == 4) {
+ break;
+ }
+ }
+ }
+ if(T1 == 4) { //三宝置き場からりんごの木まで移動
+ gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90);
+ purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90);
+ purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90);
+ purecurve(2,1,1,1700,6000,2257,6550,9,1000,5,0.1,10,0.1,800,90);
+ gogo_straight(1,1,2257,6550,2500,6550,1000,1000,5,0.1,10,0.1,800,90);
+ set_cond(2,0,3500,1,6050);
+ gogo_straight(0,0,2500,6550,2700,6600,1000,200,5,0.1,10,0.1,800,90);
+ MaxonControl(0,0,0,0);
+ pos_correction(2700,6600,90,0,0);
+ enc_correction(1,1);
+ wait(0.5);
+ T1++;
+ }
+ if(T1 == 5) {
+ while(1) {
+ wait(0.1);
+ if(T1 == 6) {
+ break;
+ }
+ }
+ }
+ if(T1 == 6) { //りんごの木からお供え台まで移動
+ gogo_straight(1,1,2700,6600,2500,6550,200,1000,5,0.1,10,0.1,800,90);
+ gogo_straight(1,1,2500,6550,1300,6550,1000,1000,5,0.1,10,0.1,800,90);
+ gogo_straight(1,1,1300,6550,1000,6500,1000,1000,5,0.1,10,0.1,800,90);
+ purecurve(5,1,1,1000,6550,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,1000,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);
+ enc_correction(1,1);
+ MaxonControl(0,0,0,0);
+ T1++;
+ }
+ if(T1 == 7) {
+ while(1) {
+ 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;
+ UserLoopSetting_enc_left();
if(T1 == 0) { //スタート位置からみかんの木まで移動
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);
@@ -134,10 +229,6 @@
if(id1_value[6] != flag)break;
}
}
- break;
- case 1:
- //-----left mode--------------------------------------------------------------------------------------------------------------//
- flag = 1;
break;
}
--- a/movement/movement.cpp Wed Mar 13 03:24:49 2019 +0000
+++ b/movement/movement.cpp Wed Mar 13 04:23:18 2019 +0000
@@ -68,7 +68,15 @@
ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算
EC1.setDiameter_mm(25.5);
EC2.setDiameter_mm(25.5); //測定輪半径//後で測定
- info.nowX.enc = -2962; //初期位置の設定
+}
+void UserLoopSetting_enc_right()
+{
+ info.nowX.enc = 2962; //エンコーダの初期位置の設定(右側フィールド)
+ info.nowY.enc = 3500;
+}
+void UserLoopSetting_enc_left()
+{
+ info.nowX.enc = -2962; //エンコーダの初期位置の設定(左側フィールド)
info.nowY.enc = 3500;
}
@@ -528,30 +536,36 @@
void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる)
{
-//距離に比例させて補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする）
+//距離に比例的に補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする/初期速度が目標速度を下回らないようにする）
- double first_speed, first_speed50 = 150, last_speed = 25, Max_speed = 500;
+ double first_speed, first_speed50 = 100,last_speed = 50, Max_speed = 300;
+ //first_speed50:５センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限
double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
double out;
+ double tgt_xx, tgt_yy;
calc_xy(tgt_angle, u, v);
- //r = hypot(now_x - tgt_x, now_y - tgt_y);
+ r = hypot(now_x - tgt_x, now_y - tgt_y);
while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
- if(id1_value[0] != 1)break;
- if(id1_value[6] != flag)break;
+ first_speed = (first_speed50 - last_speed) * r / 50 + last_speed;
- //first_speed = first_speed50 * r / 50;
-
- /*if(first_speed > Max_speed){
- gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle);
+ 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){ ////目標地点までの距離が２センチ以上３センチ未満のとき
+ 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){
+ 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{
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);
@@ -560,13 +574,11 @@
r=hypot(now_x - tgt_x, now_y - tgt_y);
if(r < R) break;
+ if(id1_value[0] != 1)break;
}
while(1) {
- if(id1_value[0] != 1)break;
- if(id1_value[6] != flag)break;
-
//calc_gyro();
now_angle=gyro.getAngle();
printf("angle = %f\n\r",now_angle);
@@ -582,6 +594,12 @@
}
if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+ if(id1_value[0] != 1)break;
}
MaxonControl(0,0,0,0);
-}
\ No newline at end of file
+}
+
+/*----------------------コメント------------------------*/
+/* 3/10(日):pos_correctionの中身を一部修正しました。（引数に変更はありません。）
+
+/*---------------------------------------------------*/
\ No newline at end of file
--- a/movement/movement.h Wed Mar 13 03:24:49 2019 +0000 +++ b/movement/movement.h Wed Mar 13 04:23:18 2019 +0000 @@ -9,6 +9,10 @@ void UserLoopSetting_sensor(); +void UserLoopSetting_enc_right(); + +void UserLoopSetting_enc_left(); + void calOmega(); void output(double FL,double BL,double BR,double FR);