3/19 23:30
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
movement/movement.cpp@11:9db93bce4eef, 2019-03-19 (annotated)
- Committer:
- yuki0701
- Date:
- Tue Mar 19 02:30:34 2019 +0000
- Revision:
- 11:9db93bce4eef
- Parent:
- 10:c741191360de
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
la00noix | 0:c61c6e4775ca | 1 | #include "EC.h" |
la00noix | 0:c61c6e4775ca | 2 | #include "R1370P.h" |
la00noix | 0:c61c6e4775ca | 3 | #include "move4wheel.h" |
la00noix | 0:c61c6e4775ca | 4 | #include "mbed.h" |
la00noix | 0:c61c6e4775ca | 5 | #include "math.h" |
la00noix | 0:c61c6e4775ca | 6 | #include "PathFollowing.h" |
la00noix | 0:c61c6e4775ca | 7 | #include "movement.h" |
la00noix | 0:c61c6e4775ca | 8 | #include "maxonsetting.h" |
la00noix | 0:c61c6e4775ca | 9 | #include "manual.h" |
la00noix | 0:c61c6e4775ca | 10 | #include "can.h" |
la00noix | 0:c61c6e4775ca | 11 | |
la00noix | 0:c61c6e4775ca | 12 | #define PI 3.141592 |
la00noix | 0:c61c6e4775ca | 13 | |
la00noix | 8:2ba338b4590e | 14 | char can_ashileddata[2]= {0}; |
la00noix | 6:26724c287387 | 15 | int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3; |
la00noix | 0:c61c6e4775ca | 16 | |
la00noix | 6:26724c287387 | 17 | Ec EC1(p22,p21,NC,500,0.05); |
yuki0701 | 3:8a0faa3b08c3 | 18 | Ec EC2(p26,p8,NC,500,0.05); |
la00noix | 6:26724c287387 | 19 | |
la00noix | 0:c61c6e4775ca | 20 | Ticker ec_ticker; //ec角速度計算用ticker |
la00noix | 0:c61c6e4775ca | 21 | |
la00noix | 0:c61c6e4775ca | 22 | R1370P gyro(p28,p27); |
la00noix | 0:c61c6e4775ca | 23 | |
la00noix | 0:c61c6e4775ca | 24 | double new_dist1=0,new_dist2=0; |
la00noix | 0:c61c6e4775ca | 25 | double old_dist1=0,old_dist2=0; |
la00noix | 0:c61c6e4775ca | 26 | double d_dist1=0,d_dist2=0; //座標計算用関数 |
la00noix | 0:c61c6e4775ca | 27 | double d_x,d_y; |
la00noix | 0:c61c6e4775ca | 28 | //現在地X,y座標、現在角度については、PathFollowingでnow_x,now_y,now_angleを定義済 |
la00noix | 0:c61c6e4775ca | 29 | double start_x=0,start_y=0; //スタート位置 |
la00noix | 0:c61c6e4775ca | 30 | double x_out,y_out,r_out; //出力値 |
la00noix | 0:c61c6e4775ca | 31 | |
la00noix | 0:c61c6e4775ca | 32 | int16_t m1=0, m2=0, m3=0, m4=0; //int16bit = int2byte |
la00noix | 0:c61c6e4775ca | 33 | |
la00noix | 0:c61c6e4775ca | 34 | double xy_type,pm_typeX,pm_typeY,x_base,y_base; |
la00noix | 0:c61c6e4775ca | 35 | |
la00noix | 9:7667dcfc7ce5 | 36 | int flag; |
la00noix | 9:7667dcfc7ce5 | 37 | |
la00noix | 0:c61c6e4775ca | 38 | ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言///////////////// |
la00noix | 0:c61c6e4775ca | 39 | |
la00noix | 0:c61c6e4775ca | 40 | /*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する |
la00noix | 0:c61c6e4775ca | 41 | *状況に応じて、どのセンサーにより算出した情報を信用するかを選択し、その都度now_angle,now_x,now_yに代入する。(何種類かのセンサーの情報を混ぜて使用することも可能) |
la00noix | 0:c61c6e4775ca | 42 | *(ex) |
la00noix | 0:c61c6e4775ca | 43 | *info.nowX.enc → エンコーダにより算出した機体位置のx座標 |
la00noix | 0:c61c6e4775ca | 44 | *info.nowY.usw → 超音波センサーにより求めた機体位置のy座標 |
la00noix | 0:c61c6e4775ca | 45 | */ |
la00noix | 0:c61c6e4775ca | 46 | |
la00noix | 6:26724c287387 | 47 | typedef struct { //使用センサーの種類 |
la00noix | 0:c61c6e4775ca | 48 | double usw; //超音波センサー |
la00noix | 0:c61c6e4775ca | 49 | double enc; //エンコーダ |
la00noix | 0:c61c6e4775ca | 50 | double gyro; //ジャイロ |
la00noix | 0:c61c6e4775ca | 51 | //double line;//ラインセンサー |
la00noix | 6:26724c287387 | 52 | } robo_sensor; |
la00noix | 0:c61c6e4775ca | 53 | |
la00noix | 6:26724c287387 | 54 | typedef struct { //機体情報の種類 |
la00noix | 0:c61c6e4775ca | 55 | robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも |
la00noix | 0:c61c6e4775ca | 56 | robo_sensor nowX; |
la00noix | 0:c61c6e4775ca | 57 | robo_sensor nowY; |
la00noix | 6:26724c287387 | 58 | } robo_data; |
la00noix | 0:c61c6e4775ca | 59 | |
la00noix | 6:26724c287387 | 60 | robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化 |
la00noix | 0:c61c6e4775ca | 61 | |
la00noix | 0:c61c6e4775ca | 62 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
la00noix | 0:c61c6e4775ca | 63 | |
la00noix | 6:26724c287387 | 64 | void UserLoopSetting_sensor() |
la00noix | 6:26724c287387 | 65 | { |
la00noix | 6:26724c287387 | 66 | |
la00noix | 0:c61c6e4775ca | 67 | gyro.initialize(); |
la00noix | 0:c61c6e4775ca | 68 | ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 |
la00noix | 4:317c53a674fa | 69 | EC1.setDiameter_mm(25.5); |
la00noix | 4:317c53a674fa | 70 | EC2.setDiameter_mm(25.5); //測定輪半径//後で測定 |
yuki0701 | 10:c741191360de | 71 | } |
yuki0701 | 10:c741191360de | 72 | void UserLoopSetting_enc_right() |
yuki0701 | 10:c741191360de | 73 | { |
yuki0701 | 11:9db93bce4eef | 74 | info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド) |
yuki0701 | 10:c741191360de | 75 | info.nowY.enc = 3500; |
yuki0701 | 10:c741191360de | 76 | } |
yuki0701 | 10:c741191360de | 77 | void UserLoopSetting_enc_left() |
yuki0701 | 10:c741191360de | 78 | { |
yuki0701 | 11:9db93bce4eef | 79 | info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド) |
yuki0701 | 1:3c11e07da92a | 80 | info.nowY.enc = 3500; |
la00noix | 0:c61c6e4775ca | 81 | } |
la00noix | 0:c61c6e4775ca | 82 | |
la00noix | 0:c61c6e4775ca | 83 | void calOmega() //角速度計算関数 |
la00noix | 0:c61c6e4775ca | 84 | { |
la00noix | 0:c61c6e4775ca | 85 | EC1.CalOmega(); |
la00noix | 0:c61c6e4775ca | 86 | EC2.CalOmega(); |
la00noix | 0:c61c6e4775ca | 87 | } |
la00noix | 0:c61c6e4775ca | 88 | |
la00noix | 0:c61c6e4775ca | 89 | void output(double FL,double BL,double BR,double FR) |
la00noix | 0:c61c6e4775ca | 90 | { |
la00noix | 0:c61c6e4775ca | 91 | m1=FL; |
la00noix | 0:c61c6e4775ca | 92 | m2=BL; |
la00noix | 0:c61c6e4775ca | 93 | m3=BR; |
la00noix | 0:c61c6e4775ca | 94 | m4=FR; |
la00noix | 0:c61c6e4775ca | 95 | } |
la00noix | 0:c61c6e4775ca | 96 | |
la00noix | 0:c61c6e4775ca | 97 | void base(double FL,double BL,double BR,double FR,double Max) |
la00noix | 0:c61c6e4775ca | 98 | //いろんな加算をしても最大OR最小がMaxになるような補正//絶対値が一番でかいやつで除算 |
la00noix | 0:c61c6e4775ca | 99 | //DCモーターならMax=1、マクソンは-4095~4095だからMax=4095にする |
la00noix | 0:c61c6e4775ca | 100 | { |
la00noix | 0:c61c6e4775ca | 101 | if(fabs(FL)>=Max||fabs(BL)>=Max||fabs(BR)>=Max||fabs(FR)>=Max) { |
la00noix | 0:c61c6e4775ca | 102 | |
la00noix | 0:c61c6e4775ca | 103 | if (fabs(FL)>=fabs(BL)&&fabs(FL)>=fabs(BR)&&fabs(FL)>=fabs(FR))output(Max*FL/fabs(FL),Max*BL/fabs(FL),Max*BR/fabs(FL),Max*FR/fabs(FL)); |
la00noix | 0:c61c6e4775ca | 104 | else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL)); |
la00noix | 0:c61c6e4775ca | 105 | else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR)); |
la00noix | 0:c61c6e4775ca | 106 | else output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR)); |
la00noix | 6:26724c287387 | 107 | } else { |
la00noix | 0:c61c6e4775ca | 108 | output(FL,BL,BR,FR); |
la00noix | 0:c61c6e4775ca | 109 | } |
la00noix | 0:c61c6e4775ca | 110 | } |
la00noix | 0:c61c6e4775ca | 111 | |
la00noix | 6:26724c287387 | 112 | void ashi_led() |
la00noix | 6:26724c287387 | 113 | { |
la00noix | 6:26724c287387 | 114 | |
la00noix | 6:26724c287387 | 115 | if(now_angle > -1 && now_angle < 1) { |
la00noix | 6:26724c287387 | 116 | can_ashileddata0_0 = 1; |
la00noix | 6:26724c287387 | 117 | } else { |
la00noix | 6:26724c287387 | 118 | can_ashileddata0_0 = 0; |
la00noix | 0:c61c6e4775ca | 119 | } |
la00noix | 6:26724c287387 | 120 | |
la00noix | 6:26724c287387 | 121 | if(now_angle > 350) { |
la00noix | 6:26724c287387 | 122 | can_ashileddata0_1 = 1; |
la00noix | 6:26724c287387 | 123 | } else { |
la00noix | 6:26724c287387 | 124 | can_ashileddata0_1 = 0; |
la00noix | 0:c61c6e4775ca | 125 | } |
la00noix | 6:26724c287387 | 126 | |
la00noix | 6:26724c287387 | 127 | if(now_x > -1 && now_x < 1) { |
la00noix | 6:26724c287387 | 128 | can_ashileddata0_2 = 1; |
la00noix | 6:26724c287387 | 129 | } else { |
la00noix | 6:26724c287387 | 130 | can_ashileddata0_2 = 0; |
la00noix | 0:c61c6e4775ca | 131 | } |
la00noix | 6:26724c287387 | 132 | |
la00noix | 6:26724c287387 | 133 | if(now_y > -1 && now_y < 1) { |
la00noix | 6:26724c287387 | 134 | can_ashileddata0_3 = 1; |
la00noix | 6:26724c287387 | 135 | } else { |
la00noix | 6:26724c287387 | 136 | can_ashileddata0_3 = 0; |
la00noix | 0:c61c6e4775ca | 137 | } |
la00noix | 6:26724c287387 | 138 | |
la00noix | 6:26724c287387 | 139 | can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4); |
la00noix | 0:c61c6e4775ca | 140 | } |
la00noix | 0:c61c6e4775ca | 141 | |
la00noix | 6:26724c287387 | 142 | void calc_gyro() |
la00noix | 6:26724c287387 | 143 | { |
la00noix | 0:c61c6e4775ca | 144 | now_angle=gyro.getAngle(); //ジャイロの値読み込み |
la00noix | 0:c61c6e4775ca | 145 | } |
la00noix | 0:c61c6e4775ca | 146 | |
la00noix | 0:c61c6e4775ca | 147 | void calc_xy_enc() //エンコーダ&ジャイロによる座標計算 |
la00noix | 0:c61c6e4775ca | 148 | { |
la00noix | 0:c61c6e4775ca | 149 | now_angle=gyro.getAngle(); //ジャイロの値読み込み |
la00noix | 0:c61c6e4775ca | 150 | |
la00noix | 0:c61c6e4775ca | 151 | new_dist1=EC1.getDistance_mm(); |
la00noix | 0:c61c6e4775ca | 152 | new_dist2=EC2.getDistance_mm(); |
la00noix | 0:c61c6e4775ca | 153 | d_dist1=new_dist1-old_dist1; |
la00noix | 0:c61c6e4775ca | 154 | d_dist2=new_dist2-old_dist2; |
la00noix | 0:c61c6e4775ca | 155 | old_dist1=new_dist1; |
la00noix | 0:c61c6e4775ca | 156 | old_dist2=new_dist2; //微小時間当たりのエンコーダ読み込み |
la00noix | 0:c61c6e4775ca | 157 | |
la00noix | 0:c61c6e4775ca | 158 | d_x=d_dist2*sin(now_angle*PI/180)-d_dist1*cos(now_angle*PI/180); |
la00noix | 0:c61c6e4775ca | 159 | d_y=d_dist2*cos(now_angle*PI/180)+d_dist1*sin(now_angle*PI/180); //微小時間毎の座標変化 |
la00noix | 0:c61c6e4775ca | 160 | info.nowX.enc = info.nowX.enc + d_x; |
la00noix | 0:c61c6e4775ca | 161 | info.nowY.enc = info.nowY.enc - d_y; //微小時間毎に座標に加算 |
la00noix | 0:c61c6e4775ca | 162 | } |
la00noix | 0:c61c6e4775ca | 163 | |
la00noix | 6:26724c287387 | 164 | void set_cond(int t, int px, double bx, int py, double by) //超音波センサーを使用するときの条件設定関数 |
la00noix | 6:26724c287387 | 165 | { |
la00noix | 0:c61c6e4775ca | 166 | //引数の詳細は関数"calc_xy_usw"参照 |
la00noix | 6:26724c287387 | 167 | |
la00noix | 6:26724c287387 | 168 | xy_type = t; |
la00noix | 6:26724c287387 | 169 | |
la00noix | 6:26724c287387 | 170 | pm_typeX = px; |
la00noix | 6:26724c287387 | 171 | x_base = bx; |
la00noix | 6:26724c287387 | 172 | |
la00noix | 6:26724c287387 | 173 | pm_typeY = py; |
la00noix | 6:26724c287387 | 174 | y_base = by; |
la00noix | 0:c61c6e4775ca | 175 | } |
la00noix | 0:c61c6e4775ca | 176 | |
la00noix | 6:26724c287387 | 177 | void calc_xy_usw(double tgt_angle) //超音波センサーによる座標計算(機体が旋回する場合はこの方法による座標計算は出来ない) |
la00noix | 6:26724c287387 | 178 | { |
la00noix | 0:c61c6e4775ca | 179 | //tgt_angle:機体の目標角度(運動初期角度と同じ/今大会では0,90,180のみ) |
la00noix | 0:c61c6e4775ca | 180 | //xy_type:(0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む) |
la00noix | 0:c61c6e4775ca | 181 | //pm_typeX,pm_typeY:(0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む) |
la00noix | 0:c61c6e4775ca | 182 | //x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標) |
la00noix | 6:26724c287387 | 183 | |
la00noix | 0:c61c6e4775ca | 184 | double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離 |
la00noix | 9:7667dcfc7ce5 | 185 | double D1=30,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離(時計回りを正とする) |
la00noix | 6:26724c287387 | 186 | |
la00noix | 0:c61c6e4775ca | 187 | now_angle=gyro.getAngle(); //ジャイロの値読み込み |
la00noix | 6:26724c287387 | 188 | |
la00noix | 6:26724c287387 | 189 | if(tgt_angle==0) { |
la00noix | 6:26724c287387 | 190 | if((xy_type==0 || xy_type==2) && pm_typeX==0) { |
la00noix | 6:26724c287387 | 191 | |
la00noix | 6:26724c287387 | 192 | info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 193 | |
la00noix | 6:26724c287387 | 194 | } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { |
la00noix | 6:26724c287387 | 195 | |
la00noix | 6:26724c287387 | 196 | info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 197 | |
la00noix | 6:26724c287387 | 198 | } |
la00noix | 6:26724c287387 | 199 | if((xy_type==1 || xy_type==2) && pm_typeY==0) { |
la00noix | 6:26724c287387 | 200 | |
la00noix | 6:26724c287387 | 201 | info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 202 | |
la00noix | 6:26724c287387 | 203 | } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { |
la00noix | 6:26724c287387 | 204 | |
la00noix | 6:26724c287387 | 205 | info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 206 | |
la00noix | 6:26724c287387 | 207 | } |
la00noix | 6:26724c287387 | 208 | |
la00noix | 6:26724c287387 | 209 | } else if(tgt_angle==90) { |
la00noix | 6:26724c287387 | 210 | if((xy_type==0 || xy_type==2) && pm_typeX==0) { |
la00noix | 6:26724c287387 | 211 | |
la00noix | 9:7667dcfc7ce5 | 212 | info.nowX.usw = x_base - (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 213 | |
la00noix | 6:26724c287387 | 214 | } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { |
la00noix | 6:26724c287387 | 215 | |
la00noix | 9:7667dcfc7ce5 | 216 | info.nowX.usw = x_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 217 | |
la00noix | 6:26724c287387 | 218 | } |
la00noix | 6:26724c287387 | 219 | if((xy_type==1 || xy_type==2) && pm_typeY==0) { |
la00noix | 6:26724c287387 | 220 | |
la00noix | 9:7667dcfc7ce5 | 221 | info.nowY.usw = y_base - (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 222 | |
la00noix | 6:26724c287387 | 223 | } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { |
la00noix | 6:26724c287387 | 224 | |
la00noix | 9:7667dcfc7ce5 | 225 | info.nowY.usw = y_base + (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 226 | |
la00noix | 6:26724c287387 | 227 | } |
la00noix | 6:26724c287387 | 228 | |
la00noix | 9:7667dcfc7ce5 | 229 | } else if(tgt_angle==180 || tgt_angle==-180) { |
la00noix | 6:26724c287387 | 230 | if((xy_type==0 || xy_type==2) && pm_typeX==0) { |
la00noix | 6:26724c287387 | 231 | |
la00noix | 9:7667dcfc7ce5 | 232 | info.nowX.usw = x_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 233 | |
la00noix | 6:26724c287387 | 234 | } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { |
la00noix | 6:26724c287387 | 235 | |
la00noix | 9:7667dcfc7ce5 | 236 | info.nowX.usw = x_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 237 | |
la00noix | 6:26724c287387 | 238 | } |
la00noix | 6:26724c287387 | 239 | if((xy_type==1 || xy_type==2) && pm_typeY==0) { |
la00noix | 6:26724c287387 | 240 | |
la00noix | 9:7667dcfc7ce5 | 241 | info.nowY.usw = y_base - (usw_data1+ R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 242 | |
la00noix | 6:26724c287387 | 243 | } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { |
la00noix | 6:26724c287387 | 244 | |
la00noix | 9:7667dcfc7ce5 | 245 | info.nowY.usw = y_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); |
la00noix | 9:7667dcfc7ce5 | 246 | |
la00noix | 9:7667dcfc7ce5 | 247 | } |
la00noix | 9:7667dcfc7ce5 | 248 | } else if(tgt_angle==-90) { |
la00noix | 9:7667dcfc7ce5 | 249 | if((xy_type==0 || xy_type==2) && pm_typeX==0) { |
la00noix | 9:7667dcfc7ce5 | 250 | |
la00noix | 9:7667dcfc7ce5 | 251 | info.nowX.usw = x_base - (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180)); |
la00noix | 9:7667dcfc7ce5 | 252 | |
la00noix | 9:7667dcfc7ce5 | 253 | } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { |
la00noix | 9:7667dcfc7ce5 | 254 | |
la00noix | 9:7667dcfc7ce5 | 255 | info.nowX.usw = x_base + (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180)); |
la00noix | 9:7667dcfc7ce5 | 256 | |
la00noix | 9:7667dcfc7ce5 | 257 | } |
la00noix | 9:7667dcfc7ce5 | 258 | if((xy_type==1 || xy_type==2) && pm_typeY==0) { |
la00noix | 9:7667dcfc7ce5 | 259 | |
la00noix | 9:7667dcfc7ce5 | 260 | info.nowY.usw = y_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180)); |
la00noix | 9:7667dcfc7ce5 | 261 | |
la00noix | 9:7667dcfc7ce5 | 262 | } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { |
la00noix | 9:7667dcfc7ce5 | 263 | |
la00noix | 9:7667dcfc7ce5 | 264 | info.nowY.usw = y_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180)); |
la00noix | 6:26724c287387 | 265 | |
la00noix | 6:26724c287387 | 266 | } |
la00noix | 0:c61c6e4775ca | 267 | } |
la00noix | 0:c61c6e4775ca | 268 | } |
la00noix | 0:c61c6e4775ca | 269 | |
la00noix | 6:26724c287387 | 270 | void calc_xy(double target_angle, double u,double v) |
la00noix | 6:26724c287387 | 271 | { |
la00noix | 0:c61c6e4775ca | 272 | //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する |
la00noix | 6:26724c287387 | 273 | |
la00noix | 6:26724c287387 | 274 | calc_xy_enc(); |
yuki0701 | 11:9db93bce4eef | 275 | |
yuki0701 | 11:9db93bce4eef | 276 | if(usw_flag == 1){ |
yuki0701 | 11:9db93bce4eef | 277 | u = 1; |
yuki0701 | 11:9db93bce4eef | 278 | v = 1; |
yuki0701 | 11:9db93bce4eef | 279 | } |
la00noix | 6:26724c287387 | 280 | |
la00noix | 6:26724c287387 | 281 | if(u != 1 || v != 1) { |
la00noix | 6:26724c287387 | 282 | calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 |
la00noix | 6:26724c287387 | 283 | } |
la00noix | 6:26724c287387 | 284 | |
la00noix | 0:c61c6e4775ca | 285 | now_x = u * info.nowX.enc + (1-u) * info.nowX.usw; |
la00noix | 0:c61c6e4775ca | 286 | now_y = v * info.nowY.enc + (1-v) * info.nowY.usw; |
la00noix | 6:26724c287387 | 287 | |
la00noix | 0:c61c6e4775ca | 288 | /*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){ //スタート時の0合わせ用 |
la00noix | 0:c61c6e4775ca | 289 | ec_led = 1; |
la00noix | 0:c61c6e4775ca | 290 | }else{ |
la00noix | 0:c61c6e4775ca | 291 | ec_led = 0; |
la00noix | 0:c61c6e4775ca | 292 | } |
la00noix | 6:26724c287387 | 293 | |
la00noix | 0:c61c6e4775ca | 294 | if(now_angle >-0.5 && now_angle <0.5){ |
la00noix | 0:c61c6e4775ca | 295 | gyro_led = 1; |
la00noix | 0:c61c6e4775ca | 296 | }else{ |
la00noix | 0:c61c6e4775ca | 297 | gyro_led = 0; |
la00noix | 0:c61c6e4775ca | 298 | }*/ |
la00noix | 0:c61c6e4775ca | 299 | } |
la00noix | 0:c61c6e4775ca | 300 | |
la00noix | 9:7667dcfc7ce5 | 301 | void enc_correction(int x_select,int y_select) //エンコーダの座標を超音波センサの座標で上書き |
la00noix | 9:7667dcfc7ce5 | 302 | { |
la00noix | 9:7667dcfc7ce5 | 303 | //x_select,y_select → (0:上書きしない/1:上書きする) |
la00noix | 9:7667dcfc7ce5 | 304 | |
la00noix | 9:7667dcfc7ce5 | 305 | if(x_select == 1) { |
la00noix | 9:7667dcfc7ce5 | 306 | info.nowX.enc = info.nowX.usw; |
la00noix | 9:7667dcfc7ce5 | 307 | } |
la00noix | 9:7667dcfc7ce5 | 308 | if(y_select == 1) { |
la00noix | 9:7667dcfc7ce5 | 309 | info.nowY.enc = info.nowY.usw; |
la00noix | 9:7667dcfc7ce5 | 310 | } |
la00noix | 9:7667dcfc7ce5 | 311 | |
la00noix | 9:7667dcfc7ce5 | 312 | } |
la00noix | 9:7667dcfc7ce5 | 313 | |
la00noix | 0:c61c6e4775ca | 314 | //ここからそれぞれのプログラム///////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
la00noix | 0:c61c6e4775ca | 315 | //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正) |
la00noix | 0:c61c6e4775ca | 316 | //ジャイロの出力は角度だが三角関数はラジアンとして計算する |
la00noix | 0:c61c6e4775ca | 317 | //通常の移動+座標のずれ補正+機体の角度補正(+必要に応じさらに別補正) |
la00noix | 0:c61c6e4775ca | 318 | //ジャイロの仕様上、角度補正をするときに計算式内で角度はそのままよりsinをとったほうがいいかもね |
la00noix | 0:c61c6e4775ca | 319 | |
la00noix | 0:c61c6e4775ca | 320 | void purecurve(int type,double u,double v, //正面を変えずに円弧or楕円を描いて曲がる |
la00noix | 0:c61c6e4775ca | 321 | double point_x1,double point_y1, |
la00noix | 0:c61c6e4775ca | 322 | double point_x2,double point_y2, |
la00noix | 0:c61c6e4775ca | 323 | int theta, |
la00noix | 0:c61c6e4775ca | 324 | double speed, |
la00noix | 0:c61c6e4775ca | 325 | double q_p,double q_d, |
la00noix | 0:c61c6e4775ca | 326 | double r_p,double r_d, |
la00noix | 0:c61c6e4775ca | 327 | double r_out_max, |
la00noix | 0:c61c6e4775ca | 328 | double target_angle) |
la00noix | 0:c61c6e4775ca | 329 | //type:動きの種類(8パターン) point_x1,point_y1=出発地点の座標 point_x2,point_x2=目標地点の座標 theta=plotの間隔(0~90°) speed=速度 |
la00noix | 0:c61c6e4775ca | 330 | { |
la00noix | 0:c61c6e4775ca | 331 | //-----PathFollowingのパラメーター設定-----// |
la00noix | 0:c61c6e4775ca | 332 | q_setPDparam(q_p,q_d); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 |
la00noix | 0:c61c6e4775ca | 333 | r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 |
la00noix | 0:c61c6e4775ca | 334 | set_r_out(r_out_max); //旋回時の最大出力値設定関数 |
la00noix | 0:c61c6e4775ca | 335 | set_target_angle(target_angle); //機体目標角度設定関数 |
la00noix | 0:c61c6e4775ca | 336 | |
la00noix | 0:c61c6e4775ca | 337 | int s; |
la00noix | 0:c61c6e4775ca | 338 | int t = 0; |
la00noix | 0:c61c6e4775ca | 339 | double X,Y;//X=楕円の中心座標、Y=楕円の中心座標 |
la00noix | 0:c61c6e4775ca | 340 | double a,b; //a=楕円のx軸方向の幅の半分,b=楕円のy軸方向の幅の半分 |
la00noix | 0:c61c6e4775ca | 341 | double plotx[(90/theta)+1]; //楕円にとるplotのx座標 |
la00noix | 0:c61c6e4775ca | 342 | double ploty[(90/theta)+1]; |
la00noix | 0:c61c6e4775ca | 343 | |
la00noix | 0:c61c6e4775ca | 344 | double x_out,y_out,r_out; |
la00noix | 0:c61c6e4775ca | 345 | |
la00noix | 0:c61c6e4775ca | 346 | a=fabs(point_x1-point_x2); |
la00noix | 0:c61c6e4775ca | 347 | b=fabs(point_y1-point_y2); |
la00noix | 0:c61c6e4775ca | 348 | |
la00noix | 0:c61c6e4775ca | 349 | switch(type) { |
la00noix | 0:c61c6e4775ca | 350 | |
la00noix | 0:c61c6e4775ca | 351 | case 1://→↑移動 |
la00noix | 0:c61c6e4775ca | 352 | X=point_x1; |
la00noix | 0:c61c6e4775ca | 353 | Y=point_y2; |
la00noix | 0:c61c6e4775ca | 354 | |
la00noix | 0:c61c6e4775ca | 355 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 356 | plotx[s] = X + a * cos(-PI/2 + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 357 | ploty[s] = Y + b * sin(-PI/2 + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 358 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 359 | } |
la00noix | 0:c61c6e4775ca | 360 | break; |
la00noix | 0:c61c6e4775ca | 361 | |
la00noix | 0:c61c6e4775ca | 362 | case 2://↑→移動 |
la00noix | 0:c61c6e4775ca | 363 | X=point_x2; |
la00noix | 0:c61c6e4775ca | 364 | Y=point_y1; |
la00noix | 0:c61c6e4775ca | 365 | |
la00noix | 0:c61c6e4775ca | 366 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 367 | plotx[s] = X + a * cos(PI - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 368 | ploty[s] = Y + b * sin(PI - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 369 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 370 | } |
la00noix | 0:c61c6e4775ca | 371 | break; |
la00noix | 0:c61c6e4775ca | 372 | |
la00noix | 0:c61c6e4775ca | 373 | case 3://↑←移動 |
la00noix | 0:c61c6e4775ca | 374 | X=point_x2; |
la00noix | 0:c61c6e4775ca | 375 | Y=point_y1; |
la00noix | 0:c61c6e4775ca | 376 | |
la00noix | 0:c61c6e4775ca | 377 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 378 | plotx[s] = X + a * cos(s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 379 | ploty[s] = Y + b * sin(s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 380 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 381 | } |
la00noix | 0:c61c6e4775ca | 382 | break; |
la00noix | 0:c61c6e4775ca | 383 | |
la00noix | 0:c61c6e4775ca | 384 | case 4://←↑移動 |
la00noix | 0:c61c6e4775ca | 385 | X=point_x1; |
la00noix | 0:c61c6e4775ca | 386 | Y=point_y2; |
la00noix | 0:c61c6e4775ca | 387 | |
la00noix | 0:c61c6e4775ca | 388 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 389 | plotx[s] = X + a * cos(-PI/2 - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 390 | ploty[s] = Y + b * sin(-PI/2 - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 391 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 392 | } |
la00noix | 0:c61c6e4775ca | 393 | break; |
la00noix | 0:c61c6e4775ca | 394 | |
la00noix | 0:c61c6e4775ca | 395 | case 5://←↓移動 |
la00noix | 0:c61c6e4775ca | 396 | X=point_x1; |
la00noix | 0:c61c6e4775ca | 397 | Y=point_y2; |
la00noix | 0:c61c6e4775ca | 398 | |
la00noix | 0:c61c6e4775ca | 399 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 400 | plotx[s] = X + a * cos(PI/2 + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 401 | ploty[s] = Y + b * sin(PI/2 + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 402 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 403 | } |
la00noix | 0:c61c6e4775ca | 404 | break; |
la00noix | 0:c61c6e4775ca | 405 | |
la00noix | 0:c61c6e4775ca | 406 | case 6://↓←移動 |
la00noix | 0:c61c6e4775ca | 407 | X=point_x2; |
la00noix | 0:c61c6e4775ca | 408 | Y=point_y1; |
la00noix | 0:c61c6e4775ca | 409 | |
la00noix | 0:c61c6e4775ca | 410 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 411 | plotx[s] = X + a * cos(-s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 412 | ploty[s] = Y + b * sin(-s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 413 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 414 | } |
la00noix | 0:c61c6e4775ca | 415 | break; |
la00noix | 0:c61c6e4775ca | 416 | |
la00noix | 0:c61c6e4775ca | 417 | case 7://↓→移動 |
la00noix | 0:c61c6e4775ca | 418 | X=point_x2; |
la00noix | 0:c61c6e4775ca | 419 | Y=point_y1; |
la00noix | 0:c61c6e4775ca | 420 | |
la00noix | 0:c61c6e4775ca | 421 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 422 | plotx[s] = X + a * cos(PI + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 423 | ploty[s] = Y + b * sin(PI + s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 424 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 425 | } |
la00noix | 0:c61c6e4775ca | 426 | break; |
la00noix | 0:c61c6e4775ca | 427 | |
la00noix | 0:c61c6e4775ca | 428 | case 8://→↓移動 |
la00noix | 0:c61c6e4775ca | 429 | X=point_x1; |
la00noix | 0:c61c6e4775ca | 430 | Y=point_y2; |
la00noix | 0:c61c6e4775ca | 431 | |
la00noix | 0:c61c6e4775ca | 432 | for(s=0; s<((90/theta)+1); s++) { |
la00noix | 0:c61c6e4775ca | 433 | plotx[s] = X + a * cos(PI/2 - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 434 | ploty[s] = Y + b * sin(PI/2 - s * (PI*theta/180)); |
la00noix | 0:c61c6e4775ca | 435 | //debug_printf("plotx[%d]=%f ploty[%d]=%f\n\r",s,plotx[s],s,ploty[s]); |
la00noix | 0:c61c6e4775ca | 436 | } |
la00noix | 0:c61c6e4775ca | 437 | break; |
la00noix | 0:c61c6e4775ca | 438 | } |
la00noix | 0:c61c6e4775ca | 439 | |
la00noix | 0:c61c6e4775ca | 440 | while(1) { |
la00noix | 0:c61c6e4775ca | 441 | |
la00noix | 9:7667dcfc7ce5 | 442 | if(id1_value[0] != 1)break; |
la00noix | 9:7667dcfc7ce5 | 443 | if(id1_value[6] != flag)break; |
la00noix | 9:7667dcfc7ce5 | 444 | |
la00noix | 0:c61c6e4775ca | 445 | calc_xy(target_angle,u,v); |
la00noix | 0:c61c6e4775ca | 446 | |
la00noix | 0:c61c6e4775ca | 447 | XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed,speed); |
la00noix | 0:c61c6e4775ca | 448 | CalMotorOut(x_out,y_out,r_out); |
la00noix | 0:c61c6e4775ca | 449 | //debug_printf("t=%d now_x=%f now_y=%f x_out=%f y_out=%f\n\r",t,now_x,now_y,x_out,y_out); |
la00noix | 0:c61c6e4775ca | 450 | |
la00noix | 0:c61c6e4775ca | 451 | base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); //m1~m4に代入 |
la00noix | 0:c61c6e4775ca | 452 | //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); |
la00noix | 6:26724c287387 | 453 | |
la00noix | 0:c61c6e4775ca | 454 | if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++; |
la00noix | 0:c61c6e4775ca | 455 | |
la00noix | 0:c61c6e4775ca | 456 | MaxonControl(m1,m2,m3,m4); //出力 |
la00noix | 0:c61c6e4775ca | 457 | debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle); |
la00noix | 6:26724c287387 | 458 | |
la00noix | 0:c61c6e4775ca | 459 | if(t == (90/theta))break; |
la00noix | 0:c61c6e4775ca | 460 | } |
la00noix | 0:c61c6e4775ca | 461 | } |
la00noix | 0:c61c6e4775ca | 462 | |
la00noix | 0:c61c6e4775ca | 463 | void gogo_straight(double u,double v, //直線運動プログラム |
la00noix | 0:c61c6e4775ca | 464 | double x1_point,double y1_point, |
la00noix | 0:c61c6e4775ca | 465 | double x2_point,double y2_point, |
la00noix | 0:c61c6e4775ca | 466 | double speed1,double speed2, |
la00noix | 0:c61c6e4775ca | 467 | double q_p,double q_d, |
la00noix | 0:c61c6e4775ca | 468 | double r_p,double r_d, |
la00noix | 0:c61c6e4775ca | 469 | double r_out_max, |
la00noix | 0:c61c6e4775ca | 470 | double target_angle) |
la00noix | 0:c61c6e4775ca | 471 | //引数:出発地点の座標(x,y)、目標地点の座標(x,y)、初速度(speed1)、目標速度(speed2)//speed1=speed2 のとき等速運動 |
la00noix | 0:c61c6e4775ca | 472 | { |
la00noix | 0:c61c6e4775ca | 473 | //-----PathFollowingのパラメーター設定-----// |
la00noix | 0:c61c6e4775ca | 474 | q_setPDparam(q_p,q_d); //ベクトルABに垂直な方向の誤差を埋めるPD制御のパラメータ設定関数 |
la00noix | 0:c61c6e4775ca | 475 | r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 |
la00noix | 0:c61c6e4775ca | 476 | set_r_out(r_out_max); //旋回時の最大出力値設定関数 |
la00noix | 0:c61c6e4775ca | 477 | set_target_angle(target_angle); //機体目標角度設定関数 |
la00noix | 6:26724c287387 | 478 | |
la00noix | 0:c61c6e4775ca | 479 | while (1) { |
la00noix | 0:c61c6e4775ca | 480 | |
la00noix | 9:7667dcfc7ce5 | 481 | if(id1_value[0] != 1)break; |
la00noix | 9:7667dcfc7ce5 | 482 | if(id1_value[6] != flag)break; |
la00noix | 9:7667dcfc7ce5 | 483 | |
la00noix | 0:c61c6e4775ca | 484 | calc_xy(target_angle,u,v); |
la00noix | 0:c61c6e4775ca | 485 | |
la00noix | 0:c61c6e4775ca | 486 | XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2); |
la00noix | 0:c61c6e4775ca | 487 | //printf("x = %f, y = %f,angle = %f,x_out=%lf, y_out=%lf, r_out=%lf\n\r",now_x,now_y,now_angle,x_out, y_out,r_out); |
la00noix | 0:c61c6e4775ca | 488 | |
la00noix | 0:c61c6e4775ca | 489 | CalMotorOut(x_out,y_out,r_out); |
la00noix | 0:c61c6e4775ca | 490 | //printf("out1=%lf, out2=%lf, out3=%lf, out4=%lf\n",GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); |
la00noix | 0:c61c6e4775ca | 491 | |
la00noix | 0:c61c6e4775ca | 492 | base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); |
la00noix | 0:c61c6e4775ca | 493 | //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4); |
la00noix | 6:26724c287387 | 494 | |
la00noix | 0:c61c6e4775ca | 495 | MaxonControl(m1,m2,m3,m4); |
la00noix | 0:c61c6e4775ca | 496 | debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle); |
la00noix | 0:c61c6e4775ca | 497 | |
la00noix | 0:c61c6e4775ca | 498 | if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break; |
la00noix | 0:c61c6e4775ca | 499 | } |
la00noix | 0:c61c6e4775ca | 500 | } |
la00noix | 0:c61c6e4775ca | 501 | |
la00noix | 9:7667dcfc7ce5 | 502 | /*void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる) |
la00noix | 6:26724c287387 | 503 | { |
la00noix | 6:26724c287387 | 504 | |
la00noix | 0:c61c6e4775ca | 505 | double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ |
la00noix | 0:c61c6e4775ca | 506 | double out; |
la00noix | 6:26724c287387 | 507 | |
la00noix | 0:c61c6e4775ca | 508 | calc_xy(tgt_angle, u, v); |
la00noix | 6:26724c287387 | 509 | |
la00noix | 6:26724c287387 | 510 | while(1) { //機体の位置を目標領域(目標座標+許容誤差)に収める |
la00noix | 4:317c53a674fa | 511 | gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,5,0.1,10,0.1,500,tgt_angle); |
la00noix | 0:c61c6e4775ca | 512 | MaxonControl(0,0,0,0); |
la00noix | 6:26724c287387 | 513 | |
la00noix | 0:c61c6e4775ca | 514 | calc_xy(tgt_angle, u, v); |
la00noix | 6:26724c287387 | 515 | |
la00noix | 0:c61c6e4775ca | 516 | r=hypot(now_x - tgt_x, now_y - tgt_y); |
la00noix | 6:26724c287387 | 517 | |
la00noix | 0:c61c6e4775ca | 518 | if(r < R) break; |
la00noix | 7:44ce34007499 | 519 | if(id1_value[0] != 1)break; |
la00noix | 0:c61c6e4775ca | 520 | } |
la00noix | 6:26724c287387 | 521 | |
la00noix | 6:26724c287387 | 522 | while(1) { |
la00noix | 6:26724c287387 | 523 | |
la00noix | 0:c61c6e4775ca | 524 | calc_gyro(); |
la00noix | 6:26724c287387 | 525 | |
la00noix | 0:c61c6e4775ca | 526 | out = 10 * (tgt_angle - now_angle); |
la00noix | 6:26724c287387 | 527 | |
la00noix | 0:c61c6e4775ca | 528 | if(out > 300) { //0~179°のときは時計回りに回転 |
la00noix | 0:c61c6e4775ca | 529 | MaxonControl(300,300,300,300); |
la00noix | 6:26724c287387 | 530 | } else if(out < -300) { |
la00noix | 6:26724c287387 | 531 | MaxonControl(-300,-300,-300,-300); |
la00noix | 6:26724c287387 | 532 | } else if(out <= 300 && out > -300) { |
la00noix | 0:c61c6e4775ca | 533 | MaxonControl(out,out,out,out); |
la00noix | 0:c61c6e4775ca | 534 | } |
la00noix | 6:26724c287387 | 535 | |
yuki0701 | 3:8a0faa3b08c3 | 536 | if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 |
la00noix | 7:44ce34007499 | 537 | if(id1_value[0] != 1)break; |
la00noix | 0:c61c6e4775ca | 538 | } |
la00noix | 0:c61c6e4775ca | 539 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 540 | }*/ |
yuki0701 | 11:9db93bce4eef | 541 | void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v, double R) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる) |
la00noix | 9:7667dcfc7ce5 | 542 | { |
yuki0701 | 10:c741191360de | 543 | //距離に比例的に補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする/初期速度が目標速度を下回らないようにする) |
la00noix | 9:7667dcfc7ce5 | 544 | |
yuki0701 | 11:9db93bce4eef | 545 | double first_speed, first_speed50 = 150,last_speed = 50, Max_speed = 300; |
yuki0701 | 10:c741191360de | 546 | //first_speed50:5センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限 |
yuki0701 | 11:9db93bce4eef | 547 | double r; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ |
la00noix | 9:7667dcfc7ce5 | 548 | double out; |
yuki0701 | 10:c741191360de | 549 | double tgt_xx, tgt_yy; |
la00noix | 9:7667dcfc7ce5 | 550 | |
la00noix | 9:7667dcfc7ce5 | 551 | calc_xy(tgt_angle, u, v); |
la00noix | 9:7667dcfc7ce5 | 552 | |
yuki0701 | 10:c741191360de | 553 | r = hypot(now_x - tgt_x, now_y - tgt_y); |
la00noix | 9:7667dcfc7ce5 | 554 | |
la00noix | 9:7667dcfc7ce5 | 555 | while(1) { //機体の位置を目標領域(目標座標+許容誤差)に収める |
la00noix | 9:7667dcfc7ce5 | 556 | |
yuki0701 | 10:c741191360de | 557 | first_speed = (first_speed50 - last_speed) * r / 50 + last_speed; |
la00noix | 9:7667dcfc7ce5 | 558 | |
yuki0701 | 10:c741191360de | 559 | if(r < 20){ //目標地点までの距離が1センチ以上2センチ未満のとき |
yuki0701 | 10:c741191360de | 560 | gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,30,30,5,0.1,10,0.1,500,tgt_angle); |
yuki0701 | 10:c741191360de | 561 | }else if(r < 30){ ////目標地点までの距離が2センチ以上3センチ未満のとき |
yuki0701 | 10:c741191360de | 562 | gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,40,40,5,0.1,10,0.1,500,tgt_angle); |
yuki0701 | 10:c741191360de | 563 | }else if(first_speed > Max_speed){ |
yuki0701 | 10:c741191360de | 564 | tgt_xx = ((Max_speed - last_speed)*now_x + (first_speed - Max_speed)*tgt_x)/(first_speed-last_speed); |
yuki0701 | 10:c741191360de | 565 | tgt_yy = ((Max_speed - last_speed)*now_y + (first_speed - Max_speed)*tgt_y)/(first_speed-last_speed); |
yuki0701 | 10:c741191360de | 566 | 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); |
yuki0701 | 10:c741191360de | 567 | 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); |
la00noix | 9:7667dcfc7ce5 | 568 | }else{ |
la00noix | 9:7667dcfc7ce5 | 569 | 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); |
yuki0701 | 10:c741191360de | 570 | } |
la00noix | 9:7667dcfc7ce5 | 571 | |
yuki0701 | 10:c741191360de | 572 | //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); |
la00noix | 9:7667dcfc7ce5 | 573 | |
la00noix | 9:7667dcfc7ce5 | 574 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 575 | |
la00noix | 9:7667dcfc7ce5 | 576 | calc_xy(tgt_angle, u, v); |
la00noix | 9:7667dcfc7ce5 | 577 | |
la00noix | 9:7667dcfc7ce5 | 578 | r=hypot(now_x - tgt_x, now_y - tgt_y); |
la00noix | 9:7667dcfc7ce5 | 579 | |
la00noix | 9:7667dcfc7ce5 | 580 | if(r < R) break; |
yuki0701 | 10:c741191360de | 581 | if(id1_value[0] != 1)break; |
la00noix | 9:7667dcfc7ce5 | 582 | } |
la00noix | 9:7667dcfc7ce5 | 583 | |
la00noix | 9:7667dcfc7ce5 | 584 | while(1) { |
la00noix | 9:7667dcfc7ce5 | 585 | |
la00noix | 9:7667dcfc7ce5 | 586 | //calc_gyro(); |
la00noix | 9:7667dcfc7ce5 | 587 | now_angle=gyro.getAngle(); |
yuki0701 | 11:9db93bce4eef | 588 | //printf("angle = %f\n\r",now_angle); |
la00noix | 9:7667dcfc7ce5 | 589 | |
la00noix | 9:7667dcfc7ce5 | 590 | out = 10 * (tgt_angle - now_angle); |
la00noix | 9:7667dcfc7ce5 | 591 | |
la00noix | 9:7667dcfc7ce5 | 592 | if(out > 300) { //0~179°のときは時計回りに回転 |
la00noix | 9:7667dcfc7ce5 | 593 | MaxonControl(-300,-300,-300,-300); |
la00noix | 9:7667dcfc7ce5 | 594 | } else if(out < -300) { |
la00noix | 9:7667dcfc7ce5 | 595 | MaxonControl(300,300,300,300); |
la00noix | 9:7667dcfc7ce5 | 596 | } else if(out <= 300 && out > -300) { |
la00noix | 9:7667dcfc7ce5 | 597 | MaxonControl(-out,-out,-out,-out); |
la00noix | 9:7667dcfc7ce5 | 598 | } |
la00noix | 9:7667dcfc7ce5 | 599 | |
la00noix | 9:7667dcfc7ce5 | 600 | if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 |
yuki0701 | 10:c741191360de | 601 | if(id1_value[0] != 1)break; |
la00noix | 9:7667dcfc7ce5 | 602 | } |
la00noix | 9:7667dcfc7ce5 | 603 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 604 | } |
yuki0701 | 10:c741191360de | 605 | |
yuki0701 | 11:9db93bce4eef | 606 | |
yuki0701 | 10:c741191360de | 607 | /*----------------------コメント------------------------*/ |
yuki0701 | 10:c741191360de | 608 | /* 3/10(日):pos_correctionの中身を一部修正しました。(引数に変更はありません。) |
yuki0701 | 10:c741191360de | 609 | |
yuki0701 | 10:c741191360de | 610 | /*---------------------------------------------------*/ |