ロボステ6期 / Mbed 2 deprecated control_UC

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

movement/movement.cpp@1:d66ba60fa9a9, 2019-06-15 (annotated)

Committer:
yuki0701
Date:
Sat Jun 15 04:46:41 2019 +0000
Revision:
1:d66ba60fa9a9
Parent:
0:0727df1dfa3e 
a

Who changed what in which revision?

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