2020_TeamA / Mbed 2 deprecated Yoshida_MiniRobo

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@23:98b634305bab, 2020-03-24 (annotated)

Committer:
yoshidayuito
Date:
Tue Mar 24 04:02:57 2020 +0000
Revision:
23:98b634305bab
Parent:
22:9d77148a3f09 
Fix KyoRobo Move;Ver.yoshida

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;
TakushimaYukimasa 0:9e851dc42cde 20
TakushimaYukimasa 0:9e851dc42cde 21 /* 定数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 22 /* マクロ定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 23 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 24
TakushimaYukimasa 0:9e851dc42cde 25 /* タイマ呼び出し用 */
TakushimaYukimasa 0:9e851dc42cde 26 void IT_CallBack(void);
TakushimaYukimasa 0:9e851dc42cde 27
TakushimaYukimasa 0:9e851dc42cde 28 /* 自己位置推定処理 */
TakushimaYukimasa 0:9e851dc42cde 29 void LocEstimate(void);
TakushimaYukimasa 0:9e851dc42cde 30
yoshidayuito 7:8bea84f72e64 31 /* 壁あて */
yoshidayuito 23:98b634305bab 32 int ToWall(void);
TakushimaYukimasa 0:9e851dc42cde 33
yoshidayuito 13:c8af1467ba8b 34 /* x座標移動 */
yoshidayuito 13:c8af1467ba8b 35 int XCooMove(int Tar,int Err);
yoshidayuito 13:c8af1467ba8b 36
yoshidayuito 16:ea3008995c41 37 /* ライン補正 */
yoshidayuito 23:98b634305bab 38 int LineCor(void);
yoshidayuito 16:ea3008995c41 39
yoshidayuito 22:9d77148a3f09 40 /* オムニ加算式 */
yoshidayuito 16:ea3008995c41 41 void SubOmuni(int X,int Y,int R);
yoshidayuito 16:ea3008995c41 42
TakushimaYukimasa 0:9e851dc42cde 43 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 44
TakushimaYukimasa 0:9e851dc42cde 45 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 46 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 47
TakushimaYukimasa 0:9e851dc42cde 48 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 49 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 50
TakushimaYukimasa 0:9e851dc42cde 51 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 52 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 53
TakushimaYukimasa 0:9e851dc42cde 54 /* 足回り値保存変数 */
yoshidayuito 7:8bea84f72e64 55 int MovMotor[4]= {0};
TakushimaYukimasa 0:9e851dc42cde 56
TakushimaYukimasa 0:9e851dc42cde 57 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 58 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 59
yoshidayuito 22:9d77148a3f09 60 /* ライン番号 */
yoshidayuito 22:9d77148a3f09 61 int Line_Num=0;
yoshidayuito 22:9d77148a3f09 62
yoshidayuito 23:98b634305bab 63 /* ラインセンサ左,右の前回の値 0…左 1…右 2…直進*/
yoshidayuito 23:98b634305bab 64 int Last_val=0;
yoshidayuito 23:98b634305bab 65
TakushimaYukimasa 0:9e851dc42cde 66 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 67
TakushimaYukimasa 0:9e851dc42cde 68 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 69 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 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
yoshidayuito 1:520233a969e2 88 /* ジャイロ用タイマー */
yoshidayuito 1:520233a969e2 89 Timer yawCnt;
yoshidayuito 1:520233a969e2 90
yoshidayuito 22:9d77148a3f09 91 /* 前進用タイマー */
yoshidayuito 22:9d77148a3f09 92 Timer Forward;
yoshidayuito 22:9d77148a3f09 93
TakushimaYukimasa 0:9e851dc42cde 94 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 95 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 96
TakushimaYukimasa 0:9e851dc42cde 97 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 98 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 99
yoshidayuito 8:367c7d6ef5a3 100 /* I2C Inputのクラス定義 */
yoshidayuito 8:367c7d6ef5a3 101 YKNCT_I2C_IN In(PB_9,PB_8);
yoshidayuito 8:367c7d6ef5a3 102
TakushimaYukimasa 0:9e851dc42cde 103 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 104 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 105
TakushimaYukimasa 0:9e851dc42cde 106
TakushimaYukimasa 0:9e851dc42cde 107 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 108 int main()
TakushimaYukimasa 0:9e851dc42cde 109 {
yoshidayuito 7:8bea84f72e64 110 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 111
yoshidayuito 7:8bea84f72e64 112 /* 割り込みの設定
yoshidayuito 7:8bea84f72e64 113 * IT_CallBack関数を0.1msで割り込み */
yoshidayuito 7:8bea84f72e64 114 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 115
yoshidayuito 7:8bea84f72e64 116 /* ジャイロの設定 */
yoshidayuito 7:8bea84f72e64 117 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 118
yoshidayuito 7:8bea84f72e64 119 /* I2CMDの設定 */
yoshidayuito 7:8bea84f72e64 120 MD.Init(0,MD_SMB);
yoshidayuito 7:8bea84f72e64 121 MD.Init(1,MD_SMB);
yoshidayuito 7:8bea84f72e64 122 MD.Init(2,MD_SMB);
yoshidayuito 7:8bea84f72e64 123 MD.Init(3,MD_SMB);
yoshidayuito 10:9ee22b22e583 124
yoshidayuito 15:909c23ab7f0c 125 /* 足リミット */
yoshidayuito 8:367c7d6ef5a3 126 In.Init(0,0);
yoshidayuito 8:367c7d6ef5a3 127 In.Init(0,1);
TakushimaYukimasa 0:9e851dc42cde 128
yoshidayuito 23:98b634305bab 129 /* 正面リミット */
yoshidayuito 23:98b634305bab 130 In.Init(0,2);
yoshidayuito 23:98b634305bab 131 In.Init(0,3);
yoshidayuito 23:98b634305bab 132
yoshidayuito 15:909c23ab7f0c 133 /* ラインセンサ */
yoshidayuito 15:909c23ab7f0c 134 In.Init(0,2);
yoshidayuito 15:909c23ab7f0c 135 In.Init(0,3);
yoshidayuito 15:909c23ab7f0c 136
yoshidayuito 22:9d77148a3f09 137 /* タイマーリセット */
yoshidayuito 22:9d77148a3f09 138 Forward.reset();
yoshidayuito 22:9d77148a3f09 139
yoshidayuito 7:8bea84f72e64 140 telemetry.printf("\n\rMainLoopStart");
yoshidayuito 7:8bea84f72e64 141 /* メインループ --------------------------------------------------------------*/
yoshidayuito 11:42310638e241 142 while(1) {
yoshidayuito 7:8bea84f72e64 143 /* オンボードLED点滅 */
yoshidayuito 7:8bea84f72e64 144 led=!led;
yoshidayuito 7:8bea84f72e64 145
yoshidayuito 7:8bea84f72e64 146 /* 表示改行 */
yoshidayuito 7:8bea84f72e64 147 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 148
yoshidayuito 7:8bea84f72e64 149 /* 自動処理関連テレメトリ */
yoshidayuito 7:8bea84f72e64 150 telemetry.printf("ope:%d ",operate);
yoshidayuito 7:8bea84f72e64 151 /* 座標テレメトリ */
yoshidayuito 7:8bea84f72e64 152 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 153
yoshidayuito 7:8bea84f72e64 154 /* 自己位置推定更新 */
yoshidayuito 7:8bea84f72e64 155 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 156
TakushimaYukimasa 14:18ec50f38993 157
TakushimaYukimasa 14:18ec50f38993 158 /* オムニ分のMD代入 */
TakushimaYukimasa 14:18ec50f38993 159 OmuniExe;
TakushimaYukimasa 14:18ec50f38993 160
yoshidayuito 16:ea3008995c41 161 for(int i=0; i<4; i++) {
yoshidayuito 16:ea3008995c41 162 MD.Set(i,MovMotor[i]);
yoshidayuito 16:ea3008995c41 163 MovMotor[i]=0;
yoshidayuito 16:ea3008995c41 164 }
yoshidayuito 16:ea3008995c41 165
yoshidayuito 7:8bea84f72e64 166 /* I2CMD実行 */
yoshidayuito 7:8bea84f72e64 167 MD.Exe();
TakushimaYukimasa 0:9e851dc42cde 168
yoshidayuito 11:42310638e241 169 /* ジャイロ用タイマースタート */
yoshidayuito 7:8bea84f72e64 170 yawCnt.start();
yoshidayuito 11:42310638e241 171 /* ジャイロ用タイマーリセット */
yoshidayuito 7:8bea84f72e64 172 yawCnt.reset();
yoshidayuito 7:8bea84f72e64 173
yoshidayuito 7:8bea84f72e64 174 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshidayuito 7:8bea84f72e64 175 if(DS3.CROSS) operate=0;
yoshidayuito 7:8bea84f72e64 176 if(DS3.TRIANGLE) operate=1;
yoshidayuito 7:8bea84f72e64 177 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 178
yoshidayuito 7:8bea84f72e64 179 /* 操縦権：なし 停止動作 */
yoshidayuito 7:8bea84f72e64 180 if(operate==0) {
yoshidayuito 7:8bea84f72e64 181 /* 足回り停止 */
yoshidayuito 16:ea3008995c41 182 SubOmuni(0,0,0);
yoshidayuito 10:9ee22b22e583 183
yoshidayuito 7:8bea84f72e64 184 }
yoshidayuito 7:8bea84f72e64 185 /* 操縦権：手動 */
yoshidayuito 7:8bea84f72e64 186 else if(operate==1) {
yoshidayuito 7:8bea84f72e64 187 /* 足回り手動動作 */
yoshidayuito 7:8bea84f72e64 188 int x_val = (double)(DS3.LX-64)*100/64;
yoshidayuito 7:8bea84f72e64 189 int y_val = (double)(64-DS3.LY)*100/64;
yoshidayuito 7:8bea84f72e64 190 int r_val = (double)(DS3.RX-64)*100/64;
yoshidayuito 7:8bea84f72e64 191
yoshidayuito 7:8bea84f72e64 192 /* タイマーをリセット */
yoshidayuito 7:8bea84f72e64 193 if(DS3.RX!=64) yawCnt.reset();
yoshidayuito 7:8bea84f72e64 194 /* 目標角度再設定 */
yoshidayuito 7:8bea84f72e64 195 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshidayuito 7:8bea84f72e64 196 /* r_val補正 */
yoshidayuito 7:8bea84f72e64 197 r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;
yoshidayuito 7:8bea84f72e64 198
yoshidayuito 16:ea3008995c41 199 SubOmuni(x_val,y_val,r_val);
yoshidayuito 10:9ee22b22e583 200
yoshidayuito 9:94112b5df540 201 /* 壁あて実行 */
yoshidayuito 9:94112b5df540 202 if(DS3.R1) ToWall();
yoshidayuito 7:8bea84f72e64 203 }
yoshidayuito 7:8bea84f72e64 204 /* 操縦権：自動 */
yoshidayuito 7:8bea84f72e64 205 else if(operate==2) {
yoshidayuito 7:8bea84f72e64 206 switch(auto_mode) {
yoshidayuito 7:8bea84f72e64 207 /* スタート待機処理 */
yoshidayuito 7:8bea84f72e64 208 case 0:
yoshidayuito 20:79f96d223e5d 209 /* STARTで次のステップに */
yoshidayuito 20:79f96d223e5d 210 if(DS3.START) auto_mode++;
yoshidayuito 7:8bea84f72e64 211 break;
yoshidayuito 7:8bea84f72e64 212
yoshidayuito 22:9d77148a3f09 213 /* X座標移動 */
yoshidayuito 7:8bea84f72e64 214 case 1:
yoshidayuito 22:9d77148a3f09 215 /* 壁あて実行 */
yoshidayuito 22:9d77148a3f09 216 ToWall();
yoshidayuito 13:c8af1467ba8b 217 /* 動作終了時次のステップに */
yoshidayuito 22:9d77148a3f09 218 if(Line_Num==0&&XCooMove(-1000,30)==0) {
yoshidayuito 22:9d77148a3f09 219 Line_Num++;
yoshidayuito 22:9d77148a3f09 220 auto_mode++;
yoshidayuito 22:9d77148a3f09 221 }
yoshidayuito 22:9d77148a3f09 222 if(Line_Num==1&&XCooMove(-1500,30)==0) {
yoshidayuito 22:9d77148a3f09 223 Line_Num++;
yoshidayuito 22:9d77148a3f09 224 auto_mode++;
yoshidayuito 22:9d77148a3f09 225 }
yoshidayuito 22:9d77148a3f09 226 if(Line_Num==2&&XCooMove(-2200,30)==0) {
yoshidayuito 22:9d77148a3f09 227 Line_Num=0;
yoshidayuito 22:9d77148a3f09 228 auto_mode++;
yoshidayuito 22:9d77148a3f09 229 }
yoshidayuito 22:9d77148a3f09 230
yoshidayuito 22:9d77148a3f09 231 break;
yoshidayuito 22:9d77148a3f09 232
yoshidayuito 22:9d77148a3f09 233 /* ラインまで移動 */
yoshidayuito 22:9d77148a3f09 234 case 2:
yoshidayuito 22:9d77148a3f09 235 /* ライン到達後次のステップに */
yoshidayuito 23:98b634305bab 236 Last_val=0;
yoshidayuito 23:98b634305bab 237 if(LineCor()==1)
yoshidayuito 22:9d77148a3f09 238 auto_mode++;
yoshidayuito 22:9d77148a3f09 239
yoshidayuito 22:9d77148a3f09 240 break;
yoshidayuito 22:9d77148a3f09 241 /* */
yoshidayuito 22:9d77148a3f09 242 case 3:
yoshidayuito 22:9d77148a3f09 243 /* ライン補正しながら直進 */
yoshidayuito 23:98b634305bab 244 Last_val=2;
yoshidayuito 23:98b634305bab 245 LineCor();
yoshidayuito 22:9d77148a3f09 246 Forward.start();
yoshidayuito 22:9d77148a3f09 247 SubOmuni(0,20,0);
yoshidayuito 22:9d77148a3f09 248 /* 動作終了時次のステップに */
yoshidayuito 23:98b634305bab 249 if(Forward.read_ms()<=2500) {
yoshidayuito 22:9d77148a3f09 250 auto_mode++;
yoshidayuito 23:98b634305bab 251 Forward.stop();
yoshidayuito 22:9d77148a3f09 252 Forward.reset();
yoshidayuito 22:9d77148a3f09 253 }
yoshidayuito 22:9d77148a3f09 254
yoshidayuito 22:9d77148a3f09 255 break;
yoshidayuito 22:9d77148a3f09 256
yoshidayuito 22:9d77148a3f09 257 case 4:
yoshidayuito 22:9d77148a3f09 258 /* ライン補正しながら後ろへ移動 */
yoshidayuito 23:98b634305bab 259 Last_val=2;
yoshidayuito 23:98b634305bab 260 if(ToWall()&&LineCor())
yoshidayuito 13:c8af1467ba8b 261 auto_mode++;
TakushimaYukimasa 14:18ec50f38993 262
yoshidayuito 7:8bea84f72e64 263 break;
yoshidayuito 7:8bea84f72e64 264
yoshidayuito 22:9d77148a3f09 265 case 5:
yoshidayuito 22:9d77148a3f09 266 /* 壁あて実行 */
yoshidayuito 22:9d77148a3f09 267 ToWall();
yoshidayuito 22:9d77148a3f09 268 /* 右へ移動 */
yoshidayuito 22:9d77148a3f09 269 if(Line_Num==1&&XCooMove(1200,10)==0)
yoshidayuito 22:9d77148a3f09 270 auto_mode++;
yoshidayuito 22:9d77148a3f09 271 if(Line_Num==2&&XCooMove(1700,10)==0)
yoshidayuito 22:9d77148a3f09 272 auto_mode++;
yoshidayuito 22:9d77148a3f09 273 if(Line_Num==3&&XCooMove(2400,10)==0)
yoshidayuito 17:5b4080915375 274 auto_mode++;
yoshidayuito 17:5b4080915375 275
yoshidayuito 17:5b4080915375 276 break;
yoshidayuito 17:5b4080915375 277
yoshidayuito 22:9d77148a3f09 278 case 6:
yoshidayuito 22:9d77148a3f09 279 /* 宝物置き場付近まで直進 */
yoshidayuito 22:9d77148a3f09 280 Forward.start();
yoshidayuito 22:9d77148a3f09 281 SubOmuni(0,20,0);
yoshidayuito 22:9d77148a3f09 282 /* 動作終了時次のステップに */
yoshidayuito 23:98b634305bab 283 if(Forward.read_ms()>=4000) {
yoshidayuito 22:9d77148a3f09 284 auto_mode++;
yoshidayuito 23:98b634305bab 285 Forward.stop();
yoshidayuito 22:9d77148a3f09 286 Forward.reset();
yoshidayuito 22:9d77148a3f09 287 }
yoshidayuito 22:9d77148a3f09 288
yoshidayuito 22:9d77148a3f09 289 break;
yoshidayuito 22:9d77148a3f09 290
yoshidayuito 22:9d77148a3f09 291 case 7:
yoshidayuito 22:9d77148a3f09 292 /* 宝物置き場のラインまで移動 */
yoshidayuito 23:98b634305bab 293 Last_val=1;
yoshidayuito 23:98b634305bab 294 if(LineCor()==1)//*
yoshidayuito 22:9d77148a3f09 295 auto_mode++;
yoshidayuito 23:98b634305bab 296
yoshidayuito 22:9d77148a3f09 297 break;
yoshidayuito 23:98b634305bab 298
yoshidayuito 23:98b634305bab 299 case 8:/* ライン補正しながら直進 */
yoshidayuito 23:98b634305bab 300 Last_val=2;
yoshidayuito 23:98b634305bab 301 LineCor();
yoshidayuito 22:9d77148a3f09 302 SubOmuni(0,20,0);
yoshidayuito 22:9d77148a3f09 303 /* 動作終了時次のステップに */
yoshidayuito 23:98b634305bab 304 if(In.Get(0)==1&&In.Get(1)==1) {
yoshidayuito 22:9d77148a3f09 305 auto_mode++;
yoshidayuito 22:9d77148a3f09 306 }
yoshidayuito 23:98b634305bab 307
yoshidayuito 22:9d77148a3f09 308 break;
yoshidayuito 23:98b634305bab 309
yoshidayuito 22:9d77148a3f09 310 case 9:
yoshidayuito 22:9d77148a3f09 311 /* スタートゾーンに移動 */
yoshidayuito 22:9d77148a3f09 312 ToWall();
yoshidayuito 22:9d77148a3f09 313 if(In.Get(0)==1&&In.Get(1)==1)
yoshidayuito 22:9d77148a3f09 314 auto_mode++;
yoshidayuito 22:9d77148a3f09 315
yoshidayuito 22:9d77148a3f09 316 break;
yoshidayuito 22:9d77148a3f09 317
yoshidayuito 22:9d77148a3f09 318
yoshidayuito 7:8bea84f72e64 319 /* 終了処理 */
yoshidayuito 7:8bea84f72e64 320 default:
yoshidayuito 7:8bea84f72e64 321 auto_mode=0;
yoshidayuito 7:8bea84f72e64 322 operate=0;
yoshidayuito 7:8bea84f72e64 323 break;
yoshidayuito 7:8bea84f72e64 324 }
yoshidayuito 7:8bea84f72e64 325 }
yoshidayuito 7:8bea84f72e64 326
TakushimaYukimasa 0:9e851dc42cde 327 }
TakushimaYukimasa 0:9e851dc42cde 328 }
TakushimaYukimasa 0:9e851dc42cde 329
TakushimaYukimasa 0:9e851dc42cde 330
TakushimaYukimasa 0:9e851dc42cde 331 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 332 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 333 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 334 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 335 *******************************************************************************/
yoshidayuito 7:8bea84f72e64 336 void LocEstimate(void)
yoshidayuito 7:8bea84f72e64 337 {
yoshidayuito 22:9d77148a3f09 338 static double GyroDeg[2]= {0};
yoshidayuito 7:8bea84f72e64 339 static double EncDeg[2][2]= {0};
yoshidayuito 7:8bea84f72e64 340 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 341
yoshidayuito 22:9d77148a3f09 342
yoshidayuito 21:22b863d32705 343
yoshidayuito 7:8bea84f72e64 344 /* ジャイロの値取得 */
yoshidayuito 7:8bea84f72e64 345 bno.get_angles();
yoshidayuito 7:8bea84f72e64 346 GyroDeg[1]=GyroDeg[0];
yoshidayuito 7:8bea84f72e64 347 GyroDeg[0]=bno.euler.yaw;
yoshidayuito 7:8bea84f72e64 348 if(GyroDeg[0]!=0) {
yoshidayuito 7:8bea84f72e64 349 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 350 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshidayuito 7:8bea84f72e64 351 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 352 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshidayuito 7:8bea84f72e64 353 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 354 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshidayuito 7:8bea84f72e64 355 }
yoshidayuito 21:22b863d32705 356 /* Enc2つの差分求める */
yoshidayuito 21:22b863d32705 357 for(int i=0; i<2; i++) {
yoshidayuito 7:8bea84f72e64 358 EncDeg[i][1]=EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 359 EncDeg[i][0]=EncoderDeg[i];
yoshidayuito 21:22b863d32705 360 disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
yoshidayuito 7:8bea84f72e64 361 }
yoshidayuito 7:8bea84f72e64 362 /* 差分を加速度として保存 */
yoshidayuito 7:8bea84f72e64 363 NowAcc.theta = disp[2];
yoshidayuito 21:22b863d32705 364 NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
yoshidayuito 21:22b863d32705 365 NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
yoshidayuito 7:8bea84f72e64 366 /* 差分を累積して現在位置を保存 */
yoshidayuito 7:8bea84f72e64 367 NowLoc.X += NowAcc.X;
yoshidayuito 21:22b863d32705 368 NowLoc.Y += NowAcc.Y;
yoshidayuito 7:8bea84f72e64 369 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 370 }
TakushimaYukimasa 0:9e851dc42cde 371
TakushimaYukimasa 0:9e851dc42cde 372
TakushimaYukimasa 0:9e851dc42cde 373
TakushimaYukimasa 0:9e851dc42cde 374
TakushimaYukimasa 0:9e851dc42cde 375 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 376 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 377 {
yoshidayuito 7:8bea84f72e64 378 static int cnt = 0;
yoshidayuito 7:8bea84f72e64 379 static int data[EncoderMAX] = {0};
yoshidayuito 7:8bea84f72e64 380 static double EncDeg[EncoderMAX][2] = {0};
TakushimaYukimasa 0:9e851dc42cde 381
yoshidayuito 7:8bea84f72e64 382 for(int i=0; i<EncoderMAX; i++)
yoshidayuito 7:8bea84f72e64 383 switch(cnt) {
yoshidayuito 7:8bea84f72e64 384 /* 最初の処理 */
yoshidayuito 7:8bea84f72e64 385 case 0:
yoshidayuito 7:8bea84f72e64 386 data[i] = 0;
yoshidayuito 7:8bea84f72e64 387 CS[i] = 0;
yoshidayuito 7:8bea84f72e64 388 CL[i] = 1;
yoshidayuito 7:8bea84f72e64 389 break;
yoshidayuito 7:8bea84f72e64 390 /* 最後の処理 */
yoshidayuito 7:8bea84f72e64 391 case 25:
yoshidayuito 7:8bea84f72e64 392 CS[i]=1;
yoshidayuito 7:8bea84f72e64 393 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshidayuito 7:8bea84f72e64 394 EncDeg[i][1] = EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 395 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshidayuito 7:8bea84f72e64 396 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 397 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshidayuito 7:8bea84f72e64 398 EncDeg[i][1] -= 360;
yoshidayuito 7:8bea84f72e64 399 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 400 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshidayuito 7:8bea84f72e64 401 EncDeg[i][1] += 360;
yoshidayuito 7:8bea84f72e64 402 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 403 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshidayuito 7:8bea84f72e64 404 break;
yoshidayuito 7:8bea84f72e64 405 /* 通常の処理 */
yoshidayuito 7:8bea84f72e64 406 default:
yoshidayuito 7:8bea84f72e64 407 CL[i]=!CL[i];
yoshidayuito 7:8bea84f72e64 408 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshidayuito 7:8bea84f72e64 409 if(cnt != 1 && cnt % 2) {
yoshidayuito 7:8bea84f72e64 410 data[i] |= (DO[i]==1);
yoshidayuito 7:8bea84f72e64 411 data[i] = data[i] << 1;
yoshidayuito 7:8bea84f72e64 412 }
yoshidayuito 7:8bea84f72e64 413 break;
yoshidayuito 7:8bea84f72e64 414 }
yoshidayuito 7:8bea84f72e64 415 cnt++;
yoshidayuito 7:8bea84f72e64 416 cnt%=26;
yoshidayuito 7:8bea84f72e64 417 }
yoshidayuito 7:8bea84f72e64 418
yoshidayuito 7:8bea84f72e64 419
yoshidayuito 7:8bea84f72e64 420
yoshidayuito 7:8bea84f72e64 421
yoshidayuito 7:8bea84f72e64 422 /*******************************************************************************
yoshidayuito 7:8bea84f72e64 423 * @概要 壁あて関数
yoshidayuito 7:8bea84f72e64 424 * @引数 なし
yoshidayuito 23:98b634305bab 425 * @返り値 0:未到達
yoshidayuito 23:98b634305bab 426 * @返り値 1:到達
yoshidayuito 7:8bea84f72e64 427 *******************************************************************************/
yoshidayuito 23:98b634305bab 428 int ToWall()
yoshidayuito 7:8bea84f72e64 429 {
TakushimaYukimasa 14:18ec50f38993 430 /* 左側リミットに応じて左2輪の操作量を決める */
yoshidayuito 9:94112b5df540 431 if(In.Get(0)==0)
yoshidayuito 7:8bea84f72e64 432 for(int i=0; i<2; i++) MovMotor[i]-=10;
TakushimaYukimasa 14:18ec50f38993 433
TakushimaYukimasa 14:18ec50f38993 434 /* 右側リミットに応じて左2輪の操作量を決める */
yoshidayuito 9:94112b5df540 435 if(In.Get(1)==0)
yoshidayuito 7:8bea84f72e64 436 for(int i=2; i<4; i++) MovMotor[i]+=10;
yoshidayuito 22:9d77148a3f09 437
yoshidayuito 23:98b634305bab 438 if(In.Get(0)==1&&In.Get(1)==1)
yoshidayuito 23:98b634305bab 439 return 1;
yoshidayuito 23:98b634305bab 440 else return 0;
yoshidayuito 23:98b634305bab 441
yoshidayuito 10:9ee22b22e583 442 }
yoshidayuito 10:9ee22b22e583 443
yoshidayuito 10:9ee22b22e583 444
yoshidayuito 10:9ee22b22e583 445
yoshidayuito 10:9ee22b22e583 446 /*******************************************************************************
yoshidayuito 10:9ee22b22e583 447 * @概要 x座標移動関数
yoshidayuito 23:98b634305bab 448 * @引数 Tar :目標値
yoshidayuito 10:9ee22b22e583 449 * @引数 Err :許容誤差
yoshidayuito 10:9ee22b22e583 450 * @返り値 val_p:残り距離
yoshidayuito 10:9ee22b22e583 451 *******************************************************************************/
yoshidayuito 11:42310638e241 452 int XCooMove(int Tar,int Err)
yoshidayuito 10:9ee22b22e583 453 {
TakushimaYukimasa 14:18ec50f38993 454 /* x座標移動初期化フラグ */
TakushimaYukimasa 14:18ec50f38993 455 static bool XCooMoveFlag=0;
TakushimaYukimasa 14:18ec50f38993 456
TakushimaYukimasa 14:18ec50f38993 457 /* x座標移動タイマー */
TakushimaYukimasa 14:18ec50f38993 458 static Timer CooCnt;
TakushimaYukimasa 14:18ec50f38993 459
TakushimaYukimasa 14:18ec50f38993 460 /* 速度 加速・減速・最終操作量 */
yoshidayuito 11:42310638e241 461 int val_p=0,val_t=0,val=0;
yoshidayuito 10:9ee22b22e583 462
TakushimaYukimasa 14:18ec50f38993 463 /* X座標移動最初だけ初期化 */
TakushimaYukimasa 14:18ec50f38993 464 if(XCooMoveFlag==0) {
yoshidayuito 13:c8af1467ba8b 465 /* タイマースタート */
yoshidayuito 13:c8af1467ba8b 466 CooCnt.start();
yoshidayuito 13:c8af1467ba8b 467 /* X座標移動用タイマーリセット */
yoshidayuito 13:c8af1467ba8b 468 CooCnt.reset();
TakushimaYukimasa 14:18ec50f38993 469 /* 初期化フラグ立てる */
TakushimaYukimasa 14:18ec50f38993 470 XCooMoveFlag=1;
yoshidayuito 13:c8af1467ba8b 471 }
yoshidayuito 10:9ee22b22e583 472
TakushimaYukimasa 14:18ec50f38993 473 /* 加速・減速それぞれ求める */
TakushimaYukimasa 14:18ec50f38993 474 val_p=(Tar-NowLoc.X)*CONST_ACC_X;
TakushimaYukimasa 14:18ec50f38993 475 val_t=CooCnt.read_ms()*CONST_DEC_X;
yoshidayuito 11:42310638e241 476
TakushimaYukimasa 14:18ec50f38993 477 /* 小さい方を操作量として採択 */
yoshidayuito 11:42310638e241 478 val=MIN(val_p,val_t);
yoshidayuito 10:9ee22b22e583 479
yoshidayuito 11:42310638e241 480 /* -100~100に調整 */
yoshidayuito 10:9ee22b22e583 481 if(val>100) val=100;
yoshidayuito 11:42310638e241 482 else if(val<-100) val=-100;
yoshidayuito 11:42310638e241 483
TakushimaYukimasa 14:18ec50f38993 484 /* X方向のみ操作量を与えてオムニ代入 */
yoshidayuito 16:ea3008995c41 485 SubOmuni(val,0,0);
yoshidayuito 11:42310638e241 486
TakushimaYukimasa 14:18ec50f38993 487 /* 残り距離求める */
yoshidayuito 13:c8af1467ba8b 488 int RemDis=Tar-NowLoc.X;
yoshidayuito 11:42310638e241 489
TakushimaYukimasa 14:18ec50f38993 490 /* 終了条件満たしているなら終了処理 */
TakushimaYukimasa 14:18ec50f38993 491 if(RemDis<=Err) {
TakushimaYukimasa 14:18ec50f38993 492 /* フラグ回収 */
TakushimaYukimasa 14:18ec50f38993 493 XCooMoveFlag=0;
TakushimaYukimasa 14:18ec50f38993 494 return 0;
TakushimaYukimasa 14:18ec50f38993 495 }
TakushimaYukimasa 14:18ec50f38993 496 /* 満たしてないなら残り距離返す */
TakushimaYukimasa 14:18ec50f38993 497 else
TakushimaYukimasa 14:18ec50f38993 498 return RemDis;
yoshidayuito 15:909c23ab7f0c 499 }
yoshidayuito 15:909c23ab7f0c 500
yoshidayuito 15:909c23ab7f0c 501
yoshidayuito 15:909c23ab7f0c 502
yoshidayuito 16:ea3008995c41 503
yoshidayuito 16:ea3008995c41 504 /*******************************************************************************
yoshidayuito 16:ea3008995c41 505 * @概要 オムニの値計算する(加算式)
yoshidayuito 16:ea3008995c41 506 * @引数 X,Y,Rotationそれぞれの操作量
yoshidayuito 16:ea3008995c41 507 * @返り値 なし
yoshidayuito 16:ea3008995c41 508 *******************************************************************************/
yoshidayuito 16:ea3008995c41 509 void SubOmuni(int X,int Y,int R)
yoshidayuito 16:ea3008995c41 510 {
yoshidayuito 16:ea3008995c41 511 /* 入力を100%に制限 */
yoshidayuito 16:ea3008995c41 512 X=Rest(X,100);
yoshidayuito 16:ea3008995c41 513 Y=Rest(Y,100);
yoshidayuito 16:ea3008995c41 514 R=Rest(R,100);
yoshidayuito 16:ea3008995c41 515
yoshidayuito 19:0840e182f96d 516
yoshidayuito 22:9d77148a3f09 517
yoshidayuito 19:0840e182f96d 518
yoshidayuito 16:ea3008995c41 519 /* オムニ計算結果をtmpに */
yoshidayuito 16:ea3008995c41 520 int tmp[4]= {0};
yoshidayuito 22:9d77148a3f09 521
yoshidayuito 19:0840e182f96d 522 /* 一度データをtmpに保存 */
yoshidayuito 22:9d77148a3f09 523 for(int i=0; i<4; i++)
yoshidayuito 19:0840e182f96d 524 tmp[i]=MovMotor[i];
yoshidayuito 22:9d77148a3f09 525
yoshidayuito 19:0840e182f96d 526 /* オムニ計算 */
yoshidayuito 16:ea3008995c41 527 omuni.XmarkOmni_Move(X,Y,R);
yoshidayuito 22:9d77148a3f09 528
yoshidayuito 16:ea3008995c41 529 /* 計算結果を加算する */
yoshidayuito 19:0840e182f96d 530 for(int i=0; i<4; i++)
yoshidayuito 19:0840e182f96d 531 MovMotor[i]+=tmp[i];
yoshidayuito 16:ea3008995c41 532 }
yoshidayuito 16:ea3008995c41 533
yoshidayuito 16:ea3008995c41 534
yoshidayuito 16:ea3008995c41 535
yoshidayuito 16:ea3008995c41 536
yoshidayuito 15:909c23ab7f0c 537 /*******************************************************************************
yoshidayuito 15:909c23ab7f0c 538 * @概要 ライン補正関数
yoshidayuito 23:98b634305bab 539 * @引数 なし
yoshidayuito 15:909c23ab7f0c 540 * @返り値 なし
yoshidayuito 15:909c23ab7f0c 541 *******************************************************************************/
yoshidayuito 23:98b634305bab 542 int LineCor(void)
yoshidayuito 15:909c23ab7f0c 543 {
yoshidayuito 15:909c23ab7f0c 544 /* xの操作量 */
yoshidayuito 16:ea3008995c41 545 int val_x=0;
yoshidayuito 23:98b634305bab 546 /* ライン到達フラグ */
yoshidayuito 15:909c23ab7f0c 547 static bool LineFlag=0;
yoshidayuito 15:909c23ab7f0c 548
yoshidayuito 15:909c23ab7f0c 549 /* どちらもラインに乗っていない状態 */
yoshidayuito 15:909c23ab7f0c 550 if(In.Get(2)==0&&In.Get(3)==0) {
yoshidayuito 17:5b4080915375 551 if(Last_val==0) val_x-=10;
yoshidayuito 17:5b4080915375 552 if(Last_val==1) val_x+=10;
yoshidayuito 22:9d77148a3f09 553 else val_x+=0;
yoshidayuito 15:909c23ab7f0c 554 }
yoshidayuito 18:61d3a2b78aee 555 /* 左側のみ */
yoshidayuito 18:61d3a2b78aee 556 if(In.Get(2)==1&&In.Get(3)==0) {
yoshidayuito 18:61d3a2b78aee 557 val_x-=10;
yoshidayuito 18:61d3a2b78aee 558 Last_val=0;
yoshidayuito 18:61d3a2b78aee 559 }
yoshidayuito 18:61d3a2b78aee 560 /* 右側のみ */
yoshidayuito 18:61d3a2b78aee 561 else if(In.Get(2)==0&&In.Get(3)==1) {
yoshidayuito 18:61d3a2b78aee 562 val_x+=10;
yoshidayuito 18:61d3a2b78aee 563 Last_val=1;
yoshidayuito 18:61d3a2b78aee 564 }
yoshidayuito 18:61d3a2b78aee 565 /* どちらも乗っている */
yoshidayuito 18:61d3a2b78aee 566 else
yoshidayuito 23:98b634305bab 567 /* フラグを立てる */
yoshidayuito 23:98b634305bab 568 LineFlag=1;
yoshidayuito 15:909c23ab7f0c 569
yoshidayuito 15:909c23ab7f0c 570
yoshidayuito 16:ea3008995c41 571 SubOmuni(val_x,0,0);
yoshidayuito 17:5b4080915375 572
yoshidayuito 17:5b4080915375 573 /* 到達 */
yoshidayuito 23:98b634305bab 574 if(LineFlag==1) {
yoshidayuito 23:98b634305bab 575 LineFlag=0;
yoshidayuito 23:98b634305bab 576 return 1;
yoshidayuito 23:98b634305bab 577 }
yoshidayuito 17:5b4080915375 578 /* 未到達 */
yoshidayuito 17:5b4080915375 579 else return 0;
TakushimaYukimasa 0:9e851dc42cde 580 }