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@6:201e3de9777d, 2020-03-18 (annotated)

Committer:
yoshikawaryota
Date:
Wed Mar 18 05:11:36 2020 +0000
Revision:
6:201e3de9777d
Parent:
5:1cf9b063e4fb
Child:
7:93ab5505ac1b
update P_control;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 2:ec149525ec2a 33 /* P制御値代入 */
yoshikawaryota 2:ec149525ec2a 34 void Pro_control(int tar_x, int tar_y, int tar_rev);
yoshikawaryota 2:ec149525ec2a 35
TakushimaYukimasa 0:9e851dc42cde 36
TakushimaYukimasa 0:9e851dc42cde 37 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 38
TakushimaYukimasa 0:9e851dc42cde 39 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 40 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 41
TakushimaYukimasa 0:9e851dc42cde 42 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 43 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 44
TakushimaYukimasa 0:9e851dc42cde 45 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 46 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 47
TakushimaYukimasa 0:9e851dc42cde 48 /* 足回り値保存変数 */
yoshikawaryota 2:ec149525ec2a 49 int MovMotor[4]= {0};
TakushimaYukimasa 0:9e851dc42cde 50
yoshikawaryota 6:201e3de9777d 51 /* 移動終了フラグ */
yoshikawaryota 6:201e3de9777d 52 bool fin_flag=0;
yoshikawaryota 4:05a6bda2e11f 53
TakushimaYukimasa 0:9e851dc42cde 54 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 55 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 56
yoshikawaryota 1:b9f698be841c 57 /* 補正値用定数 */
yoshikawaryota 1:b9f698be841c 58 int cor=4;
yoshikawaryota 1:b9f698be841c 59
TakushimaYukimasa 0:9e851dc42cde 60 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 61
TakushimaYukimasa 0:9e851dc42cde 62 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 63 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 64
yoshikawaryota 1:b9f698be841c 65 /* gyro用タイマ */
yoshikawaryota 1:b9f698be841c 66 Timer yawCnt;
yoshikawaryota 1:b9f698be841c 67
yoshikawaryota 2:ec149525ec2a 68 /* P制御終了タイマ */
yoshikawaryota 2:ec149525ec2a 69 Timer P_fin;
yoshikawaryota 2:ec149525ec2a 70
TakushimaYukimasa 0:9e851dc42cde 71 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 72 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 73
TakushimaYukimasa 0:9e851dc42cde 74 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 75 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 76
TakushimaYukimasa 0:9e851dc42cde 77 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 78 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 79
TakushimaYukimasa 0:9e851dc42cde 80 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 81 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 82
TakushimaYukimasa 0:9e851dc42cde 83 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 84 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 85 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 86 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 87
TakushimaYukimasa 0:9e851dc42cde 88 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 89 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 90
TakushimaYukimasa 0:9e851dc42cde 91 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 92 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 93
TakushimaYukimasa 0:9e851dc42cde 94 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 95 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 96
TakushimaYukimasa 0:9e851dc42cde 97
TakushimaYukimasa 0:9e851dc42cde 98 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 99 int main()
TakushimaYukimasa 0:9e851dc42cde 100 {
yoshikawaryota 2:ec149525ec2a 101 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 102
yoshikawaryota 2:ec149525ec2a 103 /* 割り込みの設定
yoshikawaryota 2:ec149525ec2a 104 * IT_CallBack関数を0.1msで割り込み */
yoshikawaryota 2:ec149525ec2a 105 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 106
yoshikawaryota 2:ec149525ec2a 107 /* ジャイロの設定 */
yoshikawaryota 2:ec149525ec2a 108 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 109
yoshikawaryota 2:ec149525ec2a 110 /* I2CMDの設定 */
yoshikawaryota 2:ec149525ec2a 111 MD.Init(0,MD_SMB);
yoshikawaryota 2:ec149525ec2a 112 MD.Init(1,MD_SMB);
yoshikawaryota 2:ec149525ec2a 113 MD.Init(2,MD_SMB);
yoshikawaryota 2:ec149525ec2a 114 MD.Init(3,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 115
yoshikawaryota 2:ec149525ec2a 116 telemetry.printf("\n\rMainLoopStart");
yoshikawaryota 2:ec149525ec2a 117 /* メインループ --------------------------------------------------------------*/
yoshikawaryota 2:ec149525ec2a 118 while(1) {
yoshikawaryota 2:ec149525ec2a 119 /* オンボードLED点滅 */
yoshikawaryota 2:ec149525ec2a 120 led=!led;
TakushimaYukimasa 0:9e851dc42cde 121
yoshikawaryota 2:ec149525ec2a 122 /* 表示改行 */
yoshikawaryota 2:ec149525ec2a 123 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 124
yoshikawaryota 2:ec149525ec2a 125 /* 自動処理関連テレメトリ */
yoshikawaryota 2:ec149525ec2a 126 telemetry.printf("ope:%d ",operate);
yoshikawaryota 2:ec149525ec2a 127 /* 座標テレメトリ */
yoshikawaryota 2:ec149525ec2a 128 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 129
yoshikawaryota 2:ec149525ec2a 130 /* 自己位置推定更新 */
yoshikawaryota 2:ec149525ec2a 131 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 132
yoshikawaryota 2:ec149525ec2a 133 /* I2CMD実行 */
yoshikawaryota 2:ec149525ec2a 134 MD.Exe();
yoshikawaryota 2:ec149525ec2a 135
yoshikawaryota 2:ec149525ec2a 136 /* タイマ-スタート */
yoshikawaryota 2:ec149525ec2a 137 yawCnt.start();
yoshikawaryota 2:ec149525ec2a 138 /* タイマーリセット */
yoshikawaryota 2:ec149525ec2a 139 yawCnt.reset();
TakushimaYukimasa 0:9e851dc42cde 140
TakushimaYukimasa 0:9e851dc42cde 141
yoshikawaryota 2:ec149525ec2a 142 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshikawaryota 2:ec149525ec2a 143 if(DS3.CROSS) operate=0;
yoshikawaryota 2:ec149525ec2a 144 if(DS3.TRIANGLE) operate=1;
yoshikawaryota 2:ec149525ec2a 145 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 146
yoshikawaryota 2:ec149525ec2a 147 /* 操縦権：なし 停止動作 */
yoshikawaryota 2:ec149525ec2a 148 if(operate==0) {
yoshikawaryota 2:ec149525ec2a 149 /* 足回り停止 */
yoshikawaryota 2:ec149525ec2a 150 omuni.XmarkOmni_Move(0,0,0);
yoshikawaryota 2:ec149525ec2a 151 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 152 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 153 }
TakushimaYukimasa 0:9e851dc42cde 154
yoshikawaryota 2:ec149525ec2a 155 }
yoshikawaryota 2:ec149525ec2a 156 /* 操縦権：手動 */
yoshikawaryota 2:ec149525ec2a 157 else if(operate==1) {
yoshikawaryota 2:ec149525ec2a 158 /* 足回り手動動作 */
yoshikawaryota 2:ec149525ec2a 159 int x_val = (double)(DS3.LX-64)*100/64;
yoshikawaryota 2:ec149525ec2a 160 int y_val = (double)(64-DS3.LY)*100/64;
yoshikawaryota 2:ec149525ec2a 161 int r_val = (double)(DS3.RX-64)*100/64;
yoshikawaryota 1:b9f698be841c 162
yoshikawaryota 2:ec149525ec2a 163 /* 目標角更新 */
yoshikawaryota 2:ec149525ec2a 164 if(DS3.RX!=64) yawCnt.reset();
yoshikawaryota 2:ec149525ec2a 165 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshikawaryota 2:ec149525ec2a 166
yoshikawaryota 2:ec149525ec2a 167 /* gyro値による補正 */
yoshikawaryota 2:ec149525ec2a 168 r_val += (TarTheta-NowLoc.theta)*cor;
yoshikawaryota 2:ec149525ec2a 169
yoshikawaryota 2:ec149525ec2a 170 omuni.XmarkOmni_Move(x_val,y_val,r_val);
yoshikawaryota 2:ec149525ec2a 171 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 172 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 173 }
TakushimaYukimasa 0:9e851dc42cde 174
yoshikawaryota 2:ec149525ec2a 175 }
yoshikawaryota 2:ec149525ec2a 176 /* 操縦権：自動 */
yoshikawaryota 2:ec149525ec2a 177 else if(operate==2) {
yoshikawaryota 2:ec149525ec2a 178 switch(auto_mode) {
yoshikawaryota 2:ec149525ec2a 179 /* スタート待機処理 */
yoshikawaryota 2:ec149525ec2a 180 case 0:
yoshikawaryota 2:ec149525ec2a 181 /* オンボードSWで次のステップに */
yoshikawaryota 2:ec149525ec2a 182 if(UB) auto_mode++;
yoshikawaryota 2:ec149525ec2a 183 break;
TakushimaYukimasa 0:9e851dc42cde 184
yoshikawaryota 2:ec149525ec2a 185 /* 〇〇の処理 */
yoshikawaryota 2:ec149525ec2a 186 case 1:
yoshikawaryota 6:201e3de9777d 187 Pro_control(1000, 1000, 360);
yoshikawaryota 2:ec149525ec2a 188 /* 〇〇の時次のステップに */
yoshikawaryota 6:201e3de9777d 189 if(fin_flag) {
yoshikawaryota 6:201e3de9777d 190 fin_flag=0;
yoshikawaryota 6:201e3de9777d 191 auto_mode++;
yoshikawaryota 6:201e3de9777d 192 }
yoshikawaryota 2:ec149525ec2a 193 break;
TakushimaYukimasa 0:9e851dc42cde 194
yoshikawaryota 2:ec149525ec2a 195 /* 終了処理 */
yoshikawaryota 2:ec149525ec2a 196 default:
yoshikawaryota 2:ec149525ec2a 197 auto_mode=0;
yoshikawaryota 2:ec149525ec2a 198 operate=0;
yoshikawaryota 2:ec149525ec2a 199 break;
yoshikawaryota 2:ec149525ec2a 200 }
yoshikawaryota 2:ec149525ec2a 201 }
yoshikawaryota 2:ec149525ec2a 202
TakushimaYukimasa 0:9e851dc42cde 203 }
TakushimaYukimasa 0:9e851dc42cde 204 }
TakushimaYukimasa 0:9e851dc42cde 205
TakushimaYukimasa 0:9e851dc42cde 206
TakushimaYukimasa 0:9e851dc42cde 207 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 208 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 209 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 210 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 211 *******************************************************************************/
yoshikawaryota 2:ec149525ec2a 212 void LocEstimate(void)
yoshikawaryota 2:ec149525ec2a 213 {
yoshikawaryota 2:ec149525ec2a 214 static double GyroDeg[2]= {0};
yoshikawaryota 2:ec149525ec2a 215 static double EncDeg[2][2]= {0};
yoshikawaryota 2:ec149525ec2a 216 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 217
yoshikawaryota 2:ec149525ec2a 218 /* ジャイロの値取得 */
yoshikawaryota 2:ec149525ec2a 219 bno.get_angles();
yoshikawaryota 2:ec149525ec2a 220 GyroDeg[1]=GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 221 GyroDeg[0]=bno.euler.yaw;
yoshikawaryota 2:ec149525ec2a 222 if(GyroDeg[0]!=0) {
yoshikawaryota 2:ec149525ec2a 223 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 224 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshikawaryota 2:ec149525ec2a 225 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 226 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshikawaryota 2:ec149525ec2a 227 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 228 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 229 }
yoshikawaryota 2:ec149525ec2a 230 /* Enc2つの差分求める */
yoshikawaryota 2:ec149525ec2a 231 for(int i=0; i<2; i++) {
yoshikawaryota 2:ec149525ec2a 232 EncDeg[i][1]=EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 233 EncDeg[i][0]=EncoderDeg[i];
yoshikawaryota 2:ec149525ec2a 234 disp[i]=EncDeg[i][1]-EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 235 }
yoshikawaryota 2:ec149525ec2a 236 /* 差分を加速度として保存 */
yoshikawaryota 2:ec149525ec2a 237 NowAcc.theta = disp[2];
yoshikawaryota 2:ec149525ec2a 238 NowAcc.X = disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
yoshikawaryota 2:ec149525ec2a 239 NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
yoshikawaryota 2:ec149525ec2a 240 /* 差分を累積して現在位置を保存 */
yoshikawaryota 2:ec149525ec2a 241 NowLoc.X += NowAcc.X;
yoshikawaryota 2:ec149525ec2a 242 NowLoc.Y += NowAcc.Y;
yoshikawaryota 2:ec149525ec2a 243 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 244 }
TakushimaYukimasa 0:9e851dc42cde 245
TakushimaYukimasa 0:9e851dc42cde 246
TakushimaYukimasa 0:9e851dc42cde 247
TakushimaYukimasa 0:9e851dc42cde 248
TakushimaYukimasa 0:9e851dc42cde 249 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 250 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 251 {
yoshikawaryota 2:ec149525ec2a 252 static int cnt = 0;
yoshikawaryota 2:ec149525ec2a 253 static int data[EncoderMAX] = {0};
yoshikawaryota 2:ec149525ec2a 254 static double EncDeg[EncoderMAX][2] = {0};
yoshikawaryota 2:ec149525ec2a 255
yoshikawaryota 2:ec149525ec2a 256 for(int i=0; i<EncoderMAX; i++)
yoshikawaryota 2:ec149525ec2a 257 switch(cnt) {
yoshikawaryota 2:ec149525ec2a 258 /* 最初の処理 */
yoshikawaryota 2:ec149525ec2a 259 case 0:
yoshikawaryota 2:ec149525ec2a 260 data[i] = 0;
yoshikawaryota 2:ec149525ec2a 261 CS[i] = 0;
yoshikawaryota 2:ec149525ec2a 262 CL[i] = 1;
yoshikawaryota 2:ec149525ec2a 263 break;
yoshikawaryota 2:ec149525ec2a 264 /* 最後の処理 */
yoshikawaryota 2:ec149525ec2a 265 case 25:
yoshikawaryota 2:ec149525ec2a 266 CS[i]=1;
yoshikawaryota 2:ec149525ec2a 267 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshikawaryota 2:ec149525ec2a 268 EncDeg[i][1] = EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 269 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshikawaryota 2:ec149525ec2a 270 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 271 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshikawaryota 2:ec149525ec2a 272 EncDeg[i][1] -= 360;
yoshikawaryota 2:ec149525ec2a 273 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 274 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshikawaryota 2:ec149525ec2a 275 EncDeg[i][1] += 360;
yoshikawaryota 2:ec149525ec2a 276 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 277 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshikawaryota 2:ec149525ec2a 278 break;
yoshikawaryota 2:ec149525ec2a 279 /* 通常の処理 */
yoshikawaryota 2:ec149525ec2a 280 default:
yoshikawaryota 2:ec149525ec2a 281 CL[i]=!CL[i];
yoshikawaryota 2:ec149525ec2a 282 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshikawaryota 2:ec149525ec2a 283 if(cnt != 1 && cnt % 2) {
yoshikawaryota 2:ec149525ec2a 284 data[i] |= (DO[i]==1);
yoshikawaryota 2:ec149525ec2a 285 data[i] = data[i] << 1;
yoshikawaryota 2:ec149525ec2a 286 }
yoshikawaryota 2:ec149525ec2a 287 break;
yoshikawaryota 2:ec149525ec2a 288 }
yoshikawaryota 2:ec149525ec2a 289 cnt++;
yoshikawaryota 2:ec149525ec2a 290 cnt%=26;
yoshikawaryota 2:ec149525ec2a 291 }
yoshikawaryota 2:ec149525ec2a 292
yoshikawaryota 2:ec149525ec2a 293
yoshikawaryota 2:ec149525ec2a 294
yoshikawaryota 2:ec149525ec2a 295
yoshikawaryota 2:ec149525ec2a 296 /*******************************************************************************
yoshikawaryota 2:ec149525ec2a 297 * @概要 P制御
yoshikawaryota 2:ec149525ec2a 298 * @引数 tar_x :目標のx座標
yoshikawaryota 2:ec149525ec2a 299 * @引数 tar_y :目標のy座標
yoshikawaryota 2:ec149525ec2a 300 * @引数 tar_rev: 目標角
yoshikawaryota 2:ec149525ec2a 301 * @返り値 なし
yoshikawaryota 2:ec149525ec2a 302 *******************************************************************************/
yoshikawaryota 2:ec149525ec2a 303 void Pro_control(int tar_x, int tar_y, int tar_theta)
yoshikawaryota 2:ec149525ec2a 304 {
yoshikawaryota 6:201e3de9777d 305 static int log_distance = 0;
yoshikawaryota 6:201e3de9777d 306 static double log_theta = 0;
yoshikawaryota 6:201e3de9777d 307 static bool dis_flag = 0;
TakushimaYukimasa 0:9e851dc42cde 308
yoshikawaryota 4:05a6bda2e11f 309 if(dis_flag==0) {
yoshikawaryota 6:201e3de9777d 310 /* 現在角度を記録しておく */
yoshikawaryota 6:201e3de9777d 311 log_theta = NowLoc.theta;
yoshikawaryota 4:05a6bda2e11f 312 /* 現在座標と目標座標との距離を記録しておく */
yoshikawaryota 4:05a6bda2e11f 313 log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 4:05a6bda2e11f 314 /* フラグ回収 */
yoshikawaryota 4:05a6bda2e11f 315 dis_flag=1;
yoshikawaryota 4:05a6bda2e11f 316 }
yoshikawaryota 2:ec149525ec2a 317
yoshikawaryota 4:05a6bda2e11f 318 int x_val = 0;
yoshikawaryota 4:05a6bda2e11f 319 int y_val = 0;
yoshikawaryota 4:05a6bda2e11f 320 int t_val = 0;
yoshikawaryota 4:05a6bda2e11f 321
yoshikawaryota 4:05a6bda2e11f 322 double theta_tar = 0.0;
yoshikawaryota 2:ec149525ec2a 323
yoshikawaryota 4:05a6bda2e11f 324 x_val = ABS((tar_x - NowLoc.X) * 0.06);
yoshikawaryota 4:05a6bda2e11f 325 y_val = ABS((tar_y - NowLoc.Y) * 0.06);
yoshikawaryota 2:ec149525ec2a 326
yoshikawaryota 4:05a6bda2e11f 327 /* 最低を決める */
yoshikawaryota 4:05a6bda2e11f 328 if (x_val < 20)
yoshikawaryota 4:05a6bda2e11f 329 x_val = 20;
yoshikawaryota 4:05a6bda2e11f 330 if (y_val < 20)
yoshikawaryota 4:05a6bda2e11f 331 y_val = 20;
yoshikawaryota 4:05a6bda2e11f 332 /* 100以内に収める */
yoshikawaryota 4:05a6bda2e11f 333 x_val = Rest(x_val, 100);
yoshikawaryota 4:05a6bda2e11f 334 y_val = Rest(y_val, 100);
yoshikawaryota 2:ec149525ec2a 335
yoshikawaryota 4:05a6bda2e11f 336 /* 正負を決める */
yoshikawaryota 4:05a6bda2e11f 337 if ((tar_x - NowLoc.X) < 0)
yoshikawaryota 4:05a6bda2e11f 338 x_val *= -1;
yoshikawaryota 4:05a6bda2e11f 339 if ((tar_y - NowLoc.Y) < 0)
yoshikawaryota 4:05a6bda2e11f 340 y_val *= -1;
yoshikawaryota 2:ec149525ec2a 341
yoshikawaryota 4:05a6bda2e11f 342 /* 残り距離計算 */
yoshikawaryota 4:05a6bda2e11f 343 int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 4:05a6bda2e11f 344 /* 移動量に応じたthetaを求める */
yoshikawaryota 6:201e3de9777d 345 theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta);
yoshikawaryota 4:05a6bda2e11f 346 /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
yoshikawaryota 4:05a6bda2e11f 347 t_val = (theta_tar - NowLoc.theta) * 1.5;
yoshikawaryota 3:b77f51108bf1 348
yoshikawaryota 4:05a6bda2e11f 349 t_val = Rest(t_val, 20);
yoshikawaryota 2:ec149525ec2a 350
yoshikawaryota 4:05a6bda2e11f 351 omuni.XmarkOmni_Move(x_val,y_val,t_val);
yoshikawaryota 2:ec149525ec2a 352
yoshikawaryota 2:ec149525ec2a 353
yoshikawaryota 4:05a6bda2e11f 354 /* 終了処理 */
yoshikawaryota 4:05a6bda2e11f 355 if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
yoshikawaryota 4:05a6bda2e11f 356 x_val = 0;
yoshikawaryota 4:05a6bda2e11f 357 y_val = 0;
yoshikawaryota 4:05a6bda2e11f 358 }
yoshikawaryota 4:05a6bda2e11f 359 if (x_val == 0 && y_val == 0)
yoshikawaryota 4:05a6bda2e11f 360 P_fin.start();
yoshikawaryota 4:05a6bda2e11f 361 else
yoshikawaryota 4:05a6bda2e11f 362 P_fin.reset();
yoshikawaryota 2:ec149525ec2a 363
yoshikawaryota 4:05a6bda2e11f 364 if (P_fin.read_ms() > 100) {
yoshikawaryota 4:05a6bda2e11f 365 /* タイマー停止 */
yoshikawaryota 4:05a6bda2e11f 366 P_fin.stop();
yoshikawaryota 4:05a6bda2e11f 367 P_fin.reset();
yoshikawaryota 4:05a6bda2e11f 368 /* フラグを建て直す */
yoshikawaryota 4:05a6bda2e11f 369 dis_flag=0;
yoshikawaryota 6:201e3de9777d 370 fin_flag=1;
yoshikawaryota 2:ec149525ec2a 371 }
TakushimaYukimasa 0:9e851dc42cde 372 }