2020_TeamA / Mbed 2 deprecated somaomuni2

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 YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C

main.cpp@13:e4de7896f808, 2020-03-26 (annotated)

Committer:
yoshikawaryota
Date:
Thu Mar 26 06:30:34 2020 +0000
Revision:
13:e4de7896f808
Parent:
12:1daa4d490a12
Child:
14:4c49218bb9fb
update OEI;Ver.yoshikawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TakushimaYukimasa 0:9e851dc42cde 1 /**
TakushimaYukimasa 0:9e851dc42cde 2 ********************************************************************************
TakushimaYukimasa 0:9e851dc42cde 3 * @file main.c
TakushimaYukimasa 0:9e851dc42cde 4 * @author You
TakushimaYukimasa 0:9e851dc42cde 5 * @version V?.?.?
TakushimaYukimasa 0:9e851dc42cde 6 * @date Today
TakushimaYukimasa 0:9e851dc42cde 7 * @brief メインファイル
TakushimaYukimasa 0:9e851dc42cde 8 ******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 9 */
TakushimaYukimasa 0:9e851dc42cde 10
TakushimaYukimasa 0:9e851dc42cde 11 /* Includes ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 12 #include <main.h>
TakushimaYukimasa 0:9e851dc42cde 13
TakushimaYukimasa 0:9e851dc42cde 14 /* 型定義 --------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 15
TakushimaYukimasa 0:9e851dc42cde 16 /* ロボットの加速度 */
TakushimaYukimasa 0:9e851dc42cde 17 ROCATION NowAcc;
TakushimaYukimasa 0:9e851dc42cde 18 /* ロボットの座標 */
TakushimaYukimasa 0:9e851dc42cde 19 ROCATION NowLoc;
yoshikawaryota 2:ec149525ec2a 20 /* ロボットの目標座標 */
yoshikawaryota 2:ec149525ec2a 21 ROCATION Target;
TakushimaYukimasa 0:9e851dc42cde 22
TakushimaYukimasa 0:9e851dc42cde 23 /* 定数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 24 /* マクロ定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 25 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 26
TakushimaYukimasa 0:9e851dc42cde 27 /* タイマ呼び出し用 */
TakushimaYukimasa 0:9e851dc42cde 28 void IT_CallBack(void);
TakushimaYukimasa 0:9e851dc42cde 29
TakushimaYukimasa 0:9e851dc42cde 30 /* 自己位置推定処理 */
TakushimaYukimasa 0:9e851dc42cde 31 void LocEstimate(void);
TakushimaYukimasa 0:9e851dc42cde 32
yoshikawaryota 7:93ab5505ac1b 33 /* オムニ関係 */
yoshikawaryota 7:93ab5505ac1b 34 void SubOmuni(int X,int Y,int R);
yoshikawaryota 2:ec149525ec2a 35
yoshikawaryota 8:ddedee42c253 36 /* 台形制御値代入 */
yoshikawaryota 9:8c550d496736 37 int omuni_control(int tar_x, int tar_y, int tar_theta, int error);
TakushimaYukimasa 0:9e851dc42cde 38
yoshikawaryota 13:e4de7896f808 39 /* 移動する点を探す */
yoshikawaryota 13:e4de7896f808 40 int lncl(double *pt1, double *pt2, double *xyr, double *xy);
yoshikawaryota 13:e4de7896f808 41
TakushimaYukimasa 0:9e851dc42cde 42 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 43
TakushimaYukimasa 0:9e851dc42cde 44 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 45 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 46
TakushimaYukimasa 0:9e851dc42cde 47 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 48 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 49
TakushimaYukimasa 0:9e851dc42cde 50 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 51 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 52
TakushimaYukimasa 0:9e851dc42cde 53 /* 足回り値保存変数 */
yoshikawaryota 2:ec149525ec2a 54 int MovMotor[4]= {0};
TakushimaYukimasa 0:9e851dc42cde 55
TakushimaYukimasa 0:9e851dc42cde 56 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 57 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 58
yoshikawaryota 1:b9f698be841c 59 /* 補正値用定数 */
yoshikawaryota 1:b9f698be841c 60 int cor=4;
yoshikawaryota 1:b9f698be841c 61
TakushimaYukimasa 0:9e851dc42cde 62 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 63
TakushimaYukimasa 0:9e851dc42cde 64 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 65 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 66
yoshikawaryota 1:b9f698be841c 67 /* gyro用タイマ */
yoshikawaryota 1:b9f698be841c 68 Timer yawCnt;
yoshikawaryota 1:b9f698be841c 69
yoshikawaryota 2:ec149525ec2a 70 /* P制御終了タイマ */
yoshikawaryota 2:ec149525ec2a 71 Timer P_fin;
yoshikawaryota 2:ec149525ec2a 72
yoshikawaryota 7:93ab5505ac1b 73 /* タイマ加速 */
yoshikawaryota 7:93ab5505ac1b 74 Timer Acc_time;
yoshikawaryota 7:93ab5505ac1b 75
TakushimaYukimasa 0:9e851dc42cde 76 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 77 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 78
TakushimaYukimasa 0:9e851dc42cde 79 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 80 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 81
TakushimaYukimasa 0:9e851dc42cde 82 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 83 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 84
TakushimaYukimasa 0:9e851dc42cde 85 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 86 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 87
TakushimaYukimasa 0:9e851dc42cde 88 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 89 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 90 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 91 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 92
TakushimaYukimasa 0:9e851dc42cde 93 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 94 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 95
TakushimaYukimasa 0:9e851dc42cde 96 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 97 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 98
TakushimaYukimasa 0:9e851dc42cde 99 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 100 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 101
TakushimaYukimasa 0:9e851dc42cde 102
TakushimaYukimasa 0:9e851dc42cde 103 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 104 int main()
TakushimaYukimasa 0:9e851dc42cde 105 {
yoshikawaryota 2:ec149525ec2a 106 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 107
yoshikawaryota 2:ec149525ec2a 108 /* 割り込みの設定
yoshikawaryota 2:ec149525ec2a 109 * IT_CallBack関数を0.1msで割り込み */
yoshikawaryota 2:ec149525ec2a 110 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 111
yoshikawaryota 2:ec149525ec2a 112 /* ジャイロの設定 */
yoshikawaryota 2:ec149525ec2a 113 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 114
yoshikawaryota 2:ec149525ec2a 115 /* I2CMDの設定 */
yoshikawaryota 2:ec149525ec2a 116 MD.Init(0,MD_SMB);
yoshikawaryota 2:ec149525ec2a 117 MD.Init(1,MD_SMB);
yoshikawaryota 2:ec149525ec2a 118 MD.Init(2,MD_SMB);
yoshikawaryota 2:ec149525ec2a 119 MD.Init(3,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 120
yoshikawaryota 2:ec149525ec2a 121 telemetry.printf("\n\rMainLoopStart");
yoshikawaryota 2:ec149525ec2a 122 /* メインループ --------------------------------------------------------------*/
yoshikawaryota 2:ec149525ec2a 123 while(1) {
yoshikawaryota 2:ec149525ec2a 124 /* オンボードLED点滅 */
yoshikawaryota 2:ec149525ec2a 125 led=!led;
TakushimaYukimasa 0:9e851dc42cde 126
yoshikawaryota 2:ec149525ec2a 127 /* 表示改行 */
yoshikawaryota 2:ec149525ec2a 128 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 129
yoshikawaryota 2:ec149525ec2a 130 /* 自動処理関連テレメトリ */
yoshikawaryota 2:ec149525ec2a 131 telemetry.printf("ope:%d ",operate);
yoshikawaryota 2:ec149525ec2a 132 /* 座標テレメトリ */
yoshikawaryota 2:ec149525ec2a 133 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 134
yoshikawaryota 2:ec149525ec2a 135 /* 自己位置推定更新 */
yoshikawaryota 2:ec149525ec2a 136 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 137
yoshikawaryota 7:93ab5505ac1b 138 for(int i=0; i<4; i++) {
yoshikawaryota 7:93ab5505ac1b 139 MD.Set(i,MovMotor[i]);
yoshikawaryota 7:93ab5505ac1b 140 MovMotor[i]=0;
yoshikawaryota 7:93ab5505ac1b 141 }
yoshikawaryota 2:ec149525ec2a 142 /* I2CMD実行 */
yoshikawaryota 2:ec149525ec2a 143 MD.Exe();
yoshikawaryota 2:ec149525ec2a 144
yoshikawaryota 2:ec149525ec2a 145 /* タイマ-スタート */
yoshikawaryota 2:ec149525ec2a 146 yawCnt.start();
yoshikawaryota 2:ec149525ec2a 147 /* タイマーリセット */
yoshikawaryota 2:ec149525ec2a 148 yawCnt.reset();
TakushimaYukimasa 0:9e851dc42cde 149
TakushimaYukimasa 0:9e851dc42cde 150
yoshikawaryota 2:ec149525ec2a 151 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshikawaryota 2:ec149525ec2a 152 if(DS3.CROSS) operate=0;
yoshikawaryota 2:ec149525ec2a 153 if(DS3.TRIANGLE) operate=1;
yoshikawaryota 2:ec149525ec2a 154 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 155
yoshikawaryota 2:ec149525ec2a 156 /* 操縦権：なし 停止動作 */
yoshikawaryota 2:ec149525ec2a 157 if(operate==0) {
yoshikawaryota 2:ec149525ec2a 158 /* 足回り停止 */
yoshikawaryota 2:ec149525ec2a 159 omuni.XmarkOmni_Move(0,0,0);
yoshikawaryota 2:ec149525ec2a 160 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 161 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 162 }
TakushimaYukimasa 0:9e851dc42cde 163
yoshikawaryota 2:ec149525ec2a 164 }
yoshikawaryota 2:ec149525ec2a 165 /* 操縦権：手動 */
yoshikawaryota 2:ec149525ec2a 166 else if(operate==1) {
yoshikawaryota 2:ec149525ec2a 167 /* 足回り手動動作 */
yoshikawaryota 2:ec149525ec2a 168 int x_val = (double)(DS3.LX-64)*100/64;
yoshikawaryota 2:ec149525ec2a 169 int y_val = (double)(64-DS3.LY)*100/64;
yoshikawaryota 2:ec149525ec2a 170 int r_val = (double)(DS3.RX-64)*100/64;
yoshikawaryota 1:b9f698be841c 171
yoshikawaryota 2:ec149525ec2a 172 /* 目標角更新 */
yoshikawaryota 2:ec149525ec2a 173 if(DS3.RX!=64) yawCnt.reset();
yoshikawaryota 2:ec149525ec2a 174 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshikawaryota 2:ec149525ec2a 175
yoshikawaryota 2:ec149525ec2a 176 /* gyro値による補正 */
yoshikawaryota 2:ec149525ec2a 177 r_val += (TarTheta-NowLoc.theta)*cor;
yoshikawaryota 2:ec149525ec2a 178
yoshikawaryota 7:93ab5505ac1b 179 SubOmuni(x_val, y_val, r_val);
TakushimaYukimasa 0:9e851dc42cde 180
yoshikawaryota 2:ec149525ec2a 181 }
yoshikawaryota 2:ec149525ec2a 182 /* 操縦権：自動 */
yoshikawaryota 2:ec149525ec2a 183 else if(operate==2) {
yoshikawaryota 2:ec149525ec2a 184 switch(auto_mode) {
yoshikawaryota 2:ec149525ec2a 185 /* スタート待機処理 */
yoshikawaryota 2:ec149525ec2a 186 case 0:
yoshikawaryota 2:ec149525ec2a 187 /* オンボードSWで次のステップに */
yoshikawaryota 2:ec149525ec2a 188 if(UB) auto_mode++;
yoshikawaryota 2:ec149525ec2a 189 break;
TakushimaYukimasa 0:9e851dc42cde 190
yoshikawaryota 12:1daa4d490a12 191 /* 〇〇の処理(青ゾーン) */
yoshikawaryota 2:ec149525ec2a 192 case 1:
yoshikawaryota 12:1daa4d490a12 193 if(omuni_control(1950, 1250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 194 break;
yoshikawaryota 12:1daa4d490a12 195 case 2:
yoshikawaryota 12:1daa4d490a12 196 if(omuni_control(1950, 2750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 197 break;
yoshikawaryota 12:1daa4d490a12 198 case 3:
yoshikawaryota 12:1daa4d490a12 199 if(omuni_control(510, 2750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 200 break;
yoshikawaryota 12:1daa4d490a12 201 case 4:
yoshikawaryota 12:1daa4d490a12 202 if(omuni_control(510, 4250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 203 break;
yoshikawaryota 12:1daa4d490a12 204 case 5:
yoshikawaryota 12:1daa4d490a12 205 if(omuni_control(1950, 4250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 206 break;
yoshikawaryota 12:1daa4d490a12 207 case 6:
yoshikawaryota 12:1daa4d490a12 208 if(omuni_control(1950, 5750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 209 break;
yoshikawaryota 12:1daa4d490a12 210 case 7:
yoshikawaryota 12:1daa4d490a12 211 if(omuni_control(1210, 5750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 212 break;
yoshikawaryota 12:1daa4d490a12 213 case 8:
yoshikawaryota 12:1daa4d490a12 214 if(omuni_control(1210, 9000, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 215 break;
yoshikawaryota 12:1daa4d490a12 216 case 9:
yoshikawaryota 12:1daa4d490a12 217 if(omuni_control(5515, 9000, 0, 100) == 1) auto_mode++;
yoshikawaryota 2:ec149525ec2a 218 break;
TakushimaYukimasa 0:9e851dc42cde 219
yoshikawaryota 12:1daa4d490a12 220 /* 〇〇の処理(赤ゾーン) */
yoshikawaryota 12:1daa4d490a12 221 /*case 1:
yoshikawaryota 12:1daa4d490a12 222 if(omuni_control(-1950, 1250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 223 break;
yoshikawaryota 12:1daa4d490a12 224 case 2:
yoshikawaryota 12:1daa4d490a12 225 if(omuni_control(-1950, 2750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 226 break;
yoshikawaryota 12:1daa4d490a12 227 case 3:
yoshikawaryota 12:1daa4d490a12 228 if(omuni_control(-510, 2750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 229 break;
yoshikawaryota 12:1daa4d490a12 230 case 4:
yoshikawaryota 12:1daa4d490a12 231 if(omuni_control(-510, 4250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 232 break;
yoshikawaryota 12:1daa4d490a12 233 case 5:
yoshikawaryota 12:1daa4d490a12 234 if(omuni_control(-1950, 4250, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 235 break;
yoshikawaryota 12:1daa4d490a12 236 case 6:
yoshikawaryota 12:1daa4d490a12 237 if(omuni_control(-1950, 5750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 238 break;
yoshikawaryota 12:1daa4d490a12 239 case 7:
yoshikawaryota 12:1daa4d490a12 240 if(omuni_control(-1210, 5750, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 241 break;
yoshikawaryota 12:1daa4d490a12 242 case 8:
yoshikawaryota 12:1daa4d490a12 243 if(omuni_control(-1210, 9000, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 244 break;
yoshikawaryota 12:1daa4d490a12 245 case 9:
yoshikawaryota 12:1daa4d490a12 246 if(omuni_control(-5515, 9000, 0, 100) == 1) auto_mode++;
yoshikawaryota 12:1daa4d490a12 247 break;
yoshikawaryota 12:1daa4d490a12 248 */
yoshikawaryota 12:1daa4d490a12 249
yoshikawaryota 2:ec149525ec2a 250 /* 終了処理 */
yoshikawaryota 2:ec149525ec2a 251 default:
yoshikawaryota 2:ec149525ec2a 252 auto_mode=0;
yoshikawaryota 2:ec149525ec2a 253 operate=0;
yoshikawaryota 2:ec149525ec2a 254 break;
yoshikawaryota 2:ec149525ec2a 255 }
yoshikawaryota 2:ec149525ec2a 256 }
yoshikawaryota 2:ec149525ec2a 257
TakushimaYukimasa 0:9e851dc42cde 258 }
TakushimaYukimasa 0:9e851dc42cde 259 }
TakushimaYukimasa 0:9e851dc42cde 260
TakushimaYukimasa 0:9e851dc42cde 261
TakushimaYukimasa 0:9e851dc42cde 262 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 263 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 264 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 265 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 266 *******************************************************************************/
yoshikawaryota 2:ec149525ec2a 267 void LocEstimate(void)
yoshikawaryota 2:ec149525ec2a 268 {
yoshikawaryota 2:ec149525ec2a 269 static double GyroDeg[2]= {0};
yoshikawaryota 2:ec149525ec2a 270 static double EncDeg[2][2]= {0};
yoshikawaryota 2:ec149525ec2a 271 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 272
yoshikawaryota 7:93ab5505ac1b 273
yoshikawaryota 7:93ab5505ac1b 274
yoshikawaryota 2:ec149525ec2a 275 /* ジャイロの値取得 */
yoshikawaryota 2:ec149525ec2a 276 bno.get_angles();
yoshikawaryota 2:ec149525ec2a 277 GyroDeg[1]=GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 278 GyroDeg[0]=bno.euler.yaw;
yoshikawaryota 2:ec149525ec2a 279 if(GyroDeg[0]!=0) {
yoshikawaryota 2:ec149525ec2a 280 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 281 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshikawaryota 2:ec149525ec2a 282 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 283 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshikawaryota 2:ec149525ec2a 284 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 285 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 286 }
yoshikawaryota 2:ec149525ec2a 287 /* Enc2つの差分求める */
yoshikawaryota 2:ec149525ec2a 288 for(int i=0; i<2; i++) {
yoshikawaryota 2:ec149525ec2a 289 EncDeg[i][1]=EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 290 EncDeg[i][0]=EncoderDeg[i];
yoshikawaryota 7:93ab5505ac1b 291 disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
yoshikawaryota 2:ec149525ec2a 292 }
yoshikawaryota 2:ec149525ec2a 293 /* 差分を加速度として保存 */
yoshikawaryota 2:ec149525ec2a 294 NowAcc.theta = disp[2];
yoshikawaryota 7:93ab5505ac1b 295 NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
yoshikawaryota 7:93ab5505ac1b 296 NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
yoshikawaryota 2:ec149525ec2a 297 /* 差分を累積して現在位置を保存 */
yoshikawaryota 2:ec149525ec2a 298 NowLoc.X += NowAcc.X;
yoshikawaryota 2:ec149525ec2a 299 NowLoc.Y += NowAcc.Y;
yoshikawaryota 2:ec149525ec2a 300 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 301 }
TakushimaYukimasa 0:9e851dc42cde 302
TakushimaYukimasa 0:9e851dc42cde 303
TakushimaYukimasa 0:9e851dc42cde 304
TakushimaYukimasa 0:9e851dc42cde 305
TakushimaYukimasa 0:9e851dc42cde 306 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 307 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 308 {
yoshikawaryota 2:ec149525ec2a 309 static int cnt = 0;
yoshikawaryota 2:ec149525ec2a 310 static int data[EncoderMAX] = {0};
yoshikawaryota 2:ec149525ec2a 311 static double EncDeg[EncoderMAX][2] = {0};
yoshikawaryota 2:ec149525ec2a 312
yoshikawaryota 2:ec149525ec2a 313 for(int i=0; i<EncoderMAX; i++)
yoshikawaryota 2:ec149525ec2a 314 switch(cnt) {
yoshikawaryota 2:ec149525ec2a 315 /* 最初の処理 */
yoshikawaryota 2:ec149525ec2a 316 case 0:
yoshikawaryota 2:ec149525ec2a 317 data[i] = 0;
yoshikawaryota 2:ec149525ec2a 318 CS[i] = 0;
yoshikawaryota 2:ec149525ec2a 319 CL[i] = 1;
yoshikawaryota 2:ec149525ec2a 320 break;
yoshikawaryota 2:ec149525ec2a 321 /* 最後の処理 */
yoshikawaryota 2:ec149525ec2a 322 case 25:
yoshikawaryota 2:ec149525ec2a 323 CS[i]=1;
yoshikawaryota 2:ec149525ec2a 324 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshikawaryota 2:ec149525ec2a 325 EncDeg[i][1] = EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 326 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshikawaryota 2:ec149525ec2a 327 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 328 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshikawaryota 2:ec149525ec2a 329 EncDeg[i][1] -= 360;
yoshikawaryota 2:ec149525ec2a 330 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 331 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshikawaryota 2:ec149525ec2a 332 EncDeg[i][1] += 360;
yoshikawaryota 2:ec149525ec2a 333 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 334 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshikawaryota 2:ec149525ec2a 335 break;
yoshikawaryota 2:ec149525ec2a 336 /* 通常の処理 */
yoshikawaryota 2:ec149525ec2a 337 default:
yoshikawaryota 2:ec149525ec2a 338 CL[i]=!CL[i];
yoshikawaryota 2:ec149525ec2a 339 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshikawaryota 2:ec149525ec2a 340 if(cnt != 1 && cnt % 2) {
yoshikawaryota 2:ec149525ec2a 341 data[i] |= (DO[i]==1);
yoshikawaryota 2:ec149525ec2a 342 data[i] = data[i] << 1;
yoshikawaryota 2:ec149525ec2a 343 }
yoshikawaryota 2:ec149525ec2a 344 break;
yoshikawaryota 2:ec149525ec2a 345 }
yoshikawaryota 2:ec149525ec2a 346 cnt++;
yoshikawaryota 2:ec149525ec2a 347 cnt%=26;
yoshikawaryota 2:ec149525ec2a 348 }
yoshikawaryota 2:ec149525ec2a 349
yoshikawaryota 2:ec149525ec2a 350
yoshikawaryota 2:ec149525ec2a 351
yoshikawaryota 2:ec149525ec2a 352
yoshikawaryota 7:93ab5505ac1b 353
yoshikawaryota 7:93ab5505ac1b 354 /*******************************************************************************
yoshikawaryota 7:93ab5505ac1b 355 * @概要 オムニの値計算する(加算式)
yoshikawaryota 7:93ab5505ac1b 356 * @引数 X,Y,Rotationそれぞれの操作量
yoshikawaryota 7:93ab5505ac1b 357 * @返り値 なし
yoshikawaryota 7:93ab5505ac1b 358 *******************************************************************************/
yoshikawaryota 7:93ab5505ac1b 359 void SubOmuni(int X,int Y,int R)
yoshikawaryota 7:93ab5505ac1b 360 {
yoshikawaryota 7:93ab5505ac1b 361 /* 入力を100%に制限 */
yoshikawaryota 7:93ab5505ac1b 362 X=Rest(X,100);
yoshikawaryota 7:93ab5505ac1b 363 Y=Rest(Y,100);
yoshikawaryota 7:93ab5505ac1b 364 R=Rest(R,100);
yoshikawaryota 7:93ab5505ac1b 365
yoshikawaryota 9:8c550d496736 366
yoshikawaryota 9:8c550d496736 367
yoshikawaryota 9:8c550d496736 368
yoshikawaryota 7:93ab5505ac1b 369 /* オムニ計算結果をtmpに */
yoshikawaryota 7:93ab5505ac1b 370 int tmp[4]= {0};
yoshikawaryota 9:8c550d496736 371
yoshikawaryota 9:8c550d496736 372 /* 一度データをtmpに保存 */
yoshikawaryota 9:8c550d496736 373 for(int i=0; i<4; i++)
yoshikawaryota 9:8c550d496736 374 tmp[i]=MovMotor[i];
yoshikawaryota 9:8c550d496736 375
yoshikawaryota 9:8c550d496736 376 /* オムニ計算 */
yoshikawaryota 7:93ab5505ac1b 377 omuni.XmarkOmni_Move(X,Y,R);
yoshikawaryota 7:93ab5505ac1b 378
yoshikawaryota 7:93ab5505ac1b 379 /* 計算結果を加算する */
yoshikawaryota 9:8c550d496736 380 for(int i=0; i<4; i++)
yoshikawaryota 9:8c550d496736 381 MovMotor[i]+=tmp[i];
yoshikawaryota 7:93ab5505ac1b 382 }
yoshikawaryota 7:93ab5505ac1b 383
yoshikawaryota 7:93ab5505ac1b 384
yoshikawaryota 7:93ab5505ac1b 385
yoshikawaryota 7:93ab5505ac1b 386
yoshikawaryota 7:93ab5505ac1b 387
yoshikawaryota 2:ec149525ec2a 388 /*******************************************************************************
yoshikawaryota 9:8c550d496736 389 * @概要 台形制御
yoshikawaryota 10:e19d1d12f0ed 390 * @引数 tar_x :目標のx座標
yoshikawaryota 10:e19d1d12f0ed 391 * @引数 tar_y :目標のy座標
yoshikawaryota 10:e19d1d12f0ed 392 * @引数 tar_theta:目標角
yoshikawaryota 12:1daa4d490a12 393 * @引数 error :次ステップに進む基準とする距離
yoshikawaryota 9:8c550d496736 394 * @返り値 0(継続) 1(移動完了)
yoshikawaryota 2:ec149525ec2a 395 *******************************************************************************/
yoshikawaryota 9:8c550d496736 396 int omuni_control(int tar_x, int tar_y, int tar_theta, int error)
yoshikawaryota 2:ec149525ec2a 397 {
yoshikawaryota 6:201e3de9777d 398 static int log_distance = 0;
yoshikawaryota 13:e4de7896f808 399 static int be_tar_x = NowLoc.X, be_tar_y = NowLoc.Y;
yoshikawaryota 6:201e3de9777d 400 static double log_theta = 0;
yoshikawaryota 6:201e3de9777d 401 static bool dis_flag = 0;
TakushimaYukimasa 0:9e851dc42cde 402
yoshikawaryota 4:05a6bda2e11f 403 if(dis_flag==0) {
yoshikawaryota 7:93ab5505ac1b 404 /* タイマー初期化 */
yoshikawaryota 7:93ab5505ac1b 405 Acc_time.reset();
yoshikawaryota 7:93ab5505ac1b 406 P_fin.reset();
yoshikawaryota 7:93ab5505ac1b 407 /* タイマー始動 */
yoshikawaryota 7:93ab5505ac1b 408 Acc_time.start();
yoshikawaryota 7:93ab5505ac1b 409 P_fin.start();
yoshikawaryota 6:201e3de9777d 410 /* 現在角度を記録しておく */
yoshikawaryota 6:201e3de9777d 411 log_theta = NowLoc.theta;
yoshikawaryota 4:05a6bda2e11f 412 /* 現在座標と目標座標との距離を記録しておく */
yoshikawaryota 4:05a6bda2e11f 413 log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 4:05a6bda2e11f 414 /* フラグ回収 */
yoshikawaryota 4:05a6bda2e11f 415 dis_flag=1;
yoshikawaryota 4:05a6bda2e11f 416 }
yoshikawaryota 2:ec149525ec2a 417
yoshikawaryota 13:e4de7896f808 418 int x_val = 0, x_dec_val = 0;
yoshikawaryota 13:e4de7896f808 419 int y_val = 0, y_dec_val = 0;
yoshikawaryota 4:05a6bda2e11f 420 int t_val = 0;
yoshikawaryota 4:05a6bda2e11f 421
yoshikawaryota 4:05a6bda2e11f 422 double theta_tar = 0.0;
yoshikawaryota 13:e4de7896f808 423 double start[2] = {0}, end[2] = {0}, circle[3] = {0}, P_tar[2] = {0};
yoshikawaryota 13:e4de7896f808 424
yoshikawaryota 13:e4de7896f808 425 /* 線の始点代入 */
yoshikawaryota 13:e4de7896f808 426 start[0] = be_tar_x;
yoshikawaryota 13:e4de7896f808 427 start[1] = be_tar_y;
yoshikawaryota 13:e4de7896f808 428 /* 線の終点代入 */
yoshikawaryota 13:e4de7896f808 429 end[0] = tar_x;
yoshikawaryota 13:e4de7896f808 430 end[1] = tar_y;
yoshikawaryota 13:e4de7896f808 431 /* 探査円の中心と半径代入 */
yoshikawaryota 13:e4de7896f808 432 circle[0] = NowLoc.X;
yoshikawaryota 13:e4de7896f808 433 circle[1] = NowLoc.Y;
yoshikawaryota 13:e4de7896f808 434 circle[2] = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
yoshikawaryota 13:e4de7896f808 435 /* 半径の制限 */
yoshikawaryota 13:e4de7896f808 436 circle[2] = Rest(circle[2], 100);
yoshikawaryota 13:e4de7896f808 437
yoshikawaryota 13:e4de7896f808 438 /* 目標点の探索、代入 */
yoshikawaryota 13:e4de7896f808 439 lncl(start, end, circle, P_tar);
yoshikawaryota 13:e4de7896f808 440
yoshikawaryota 7:93ab5505ac1b 441 /* 減速処理の値代入 */
yoshikawaryota 13:e4de7896f808 442 x_dec_val = ABS((P_tar[0] - NowLoc.X) * 0.06);
yoshikawaryota 13:e4de7896f808 443 y_dec_val = ABS((P_tar[1] - NowLoc.Y) * 0.06);
yoshikawaryota 12:1daa4d490a12 444 x_val = x_dec_val;
yoshikawaryota 12:1daa4d490a12 445 y_val = y_dec_val;
yoshikawaryota 9:8c550d496736 446
yoshikawaryota 4:05a6bda2e11f 447 /* 最低を決める */
yoshikawaryota 7:93ab5505ac1b 448 x_val = M_MAX(x_val, 20);
yoshikawaryota 7:93ab5505ac1b 449 y_val = M_MAX(y_val, 20);
yoshikawaryota 9:8c550d496736 450
yoshikawaryota 4:05a6bda2e11f 451 /* 100以内に収める */
yoshikawaryota 4:05a6bda2e11f 452 x_val = Rest(x_val, 100);
yoshikawaryota 4:05a6bda2e11f 453 y_val = Rest(y_val, 100);
yoshikawaryota 2:ec149525ec2a 454
yoshikawaryota 4:05a6bda2e11f 455 /* 正負を決める */
yoshikawaryota 13:e4de7896f808 456 if ((P_tar[0] - NowLoc.X) < 0)
yoshikawaryota 4:05a6bda2e11f 457 x_val *= -1;
yoshikawaryota 13:e4de7896f808 458 if ((P_tar[1] - NowLoc.Y) < 0)
yoshikawaryota 4:05a6bda2e11f 459 y_val *= -1;
yoshikawaryota 2:ec149525ec2a 460
yoshikawaryota 4:05a6bda2e11f 461 /* 残り距離計算 */
yoshikawaryota 4:05a6bda2e11f 462 int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 4:05a6bda2e11f 463 /* 移動量に応じたthetaを求める */
yoshikawaryota 8:ddedee42c253 464 theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta) + log_theta;
yoshikawaryota 4:05a6bda2e11f 465 /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
yoshikawaryota 4:05a6bda2e11f 466 t_val = (theta_tar - NowLoc.theta) * 1.5;
yoshikawaryota 3:b77f51108bf1 467
yoshikawaryota 4:05a6bda2e11f 468 t_val = Rest(t_val, 20);
yoshikawaryota 2:ec149525ec2a 469
yoshikawaryota 7:93ab5505ac1b 470 SubOmuni(x_val, y_val, t_val);
yoshikawaryota 2:ec149525ec2a 471
yoshikawaryota 4:05a6bda2e11f 472 /* 終了処理 */
yoshikawaryota 9:8c550d496736 473 if(error != 0 && error >= remain) {
yoshikawaryota 9:8c550d496736 474 be_tar_x = tar_x;
yoshikawaryota 9:8c550d496736 475 be_tar_y = tar_y;
yoshikawaryota 9:8c550d496736 476 return 1;
yoshikawaryota 9:8c550d496736 477 } else if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
yoshikawaryota 4:05a6bda2e11f 478 x_val = 0;
yoshikawaryota 4:05a6bda2e11f 479 y_val = 0;
yoshikawaryota 4:05a6bda2e11f 480 }
yoshikawaryota 4:05a6bda2e11f 481 if (x_val == 0 && y_val == 0)
yoshikawaryota 4:05a6bda2e11f 482 P_fin.start();
yoshikawaryota 4:05a6bda2e11f 483 else
yoshikawaryota 4:05a6bda2e11f 484 P_fin.reset();
yoshikawaryota 2:ec149525ec2a 485
yoshikawaryota 4:05a6bda2e11f 486 if (P_fin.read_ms() > 100) {
yoshikawaryota 4:05a6bda2e11f 487 /* タイマー停止 */
yoshikawaryota 4:05a6bda2e11f 488 P_fin.stop();
yoshikawaryota 4:05a6bda2e11f 489 /* フラグを建て直す */
yoshikawaryota 4:05a6bda2e11f 490 dis_flag=0;
yoshikawaryota 9:8c550d496736 491
yoshikawaryota 8:ddedee42c253 492 return 1;
yoshikawaryota 2:ec149525ec2a 493 }
yoshikawaryota 8:ddedee42c253 494 return 0;
yoshikawaryota 13:e4de7896f808 495 }
yoshikawaryota 13:e4de7896f808 496
yoshikawaryota 13:e4de7896f808 497
yoshikawaryota 13:e4de7896f808 498
yoshikawaryota 13:e4de7896f808 499
yoshikawaryota 13:e4de7896f808 500
yoshikawaryota 13:e4de7896f808 501 /*******************************************************************************
yoshikawaryota 13:e4de7896f808 502 * @概要 円と線の交点を求める
yoshikawaryota 13:e4de7896f808 503 * なんかもうわかんない．あんまコメントアウトは参考にしないように
yoshikawaryota 13:e4de7896f808 504 * @引数 *pt1 :直線始まりの座標配列の先頭アドレス
yoshikawaryota 13:e4de7896f808 505 * @引数 *pt2 :直線終わりの座標配列の先頭アドレス
yoshikawaryota 13:e4de7896f808 506 * @引数 *xyr :円のXY半径の座標配列の先頭アドレス
yoshikawaryota 13:e4de7896f808 507 * @引数 xy :直線と円の交点の座標配列の先頭アドレス
yoshikawaryota 13:e4de7896f808 508 * @返り値 交点の個数(-1…エラー)
yoshikawaryota 13:e4de7896f808 509 *******************************************************************************/
yoshikawaryota 13:e4de7896f808 510 int lncl(double *pt1, double *pt2, double *xyr, double *xy)
yoshikawaryota 13:e4de7896f808 511 {
yoshikawaryota 13:e4de7896f808 512 double root, factor, xo, yo, f, g, fsq, gsq, fgsq, xjo, yjo, a, b, c;
yoshikawaryota 13:e4de7896f808 513 double fygx, fxgy, t, fginv, t1, t2, x1, y1, x2, y2, sqdst1, sqdst2;
yoshikawaryota 13:e4de7896f808 514 double accy = 1.0E-15;
yoshikawaryota 13:e4de7896f808 515
yoshikawaryota 13:e4de7896f808 516
yoshikawaryota 13:e4de7896f808 517
yoshikawaryota 13:e4de7896f808 518
yoshikawaryota 13:e4de7896f808 519 /* X,Yそれぞれの変化量 */
yoshikawaryota 13:e4de7896f808 520 double xlk = pt2[0] - pt1[0];
yoshikawaryota 13:e4de7896f808 521 double ylk = pt2[1] - pt1[1];
yoshikawaryota 13:e4de7896f808 522 /* 偏差 */
yoshikawaryota 13:e4de7896f808 523 double rsq = sqrt(pow(xlk, 2.0) + pow(ylk, 2.0));
yoshikawaryota 13:e4de7896f808 524
yoshikawaryota 13:e4de7896f808 525
yoshikawaryota 13:e4de7896f808 526
yoshikawaryota 13:e4de7896f808 527 /* 0の除算時異常終了 */
yoshikawaryota 13:e4de7896f808 528 if (rsq < accy) return (-1);
yoshikawaryota 13:e4de7896f808 529 double rinv = 1.0 / rsq;
yoshikawaryota 13:e4de7896f808 530
yoshikawaryota 13:e4de7896f808 531
yoshikawaryota 13:e4de7896f808 532
yoshikawaryota 13:e4de7896f808 533 /* ax+by+cの式の係数をそれぞれ */
yoshikawaryota 13:e4de7896f808 534 a = -ylk * rinv;
yoshikawaryota 13:e4de7896f808 535 b = xlk * rinv;
yoshikawaryota 13:e4de7896f808 536 c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv;
yoshikawaryota 13:e4de7896f808 537
yoshikawaryota 13:e4de7896f808 538
yoshikawaryota 13:e4de7896f808 539
yoshikawaryota 13:e4de7896f808 540 /* rootは1になる */
yoshikawaryota 13:e4de7896f808 541 root = 1.0 / (a * a + b * b);
yoshikawaryota 13:e4de7896f808 542 /* 直線の法線ベクトルと直線上の点の位置ベクトルとの内積 */
yoshikawaryota 13:e4de7896f808 543 factor = -c * root;
yoshikawaryota 13:e4de7896f808 544 xo = a * factor;
yoshikawaryota 13:e4de7896f808 545 yo = b * factor;
yoshikawaryota 13:e4de7896f808 546 root = sqrt(root);
yoshikawaryota 13:e4de7896f808 547 /* xについての関数fとyについてg */
yoshikawaryota 13:e4de7896f808 548 f = b * root;
yoshikawaryota 13:e4de7896f808 549 g = -a * root;
yoshikawaryota 13:e4de7896f808 550 /* 2乗バージョン */
yoshikawaryota 13:e4de7896f808 551 fsq = f * f;
yoshikawaryota 13:e4de7896f808 552 gsq = g * g;
yoshikawaryota 13:e4de7896f808 553 /* 2つの合成 */
yoshikawaryota 13:e4de7896f808 554 fgsq = fsq + gsq;
yoshikawaryota 13:e4de7896f808 555
yoshikawaryota 13:e4de7896f808 556
yoshikawaryota 13:e4de7896f808 557
yoshikawaryota 13:e4de7896f808 558
yoshikawaryota 13:e4de7896f808 559 xjo = xyr[0] - xo;
yoshikawaryota 13:e4de7896f808 560 yjo = xyr[1] - yo;
yoshikawaryota 13:e4de7896f808 561 fygx = f * yjo - g * xjo;
yoshikawaryota 13:e4de7896f808 562 root = xyr[2] * xyr[2] * fgsq - fygx * fygx;
yoshikawaryota 13:e4de7896f808 563
yoshikawaryota 13:e4de7896f808 564
yoshikawaryota 13:e4de7896f808 565
yoshikawaryota 13:e4de7896f808 566 /* 2つの関数が近い 3点で接する時異常終了 */
yoshikawaryota 13:e4de7896f808 567 if (fgsq < accy) return (3);
yoshikawaryota 13:e4de7896f808 568 /* 円と線が接さない 0点で接する時異常終了 */
yoshikawaryota 13:e4de7896f808 569 else if (root < -accy) return (-1);
yoshikawaryota 13:e4de7896f808 570 /* 円と線が正接 1点で接する */
yoshikawaryota 13:e4de7896f808 571 else if (root < accy) {
yoshikawaryota 13:e4de7896f808 572 fxgy = f * xjo + g * yjo;
yoshikawaryota 13:e4de7896f808 573 t = fxgy / fgsq;
yoshikawaryota 13:e4de7896f808 574 xy[0] = xo + f * t;
yoshikawaryota 13:e4de7896f808 575 xy[1] = yo + g * t;
yoshikawaryota 13:e4de7896f808 576 return (1);
yoshikawaryota 13:e4de7896f808 577 }
yoshikawaryota 13:e4de7896f808 578 /* 円と線が交差する 2点で接する */
yoshikawaryota 13:e4de7896f808 579 else {
yoshikawaryota 13:e4de7896f808 580 fxgy = f * xjo + g * yjo;
yoshikawaryota 13:e4de7896f808 581 root = sqrt(root);
yoshikawaryota 13:e4de7896f808 582 fginv = 1.0 / fgsq;
yoshikawaryota 13:e4de7896f808 583 /* 根号の+-の2通り求める */
yoshikawaryota 13:e4de7896f808 584 t1 = (fxgy - root)*fginv;
yoshikawaryota 13:e4de7896f808 585 t2 = (fxgy + root)*fginv;
yoshikawaryota 13:e4de7896f808 586 x1 = xo + f * t1;
yoshikawaryota 13:e4de7896f808 587 y1 = yo + g * t1;
yoshikawaryota 13:e4de7896f808 588 x2 = xo + f * t2;
yoshikawaryota 13:e4de7896f808 589 y2 = yo + g * t2;
yoshikawaryota 13:e4de7896f808 590 /*終点と距離をとる √は省略 */
yoshikawaryota 13:e4de7896f808 591 sqdst1 = pow((pt2[0] - x1), 2.0) + pow((pt2[1] - y1), 2.0);
yoshikawaryota 13:e4de7896f808 592 sqdst2 = pow((pt2[0] - x2), 2.0) + pow((pt2[1] - y2), 2.0);
yoshikawaryota 13:e4de7896f808 593 /* 終点までの距離が近い方を採用 */
yoshikawaryota 13:e4de7896f808 594 if (sqdst1 < sqdst2) {
yoshikawaryota 13:e4de7896f808 595 xy[0] = x1;
yoshikawaryota 13:e4de7896f808 596 xy[1] = y1;
yoshikawaryota 13:e4de7896f808 597 }
yoshikawaryota 13:e4de7896f808 598 else {
yoshikawaryota 13:e4de7896f808 599 xy[0] = x2;
yoshikawaryota 13:e4de7896f808 600 xy[1] = y2;
yoshikawaryota 13:e4de7896f808 601 }
yoshikawaryota 13:e4de7896f808 602 return (2);
yoshikawaryota 13:e4de7896f808 603 }
TakushimaYukimasa 0:9e851dc42cde 604 }