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@11:80b1daeb243f, 2020-03-23 (annotated)

Committer:
yoshikawaryota
Date:
Mon Mar 23 07:39:52 2020 +0000
Revision:
11:80b1daeb243f
Parent:
10:e19d1d12f0ed
Child:
12:1daa4d490a12
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
TakushimaYukimasa 0:9e851dc42cde 39 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 40
TakushimaYukimasa 0:9e851dc42cde 41 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 42 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 43
TakushimaYukimasa 0:9e851dc42cde 44 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 45 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 46
TakushimaYukimasa 0:9e851dc42cde 47 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 48 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 49
TakushimaYukimasa 0:9e851dc42cde 50 /* 足回り値保存変数 */
yoshikawaryota 2:ec149525ec2a 51 int MovMotor[4]= {0};
TakushimaYukimasa 0:9e851dc42cde 52
TakushimaYukimasa 0:9e851dc42cde 53 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 54 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 55
yoshikawaryota 1:b9f698be841c 56 /* 補正値用定数 */
yoshikawaryota 1:b9f698be841c 57 int cor=4;
yoshikawaryota 1:b9f698be841c 58
TakushimaYukimasa 0:9e851dc42cde 59 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 60
TakushimaYukimasa 0:9e851dc42cde 61 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 62 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 63
yoshikawaryota 1:b9f698be841c 64 /* gyro用タイマ */
yoshikawaryota 1:b9f698be841c 65 Timer yawCnt;
yoshikawaryota 1:b9f698be841c 66
yoshikawaryota 2:ec149525ec2a 67 /* P制御終了タイマ */
yoshikawaryota 2:ec149525ec2a 68 Timer P_fin;
yoshikawaryota 2:ec149525ec2a 69
yoshikawaryota 7:93ab5505ac1b 70 /* タイマ加速 */
yoshikawaryota 7:93ab5505ac1b 71 Timer Acc_time;
yoshikawaryota 7:93ab5505ac1b 72
TakushimaYukimasa 0:9e851dc42cde 73 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 74 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 75
TakushimaYukimasa 0:9e851dc42cde 76 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 77 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 78
TakushimaYukimasa 0:9e851dc42cde 79 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 80 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 81
TakushimaYukimasa 0:9e851dc42cde 82 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 83 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 84
TakushimaYukimasa 0:9e851dc42cde 85 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 86 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 87 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 88 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 89
TakushimaYukimasa 0:9e851dc42cde 90 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 91 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 92
TakushimaYukimasa 0:9e851dc42cde 93 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 94 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 95
TakushimaYukimasa 0:9e851dc42cde 96 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 97 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 98
TakushimaYukimasa 0:9e851dc42cde 99
TakushimaYukimasa 0:9e851dc42cde 100 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 101 int main()
TakushimaYukimasa 0:9e851dc42cde 102 {
yoshikawaryota 2:ec149525ec2a 103 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 104
yoshikawaryota 2:ec149525ec2a 105 /* 割り込みの設定
yoshikawaryota 2:ec149525ec2a 106 * IT_CallBack関数を0.1msで割り込み */
yoshikawaryota 2:ec149525ec2a 107 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 108
yoshikawaryota 2:ec149525ec2a 109 /* ジャイロの設定 */
yoshikawaryota 2:ec149525ec2a 110 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 111
yoshikawaryota 2:ec149525ec2a 112 /* I2CMDの設定 */
yoshikawaryota 2:ec149525ec2a 113 MD.Init(0,MD_SMB);
yoshikawaryota 2:ec149525ec2a 114 MD.Init(1,MD_SMB);
yoshikawaryota 2:ec149525ec2a 115 MD.Init(2,MD_SMB);
yoshikawaryota 2:ec149525ec2a 116 MD.Init(3,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 117
yoshikawaryota 2:ec149525ec2a 118 telemetry.printf("\n\rMainLoopStart");
yoshikawaryota 2:ec149525ec2a 119 /* メインループ --------------------------------------------------------------*/
yoshikawaryota 2:ec149525ec2a 120 while(1) {
yoshikawaryota 2:ec149525ec2a 121 /* オンボードLED点滅 */
yoshikawaryota 2:ec149525ec2a 122 led=!led;
TakushimaYukimasa 0:9e851dc42cde 123
yoshikawaryota 2:ec149525ec2a 124 /* 表示改行 */
yoshikawaryota 2:ec149525ec2a 125 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 126
yoshikawaryota 2:ec149525ec2a 127 /* 自動処理関連テレメトリ */
yoshikawaryota 2:ec149525ec2a 128 telemetry.printf("ope:%d ",operate);
yoshikawaryota 2:ec149525ec2a 129 /* 座標テレメトリ */
yoshikawaryota 2:ec149525ec2a 130 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 131
yoshikawaryota 2:ec149525ec2a 132 /* 自己位置推定更新 */
yoshikawaryota 2:ec149525ec2a 133 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 134
yoshikawaryota 7:93ab5505ac1b 135 for(int i=0; i<4; i++) {
yoshikawaryota 7:93ab5505ac1b 136 MD.Set(i,MovMotor[i]);
yoshikawaryota 7:93ab5505ac1b 137 MovMotor[i]=0;
yoshikawaryota 7:93ab5505ac1b 138 }
yoshikawaryota 2:ec149525ec2a 139 /* I2CMD実行 */
yoshikawaryota 2:ec149525ec2a 140 MD.Exe();
yoshikawaryota 2:ec149525ec2a 141
yoshikawaryota 2:ec149525ec2a 142 /* タイマ-スタート */
yoshikawaryota 2:ec149525ec2a 143 yawCnt.start();
yoshikawaryota 2:ec149525ec2a 144 /* タイマーリセット */
yoshikawaryota 2:ec149525ec2a 145 yawCnt.reset();
TakushimaYukimasa 0:9e851dc42cde 146
TakushimaYukimasa 0:9e851dc42cde 147
yoshikawaryota 2:ec149525ec2a 148 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshikawaryota 2:ec149525ec2a 149 if(DS3.CROSS) operate=0;
yoshikawaryota 2:ec149525ec2a 150 if(DS3.TRIANGLE) operate=1;
yoshikawaryota 2:ec149525ec2a 151 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 152
yoshikawaryota 2:ec149525ec2a 153 /* 操縦権：なし 停止動作 */
yoshikawaryota 2:ec149525ec2a 154 if(operate==0) {
yoshikawaryota 2:ec149525ec2a 155 /* 足回り停止 */
yoshikawaryota 2:ec149525ec2a 156 omuni.XmarkOmni_Move(0,0,0);
yoshikawaryota 2:ec149525ec2a 157 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 158 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 159 }
TakushimaYukimasa 0:9e851dc42cde 160
yoshikawaryota 2:ec149525ec2a 161 }
yoshikawaryota 2:ec149525ec2a 162 /* 操縦権：手動 */
yoshikawaryota 2:ec149525ec2a 163 else if(operate==1) {
yoshikawaryota 2:ec149525ec2a 164 /* 足回り手動動作 */
yoshikawaryota 2:ec149525ec2a 165 int x_val = (double)(DS3.LX-64)*100/64;
yoshikawaryota 2:ec149525ec2a 166 int y_val = (double)(64-DS3.LY)*100/64;
yoshikawaryota 2:ec149525ec2a 167 int r_val = (double)(DS3.RX-64)*100/64;
yoshikawaryota 1:b9f698be841c 168
yoshikawaryota 2:ec149525ec2a 169 /* 目標角更新 */
yoshikawaryota 2:ec149525ec2a 170 if(DS3.RX!=64) yawCnt.reset();
yoshikawaryota 2:ec149525ec2a 171 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshikawaryota 2:ec149525ec2a 172
yoshikawaryota 2:ec149525ec2a 173 /* gyro値による補正 */
yoshikawaryota 2:ec149525ec2a 174 r_val += (TarTheta-NowLoc.theta)*cor;
yoshikawaryota 2:ec149525ec2a 175
yoshikawaryota 7:93ab5505ac1b 176 SubOmuni(x_val, y_val, r_val);
TakushimaYukimasa 0:9e851dc42cde 177
yoshikawaryota 2:ec149525ec2a 178 }
yoshikawaryota 2:ec149525ec2a 179 /* 操縦権：自動 */
yoshikawaryota 2:ec149525ec2a 180 else if(operate==2) {
yoshikawaryota 2:ec149525ec2a 181 switch(auto_mode) {
yoshikawaryota 2:ec149525ec2a 182 /* スタート待機処理 */
yoshikawaryota 2:ec149525ec2a 183 case 0:
yoshikawaryota 2:ec149525ec2a 184 /* オンボードSWで次のステップに */
yoshikawaryota 2:ec149525ec2a 185 if(UB) auto_mode++;
yoshikawaryota 2:ec149525ec2a 186 break;
TakushimaYukimasa 0:9e851dc42cde 187
yoshikawaryota 2:ec149525ec2a 188 /* 〇〇の処理 */
yoshikawaryota 2:ec149525ec2a 189 case 1:
yoshikawaryota 2:ec149525ec2a 190 /* 〇〇の時次のステップに */
yoshikawaryota 9:8c550d496736 191 if(omuni_control(1000, 1000, 360, 100) == 1) auto_mode++;
yoshikawaryota 2:ec149525ec2a 192 break;
TakushimaYukimasa 0:9e851dc42cde 193
yoshikawaryota 2:ec149525ec2a 194 /* 終了処理 */
yoshikawaryota 2:ec149525ec2a 195 default:
yoshikawaryota 2:ec149525ec2a 196 auto_mode=0;
yoshikawaryota 2:ec149525ec2a 197 operate=0;
yoshikawaryota 2:ec149525ec2a 198 break;
yoshikawaryota 2:ec149525ec2a 199 }
yoshikawaryota 2:ec149525ec2a 200 }
yoshikawaryota 2:ec149525ec2a 201
TakushimaYukimasa 0:9e851dc42cde 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 *******************************************************************************/
yoshikawaryota 2:ec149525ec2a 211 void LocEstimate(void)
yoshikawaryota 2:ec149525ec2a 212 {
yoshikawaryota 2:ec149525ec2a 213 static double GyroDeg[2]= {0};
yoshikawaryota 2:ec149525ec2a 214 static double EncDeg[2][2]= {0};
yoshikawaryota 2:ec149525ec2a 215 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 216
yoshikawaryota 7:93ab5505ac1b 217
yoshikawaryota 7:93ab5505ac1b 218
yoshikawaryota 2:ec149525ec2a 219 /* ジャイロの値取得 */
yoshikawaryota 2:ec149525ec2a 220 bno.get_angles();
yoshikawaryota 2:ec149525ec2a 221 GyroDeg[1]=GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 222 GyroDeg[0]=bno.euler.yaw;
yoshikawaryota 2:ec149525ec2a 223 if(GyroDeg[0]!=0) {
yoshikawaryota 2:ec149525ec2a 224 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 225 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshikawaryota 2:ec149525ec2a 226 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 227 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshikawaryota 2:ec149525ec2a 228 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 229 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshikawaryota 2:ec149525ec2a 230 }
yoshikawaryota 2:ec149525ec2a 231 /* Enc2つの差分求める */
yoshikawaryota 2:ec149525ec2a 232 for(int i=0; i<2; i++) {
yoshikawaryota 2:ec149525ec2a 233 EncDeg[i][1]=EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 234 EncDeg[i][0]=EncoderDeg[i];
yoshikawaryota 7:93ab5505ac1b 235 disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
yoshikawaryota 2:ec149525ec2a 236 }
yoshikawaryota 2:ec149525ec2a 237 /* 差分を加速度として保存 */
yoshikawaryota 2:ec149525ec2a 238 NowAcc.theta = disp[2];
yoshikawaryota 7:93ab5505ac1b 239 NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
yoshikawaryota 7:93ab5505ac1b 240 NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
yoshikawaryota 2:ec149525ec2a 241 /* 差分を累積して現在位置を保存 */
yoshikawaryota 2:ec149525ec2a 242 NowLoc.X += NowAcc.X;
yoshikawaryota 2:ec149525ec2a 243 NowLoc.Y += NowAcc.Y;
yoshikawaryota 2:ec149525ec2a 244 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 245 }
TakushimaYukimasa 0:9e851dc42cde 246
TakushimaYukimasa 0:9e851dc42cde 247
TakushimaYukimasa 0:9e851dc42cde 248
TakushimaYukimasa 0:9e851dc42cde 249
TakushimaYukimasa 0:9e851dc42cde 250 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 251 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 252 {
yoshikawaryota 2:ec149525ec2a 253 static int cnt = 0;
yoshikawaryota 2:ec149525ec2a 254 static int data[EncoderMAX] = {0};
yoshikawaryota 2:ec149525ec2a 255 static double EncDeg[EncoderMAX][2] = {0};
yoshikawaryota 2:ec149525ec2a 256
yoshikawaryota 2:ec149525ec2a 257 for(int i=0; i<EncoderMAX; i++)
yoshikawaryota 2:ec149525ec2a 258 switch(cnt) {
yoshikawaryota 2:ec149525ec2a 259 /* 最初の処理 */
yoshikawaryota 2:ec149525ec2a 260 case 0:
yoshikawaryota 2:ec149525ec2a 261 data[i] = 0;
yoshikawaryota 2:ec149525ec2a 262 CS[i] = 0;
yoshikawaryota 2:ec149525ec2a 263 CL[i] = 1;
yoshikawaryota 2:ec149525ec2a 264 break;
yoshikawaryota 2:ec149525ec2a 265 /* 最後の処理 */
yoshikawaryota 2:ec149525ec2a 266 case 25:
yoshikawaryota 2:ec149525ec2a 267 CS[i]=1;
yoshikawaryota 2:ec149525ec2a 268 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshikawaryota 2:ec149525ec2a 269 EncDeg[i][1] = EncDeg[i][0];
yoshikawaryota 2:ec149525ec2a 270 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshikawaryota 2:ec149525ec2a 271 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshikawaryota 2:ec149525ec2a 272 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshikawaryota 2:ec149525ec2a 273 EncDeg[i][1] -= 360;
yoshikawaryota 2:ec149525ec2a 274 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshikawaryota 2:ec149525ec2a 275 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshikawaryota 2:ec149525ec2a 276 EncDeg[i][1] += 360;
yoshikawaryota 2:ec149525ec2a 277 /* 差を求める*/
yoshikawaryota 2:ec149525ec2a 278 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshikawaryota 2:ec149525ec2a 279 break;
yoshikawaryota 2:ec149525ec2a 280 /* 通常の処理 */
yoshikawaryota 2:ec149525ec2a 281 default:
yoshikawaryota 2:ec149525ec2a 282 CL[i]=!CL[i];
yoshikawaryota 2:ec149525ec2a 283 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshikawaryota 2:ec149525ec2a 284 if(cnt != 1 && cnt % 2) {
yoshikawaryota 2:ec149525ec2a 285 data[i] |= (DO[i]==1);
yoshikawaryota 2:ec149525ec2a 286 data[i] = data[i] << 1;
yoshikawaryota 2:ec149525ec2a 287 }
yoshikawaryota 2:ec149525ec2a 288 break;
yoshikawaryota 2:ec149525ec2a 289 }
yoshikawaryota 2:ec149525ec2a 290 cnt++;
yoshikawaryota 2:ec149525ec2a 291 cnt%=26;
yoshikawaryota 2:ec149525ec2a 292 }
yoshikawaryota 2:ec149525ec2a 293
yoshikawaryota 2:ec149525ec2a 294
yoshikawaryota 2:ec149525ec2a 295
yoshikawaryota 2:ec149525ec2a 296
yoshikawaryota 7:93ab5505ac1b 297
yoshikawaryota 7:93ab5505ac1b 298 /*******************************************************************************
yoshikawaryota 7:93ab5505ac1b 299 * @概要 オムニの値計算する(加算式)
yoshikawaryota 7:93ab5505ac1b 300 * @引数 X,Y,Rotationそれぞれの操作量
yoshikawaryota 7:93ab5505ac1b 301 * @返り値 なし
yoshikawaryota 7:93ab5505ac1b 302 *******************************************************************************/
yoshikawaryota 7:93ab5505ac1b 303 void SubOmuni(int X,int Y,int R)
yoshikawaryota 7:93ab5505ac1b 304 {
yoshikawaryota 7:93ab5505ac1b 305 /* 入力を100%に制限 */
yoshikawaryota 7:93ab5505ac1b 306 X=Rest(X,100);
yoshikawaryota 7:93ab5505ac1b 307 Y=Rest(Y,100);
yoshikawaryota 7:93ab5505ac1b 308 R=Rest(R,100);
yoshikawaryota 7:93ab5505ac1b 309
yoshikawaryota 9:8c550d496736 310
yoshikawaryota 9:8c550d496736 311
yoshikawaryota 9:8c550d496736 312
yoshikawaryota 7:93ab5505ac1b 313 /* オムニ計算結果をtmpに */
yoshikawaryota 7:93ab5505ac1b 314 int tmp[4]= {0};
yoshikawaryota 9:8c550d496736 315
yoshikawaryota 9:8c550d496736 316 /* 一度データをtmpに保存 */
yoshikawaryota 9:8c550d496736 317 for(int i=0; i<4; i++)
yoshikawaryota 9:8c550d496736 318 tmp[i]=MovMotor[i];
yoshikawaryota 9:8c550d496736 319
yoshikawaryota 9:8c550d496736 320 /* オムニ計算 */
yoshikawaryota 7:93ab5505ac1b 321 omuni.XmarkOmni_Move(X,Y,R);
yoshikawaryota 7:93ab5505ac1b 322
yoshikawaryota 7:93ab5505ac1b 323 /* 計算結果を加算する */
yoshikawaryota 9:8c550d496736 324 for(int i=0; i<4; i++)
yoshikawaryota 9:8c550d496736 325 MovMotor[i]+=tmp[i];
yoshikawaryota 7:93ab5505ac1b 326 }
yoshikawaryota 7:93ab5505ac1b 327
yoshikawaryota 7:93ab5505ac1b 328
yoshikawaryota 7:93ab5505ac1b 329
yoshikawaryota 7:93ab5505ac1b 330
yoshikawaryota 7:93ab5505ac1b 331
yoshikawaryota 2:ec149525ec2a 332 /*******************************************************************************
yoshikawaryota 9:8c550d496736 333 * @概要 台形制御
yoshikawaryota 10:e19d1d12f0ed 334 * @引数 tar_x :目標のx座標
yoshikawaryota 10:e19d1d12f0ed 335 * @引数 tar_y :目標のy座標
yoshikawaryota 10:e19d1d12f0ed 336 * @引数 tar_theta:目標角
yoshikawaryota 10:e19d1d12f0ed 337 * @引数 error :次ステップに進む基準とする距離
yoshikawaryota 9:8c550d496736 338 * @返り値 0(継続) 1(移動完了)
yoshikawaryota 2:ec149525ec2a 339 *******************************************************************************/
yoshikawaryota 9:8c550d496736 340 int omuni_control(int tar_x, int tar_y, int tar_theta, int error)
yoshikawaryota 2:ec149525ec2a 341 {
yoshikawaryota 6:201e3de9777d 342 static int log_distance = 0;
yoshikawaryota 9:8c550d496736 343 static int be_tar_x = 0, be_tar_y = 0;
yoshikawaryota 9:8c550d496736 344 static int a = 0, b = 0;
yoshikawaryota 6:201e3de9777d 345 static double log_theta = 0;
yoshikawaryota 6:201e3de9777d 346 static bool dis_flag = 0;
TakushimaYukimasa 0:9e851dc42cde 347
yoshikawaryota 4:05a6bda2e11f 348 if(dis_flag==0) {
yoshikawaryota 7:93ab5505ac1b 349 /* タイマー初期化 */
yoshikawaryota 7:93ab5505ac1b 350 Acc_time.reset();
yoshikawaryota 7:93ab5505ac1b 351 P_fin.reset();
yoshikawaryota 7:93ab5505ac1b 352 /* タイマー始動 */
yoshikawaryota 7:93ab5505ac1b 353 Acc_time.start();
yoshikawaryota 7:93ab5505ac1b 354 P_fin.start();
yoshikawaryota 6:201e3de9777d 355 /* 現在角度を記録しておく */
yoshikawaryota 6:201e3de9777d 356 log_theta = NowLoc.theta;
yoshikawaryota 4:05a6bda2e11f 357 /* 現在座標と目標座標との距離を記録しておく */
yoshikawaryota 4:05a6bda2e11f 358 log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 9:8c550d496736 359 /* 目標地点間を結ぶ直線の傾きと切片を求める */
yoshikawaryota 10:e19d1d12f0ed 360 if(tar_x != be_tar_x) a = (tar_y - be_tar_y) / (tar_x - be_tar_x);
yoshikawaryota 9:8c550d496736 361 b = tar_y - a * tar_x;
yoshikawaryota 4:05a6bda2e11f 362 /* フラグ回収 */
yoshikawaryota 4:05a6bda2e11f 363 dis_flag=1;
yoshikawaryota 4:05a6bda2e11f 364 }
yoshikawaryota 2:ec149525ec2a 365
yoshikawaryota 9:8c550d496736 366 int x_val = 0, x_acc_val = 0, x_dec_val = 0, Px = 0;
yoshikawaryota 9:8c550d496736 367 int y_val = 0, y_acc_val = 0, y_dec_val = 0, Py = 0;
yoshikawaryota 4:05a6bda2e11f 368 int t_val = 0;
yoshikawaryota 10:e19d1d12f0ed 369 int P_dis = 0, r = 0;
yoshikawaryota 10:e19d1d12f0ed 370 int Max[2] = {0}, Min[2] = {0}, x = 0, y = 0;
yoshikawaryota 4:05a6bda2e11f 371
yoshikawaryota 4:05a6bda2e11f 372 double theta_tar = 0.0;
yoshikawaryota 9:8c550d496736 373
yoshikawaryota 10:e19d1d12f0ed 374 /* x,yの値域を代入(0=x,1=y) */
yoshikawaryota 9:8c550d496736 375 if(NowLoc.X <= tar_x) {
yoshikawaryota 10:e19d1d12f0ed 376 Max[0] = tar_x;
yoshikawaryota 10:e19d1d12f0ed 377 Min[0] = NowLoc.X;
yoshikawaryota 9:8c550d496736 378 } else {
yoshikawaryota 10:e19d1d12f0ed 379 Max[0] = NowLoc.X;
yoshikawaryota 10:e19d1d12f0ed 380 Min[0] = tar_x;
yoshikawaryota 10:e19d1d12f0ed 381 }
yoshikawaryota 10:e19d1d12f0ed 382 if(NowLoc.Y <= tar_y) {
yoshikawaryota 10:e19d1d12f0ed 383 Max[1] = tar_y;
yoshikawaryota 10:e19d1d12f0ed 384 Min[1] = NowLoc.Y;
yoshikawaryota 10:e19d1d12f0ed 385 } else {
yoshikawaryota 10:e19d1d12f0ed 386 Max[1] = NowLoc.Y;
yoshikawaryota 10:e19d1d12f0ed 387 Min[1] = tar_y;
yoshikawaryota 9:8c550d496736 388 }
yoshikawaryota 9:8c550d496736 389 /* 目標にする点Pの決定 */
yoshikawaryota 10:e19d1d12f0ed 390 if(Max[0] != Min[0]) {
yoshikawaryota 10:e19d1d12f0ed 391 r = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
yoshikawaryota 10:e19d1d12f0ed 392 /* 半径の最大を設定 */
yoshikawaryota 10:e19d1d12f0ed 393 r = Rest(r, 100);
yoshikawaryota 10:e19d1d12f0ed 394 for(x=Min[0]; x<=Max[0]; x++) {
yoshikawaryota 10:e19d1d12f0ed 395 y = a * x + b;
yoshikawaryota 10:e19d1d12f0ed 396 P_dis = sqrt(pow(x-NowLoc.X,2)+pow(y-NowLoc.Y,2));
yoshikawaryota 10:e19d1d12f0ed 397 if(P_dis == r && ABS(tar_x - x) < ABS(tar_x - NowLoc.X) || P_dis == r && ABS(tar_y - y) < ABS(tar_y - NowLoc.Y)) {
yoshikawaryota 10:e19d1d12f0ed 398 Px = x;
yoshikawaryota 10:e19d1d12f0ed 399 Py = y;
yoshikawaryota 10:e19d1d12f0ed 400 break;
yoshikawaryota 10:e19d1d12f0ed 401 }
yoshikawaryota 10:e19d1d12f0ed 402 if(x == Max[0]) {
yoshikawaryota 10:e19d1d12f0ed 403 Px = tar_x;
yoshikawaryota 10:e19d1d12f0ed 404 Py = tar_y;
yoshikawaryota 10:e19d1d12f0ed 405 }
yoshikawaryota 9:8c550d496736 406 }
yoshikawaryota 10:e19d1d12f0ed 407 }
yoshikawaryota 11:80b1daeb243f 408 /* x=定数の式(縦の直線)になった時 */
yoshikawaryota 10:e19d1d12f0ed 409 else {
yoshikawaryota 10:e19d1d12f0ed 410 r = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
yoshikawaryota 10:e19d1d12f0ed 411 /* 半径の最大を設定 */
yoshikawaryota 10:e19d1d12f0ed 412 r = Rest(r, 100);
yoshikawaryota 10:e19d1d12f0ed 413 for(y=Min[1]; y<=Max[1]; y++) {
yoshikawaryota 10:e19d1d12f0ed 414 P_dis = y;
yoshikawaryota 10:e19d1d12f0ed 415
yoshikawaryota 9:8c550d496736 416 Px = tar_x;
yoshikawaryota 10:e19d1d12f0ed 417 if(P_dis == r && ABS(tar_y - y) < ABS(tar_y - NowLoc.Y)) {
yoshikawaryota 10:e19d1d12f0ed 418 Py = y;
yoshikawaryota 10:e19d1d12f0ed 419 break;
yoshikawaryota 10:e19d1d12f0ed 420 }
yoshikawaryota 10:e19d1d12f0ed 421 if(y == Max[1]) Py = tar_y;
yoshikawaryota 9:8c550d496736 422 }
yoshikawaryota 9:8c550d496736 423 }
yoshikawaryota 7:93ab5505ac1b 424 /* 減速処理の値代入 */
yoshikawaryota 9:8c550d496736 425 x_dec_val = ABS((Px - NowLoc.X) * 0.06);
yoshikawaryota 9:8c550d496736 426 y_dec_val = ABS((Py - NowLoc.Y) * 0.06);
yoshikawaryota 7:93ab5505ac1b 427 /* 加速処理の値代入 */
yoshikawaryota 7:93ab5505ac1b 428 x_acc_val=ABS((int)Acc_time.read_ms() * 0.15);
yoshikawaryota 7:93ab5505ac1b 429 y_acc_val=ABS((int)Acc_time.read_ms() * 0.15);
yoshikawaryota 7:93ab5505ac1b 430 /* 低い方の値代入 */
yoshikawaryota 7:93ab5505ac1b 431 x_val = M_MIN(x_acc_val, x_dec_val);
yoshikawaryota 7:93ab5505ac1b 432 y_val = M_MIN(y_acc_val, y_dec_val);
yoshikawaryota 9:8c550d496736 433
yoshikawaryota 4:05a6bda2e11f 434 /* 最低を決める */
yoshikawaryota 7:93ab5505ac1b 435 x_val = M_MAX(x_val, 20);
yoshikawaryota 7:93ab5505ac1b 436 y_val = M_MAX(y_val, 20);
yoshikawaryota 9:8c550d496736 437
yoshikawaryota 4:05a6bda2e11f 438 /* 100以内に収める */
yoshikawaryota 4:05a6bda2e11f 439 x_val = Rest(x_val, 100);
yoshikawaryota 4:05a6bda2e11f 440 y_val = Rest(y_val, 100);
yoshikawaryota 2:ec149525ec2a 441
yoshikawaryota 4:05a6bda2e11f 442 /* 正負を決める */
yoshikawaryota 9:8c550d496736 443 if ((Px - NowLoc.X) < 0)
yoshikawaryota 4:05a6bda2e11f 444 x_val *= -1;
yoshikawaryota 9:8c550d496736 445 if ((Py - NowLoc.Y) < 0)
yoshikawaryota 4:05a6bda2e11f 446 y_val *= -1;
yoshikawaryota 2:ec149525ec2a 447
yoshikawaryota 4:05a6bda2e11f 448 /* 残り距離計算 */
yoshikawaryota 4:05a6bda2e11f 449 int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
yoshikawaryota 4:05a6bda2e11f 450 /* 移動量に応じたthetaを求める */
yoshikawaryota 8:ddedee42c253 451 theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta) + log_theta;
yoshikawaryota 4:05a6bda2e11f 452 /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
yoshikawaryota 4:05a6bda2e11f 453 t_val = (theta_tar - NowLoc.theta) * 1.5;
yoshikawaryota 3:b77f51108bf1 454
yoshikawaryota 4:05a6bda2e11f 455 t_val = Rest(t_val, 20);
yoshikawaryota 2:ec149525ec2a 456
yoshikawaryota 7:93ab5505ac1b 457 SubOmuni(x_val, y_val, t_val);
yoshikawaryota 2:ec149525ec2a 458
yoshikawaryota 4:05a6bda2e11f 459 /* 終了処理 */
yoshikawaryota 9:8c550d496736 460 if(error != 0 && error >= remain) {
yoshikawaryota 9:8c550d496736 461 be_tar_x = tar_x;
yoshikawaryota 9:8c550d496736 462 be_tar_y = tar_y;
yoshikawaryota 9:8c550d496736 463 return 1;
yoshikawaryota 9:8c550d496736 464 } else if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
yoshikawaryota 4:05a6bda2e11f 465 x_val = 0;
yoshikawaryota 4:05a6bda2e11f 466 y_val = 0;
yoshikawaryota 4:05a6bda2e11f 467 }
yoshikawaryota 4:05a6bda2e11f 468 if (x_val == 0 && y_val == 0)
yoshikawaryota 4:05a6bda2e11f 469 P_fin.start();
yoshikawaryota 4:05a6bda2e11f 470 else
yoshikawaryota 4:05a6bda2e11f 471 P_fin.reset();
yoshikawaryota 2:ec149525ec2a 472
yoshikawaryota 4:05a6bda2e11f 473 if (P_fin.read_ms() > 100) {
yoshikawaryota 4:05a6bda2e11f 474 /* タイマー停止 */
yoshikawaryota 4:05a6bda2e11f 475 P_fin.stop();
yoshikawaryota 4:05a6bda2e11f 476 /* フラグを建て直す */
yoshikawaryota 4:05a6bda2e11f 477 dis_flag=0;
yoshikawaryota 9:8c550d496736 478
yoshikawaryota 8:ddedee42c253 479 return 1;
yoshikawaryota 2:ec149525ec2a 480 }
yoshikawaryota 8:ddedee42c253 481 return 0;
TakushimaYukimasa 0:9e851dc42cde 482 }