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@15:02c68793a266, 2020-03-26 (annotated)

Committer:
TakushimaYukimasa
Date:
Thu Mar 26 17:13:54 2020 +0000
Revision:
15:02c68793a266
Parent:
14:4c49218bb9fb 
update OEI&main; Ver.Takushima

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