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@21:22b863d32705, 2020-03-20 (annotated)

Committer:
yoshidayuito
Date:
Fri Mar 20 04:12:22 2020 +0000
Revision:
21:22b863d32705
Parent:
20:79f96d223e5d
Child:
22:9d77148a3f09
Fix Own Posi;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 7:8bea84f72e64 32 void 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 17:5b4080915375 38 int LineCor(int Line_Dire);
yoshidayuito 16:ea3008995c41 39
yoshidayuito 16:ea3008995c41 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 17:5b4080915375 60 /* ライン補正移動方向 */
yoshidayuito 17:5b4080915375 61 int Line_Dire=0;
yoshidayuito 13:c8af1467ba8b 62
TakushimaYukimasa 0:9e851dc42cde 63 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 64
TakushimaYukimasa 0:9e851dc42cde 65 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 66 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 67
TakushimaYukimasa 0:9e851dc42cde 68 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 69 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 70
TakushimaYukimasa 0:9e851dc42cde 71 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 72 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 73
TakushimaYukimasa 0:9e851dc42cde 74 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 75 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 76
TakushimaYukimasa 0:9e851dc42cde 77 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 78 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 79
TakushimaYukimasa 0:9e851dc42cde 80 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 81 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 82 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 83 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 84
yoshidayuito 1:520233a969e2 85 /* ジャイロ用タイマー */
yoshidayuito 1:520233a969e2 86 Timer yawCnt;
yoshidayuito 1:520233a969e2 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
yoshidayuito 8:367c7d6ef5a3 94 /* I2C Inputのクラス定義 */
yoshidayuito 8:367c7d6ef5a3 95 YKNCT_I2C_IN In(PB_9,PB_8);
yoshidayuito 8:367c7d6ef5a3 96
TakushimaYukimasa 0:9e851dc42cde 97 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 98 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 99
TakushimaYukimasa 0:9e851dc42cde 100
TakushimaYukimasa 0:9e851dc42cde 101 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 102 int main()
TakushimaYukimasa 0:9e851dc42cde 103 {
yoshidayuito 7:8bea84f72e64 104 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 105
yoshidayuito 7:8bea84f72e64 106 /* 割り込みの設定
yoshidayuito 7:8bea84f72e64 107 * IT_CallBack関数を0.1msで割り込み */
yoshidayuito 7:8bea84f72e64 108 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 109
yoshidayuito 7:8bea84f72e64 110 /* ジャイロの設定 */
yoshidayuito 7:8bea84f72e64 111 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 112
yoshidayuito 7:8bea84f72e64 113 /* I2CMDの設定 */
yoshidayuito 7:8bea84f72e64 114 MD.Init(0,MD_SMB);
yoshidayuito 7:8bea84f72e64 115 MD.Init(1,MD_SMB);
yoshidayuito 7:8bea84f72e64 116 MD.Init(2,MD_SMB);
yoshidayuito 7:8bea84f72e64 117 MD.Init(3,MD_SMB);
yoshidayuito 10:9ee22b22e583 118
yoshidayuito 15:909c23ab7f0c 119 /* 足リミット */
yoshidayuito 8:367c7d6ef5a3 120 In.Init(0,0);
yoshidayuito 8:367c7d6ef5a3 121 In.Init(0,1);
TakushimaYukimasa 0:9e851dc42cde 122
yoshidayuito 15:909c23ab7f0c 123 /* ラインセンサ */
yoshidayuito 15:909c23ab7f0c 124 In.Init(0,2);
yoshidayuito 15:909c23ab7f0c 125 In.Init(0,3);
yoshidayuito 15:909c23ab7f0c 126
yoshidayuito 7:8bea84f72e64 127 telemetry.printf("\n\rMainLoopStart");
yoshidayuito 7:8bea84f72e64 128 /* メインループ --------------------------------------------------------------*/
yoshidayuito 11:42310638e241 129 while(1) {
yoshidayuito 7:8bea84f72e64 130 /* オンボードLED点滅 */
yoshidayuito 7:8bea84f72e64 131 led=!led;
yoshidayuito 7:8bea84f72e64 132
yoshidayuito 7:8bea84f72e64 133 /* 表示改行 */
yoshidayuito 7:8bea84f72e64 134 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 135
yoshidayuito 7:8bea84f72e64 136 /* 自動処理関連テレメトリ */
yoshidayuito 7:8bea84f72e64 137 telemetry.printf("ope:%d ",operate);
yoshidayuito 7:8bea84f72e64 138 /* 座標テレメトリ */
yoshidayuito 7:8bea84f72e64 139 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 140
yoshidayuito 7:8bea84f72e64 141 /* 自己位置推定更新 */
yoshidayuito 7:8bea84f72e64 142 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 143
TakushimaYukimasa 14:18ec50f38993 144
TakushimaYukimasa 14:18ec50f38993 145 /* オムニ分のMD代入 */
TakushimaYukimasa 14:18ec50f38993 146 OmuniExe;
TakushimaYukimasa 14:18ec50f38993 147
yoshidayuito 16:ea3008995c41 148 for(int i=0; i<4; i++) {
yoshidayuito 16:ea3008995c41 149 MD.Set(i,MovMotor[i]);
yoshidayuito 16:ea3008995c41 150 MovMotor[i]=0;
yoshidayuito 16:ea3008995c41 151 }
yoshidayuito 16:ea3008995c41 152
yoshidayuito 7:8bea84f72e64 153 /* I2CMD実行 */
yoshidayuito 7:8bea84f72e64 154 MD.Exe();
TakushimaYukimasa 0:9e851dc42cde 155
yoshidayuito 11:42310638e241 156 /* ジャイロ用タイマースタート */
yoshidayuito 7:8bea84f72e64 157 yawCnt.start();
yoshidayuito 11:42310638e241 158 /* ジャイロ用タイマーリセット */
yoshidayuito 7:8bea84f72e64 159 yawCnt.reset();
yoshidayuito 7:8bea84f72e64 160
yoshidayuito 7:8bea84f72e64 161 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshidayuito 7:8bea84f72e64 162 if(DS3.CROSS) operate=0;
yoshidayuito 7:8bea84f72e64 163 if(DS3.TRIANGLE) operate=1;
yoshidayuito 7:8bea84f72e64 164 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 165
yoshidayuito 7:8bea84f72e64 166 /* 操縦権：なし 停止動作 */
yoshidayuito 7:8bea84f72e64 167 if(operate==0) {
yoshidayuito 7:8bea84f72e64 168 /* 足回り停止 */
yoshidayuito 16:ea3008995c41 169 SubOmuni(0,0,0);
yoshidayuito 10:9ee22b22e583 170
yoshidayuito 7:8bea84f72e64 171 }
yoshidayuito 7:8bea84f72e64 172 /* 操縦権：手動 */
yoshidayuito 7:8bea84f72e64 173 else if(operate==1) {
yoshidayuito 7:8bea84f72e64 174 /* 足回り手動動作 */
yoshidayuito 7:8bea84f72e64 175 int x_val = (double)(DS3.LX-64)*100/64;
yoshidayuito 7:8bea84f72e64 176 int y_val = (double)(64-DS3.LY)*100/64;
yoshidayuito 7:8bea84f72e64 177 int r_val = (double)(DS3.RX-64)*100/64;
yoshidayuito 7:8bea84f72e64 178
yoshidayuito 7:8bea84f72e64 179 /* タイマーをリセット */
yoshidayuito 7:8bea84f72e64 180 if(DS3.RX!=64) yawCnt.reset();
yoshidayuito 7:8bea84f72e64 181 /* 目標角度再設定 */
yoshidayuito 7:8bea84f72e64 182 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshidayuito 7:8bea84f72e64 183 /* r_val補正 */
yoshidayuito 7:8bea84f72e64 184 r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;
yoshidayuito 7:8bea84f72e64 185
yoshidayuito 16:ea3008995c41 186 SubOmuni(x_val,y_val,r_val);
yoshidayuito 10:9ee22b22e583 187
yoshidayuito 9:94112b5df540 188 /* 壁あて実行 */
yoshidayuito 9:94112b5df540 189 if(DS3.R1) ToWall();
yoshidayuito 7:8bea84f72e64 190 }
yoshidayuito 7:8bea84f72e64 191 /* 操縦権：自動 */
yoshidayuito 7:8bea84f72e64 192 else if(operate==2) {
yoshidayuito 7:8bea84f72e64 193 switch(auto_mode) {
yoshidayuito 7:8bea84f72e64 194 /* スタート待機処理 */
yoshidayuito 7:8bea84f72e64 195 case 0:
yoshidayuito 20:79f96d223e5d 196 /* STARTで次のステップに */
yoshidayuito 20:79f96d223e5d 197 if(DS3.START) auto_mode++;
yoshidayuito 7:8bea84f72e64 198 break;
yoshidayuito 7:8bea84f72e64 199
yoshidayuito 17:5b4080915375 200 /* X座標移動処理 */
yoshidayuito 7:8bea84f72e64 201 case 1:
yoshidayuito 13:c8af1467ba8b 202 /* 動作終了時次のステップに */
yoshidayuito 13:c8af1467ba8b 203 if(XCooMove(1000,50)==0)
yoshidayuito 13:c8af1467ba8b 204 auto_mode++;
TakushimaYukimasa 14:18ec50f38993 205
yoshidayuito 7:8bea84f72e64 206 break;
yoshidayuito 7:8bea84f72e64 207
yoshidayuito 17:5b4080915375 208 /* ライン補正処理 */
yoshidayuito 17:5b4080915375 209 case 2:
yoshidayuito 17:5b4080915375 210 /* ライン補正完了後次のステップに */
yoshidayuito 17:5b4080915375 211 if(LineCor(Line_Dire)==1)
yoshidayuito 17:5b4080915375 212 auto_mode++;
yoshidayuito 17:5b4080915375 213
yoshidayuito 17:5b4080915375 214 break;
yoshidayuito 17:5b4080915375 215
yoshidayuito 7:8bea84f72e64 216 /* 終了処理 */
yoshidayuito 7:8bea84f72e64 217 default:
yoshidayuito 7:8bea84f72e64 218 auto_mode=0;
yoshidayuito 7:8bea84f72e64 219 operate=0;
yoshidayuito 7:8bea84f72e64 220 break;
yoshidayuito 7:8bea84f72e64 221 }
yoshidayuito 7:8bea84f72e64 222 }
yoshidayuito 7:8bea84f72e64 223
TakushimaYukimasa 0:9e851dc42cde 224 }
TakushimaYukimasa 0:9e851dc42cde 225 }
TakushimaYukimasa 0:9e851dc42cde 226
TakushimaYukimasa 0:9e851dc42cde 227
TakushimaYukimasa 0:9e851dc42cde 228 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 229 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 230 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 231 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 232 *******************************************************************************/
yoshidayuito 7:8bea84f72e64 233 void LocEstimate(void)
yoshidayuito 7:8bea84f72e64 234 {
yoshidayuito 21:22b863d32705 235 static double GyroDeg[2]= {0};
yoshidayuito 7:8bea84f72e64 236 static double EncDeg[2][2]= {0};
yoshidayuito 7:8bea84f72e64 237 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 238
yoshidayuito 21:22b863d32705 239
yoshidayuito 21:22b863d32705 240
yoshidayuito 7:8bea84f72e64 241 /* ジャイロの値取得 */
yoshidayuito 7:8bea84f72e64 242 bno.get_angles();
yoshidayuito 7:8bea84f72e64 243 GyroDeg[1]=GyroDeg[0];
yoshidayuito 7:8bea84f72e64 244 GyroDeg[0]=bno.euler.yaw;
yoshidayuito 7:8bea84f72e64 245 if(GyroDeg[0]!=0) {
yoshidayuito 7:8bea84f72e64 246 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 247 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshidayuito 7:8bea84f72e64 248 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 249 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshidayuito 7:8bea84f72e64 250 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 251 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshidayuito 7:8bea84f72e64 252 }
yoshidayuito 21:22b863d32705 253 /* Enc2つの差分求める */
yoshidayuito 21:22b863d32705 254 for(int i=0; i<2; i++) {
yoshidayuito 7:8bea84f72e64 255 EncDeg[i][1]=EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 256 EncDeg[i][0]=EncoderDeg[i];
yoshidayuito 21:22b863d32705 257 disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
yoshidayuito 7:8bea84f72e64 258 }
yoshidayuito 7:8bea84f72e64 259 /* 差分を加速度として保存 */
yoshidayuito 7:8bea84f72e64 260 NowAcc.theta = disp[2];
yoshidayuito 21:22b863d32705 261 NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
yoshidayuito 21:22b863d32705 262 NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
yoshidayuito 7:8bea84f72e64 263 /* 差分を累積して現在位置を保存 */
yoshidayuito 7:8bea84f72e64 264 NowLoc.X += NowAcc.X;
yoshidayuito 21:22b863d32705 265 NowLoc.Y += NowAcc.Y;
yoshidayuito 7:8bea84f72e64 266 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 267 }
TakushimaYukimasa 0:9e851dc42cde 268
TakushimaYukimasa 0:9e851dc42cde 269
TakushimaYukimasa 0:9e851dc42cde 270
TakushimaYukimasa 0:9e851dc42cde 271
TakushimaYukimasa 0:9e851dc42cde 272 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 273 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 274 {
yoshidayuito 7:8bea84f72e64 275 static int cnt = 0;
yoshidayuito 7:8bea84f72e64 276 static int data[EncoderMAX] = {0};
yoshidayuito 7:8bea84f72e64 277 static double EncDeg[EncoderMAX][2] = {0};
TakushimaYukimasa 0:9e851dc42cde 278
yoshidayuito 7:8bea84f72e64 279 for(int i=0; i<EncoderMAX; i++)
yoshidayuito 7:8bea84f72e64 280 switch(cnt) {
yoshidayuito 7:8bea84f72e64 281 /* 最初の処理 */
yoshidayuito 7:8bea84f72e64 282 case 0:
yoshidayuito 7:8bea84f72e64 283 data[i] = 0;
yoshidayuito 7:8bea84f72e64 284 CS[i] = 0;
yoshidayuito 7:8bea84f72e64 285 CL[i] = 1;
yoshidayuito 7:8bea84f72e64 286 break;
yoshidayuito 7:8bea84f72e64 287 /* 最後の処理 */
yoshidayuito 7:8bea84f72e64 288 case 25:
yoshidayuito 7:8bea84f72e64 289 CS[i]=1;
yoshidayuito 7:8bea84f72e64 290 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshidayuito 7:8bea84f72e64 291 EncDeg[i][1] = EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 292 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshidayuito 7:8bea84f72e64 293 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 294 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshidayuito 7:8bea84f72e64 295 EncDeg[i][1] -= 360;
yoshidayuito 7:8bea84f72e64 296 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 297 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshidayuito 7:8bea84f72e64 298 EncDeg[i][1] += 360;
yoshidayuito 7:8bea84f72e64 299 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 300 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshidayuito 7:8bea84f72e64 301 break;
yoshidayuito 7:8bea84f72e64 302 /* 通常の処理 */
yoshidayuito 7:8bea84f72e64 303 default:
yoshidayuito 7:8bea84f72e64 304 CL[i]=!CL[i];
yoshidayuito 7:8bea84f72e64 305 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshidayuito 7:8bea84f72e64 306 if(cnt != 1 && cnt % 2) {
yoshidayuito 7:8bea84f72e64 307 data[i] |= (DO[i]==1);
yoshidayuito 7:8bea84f72e64 308 data[i] = data[i] << 1;
yoshidayuito 7:8bea84f72e64 309 }
yoshidayuito 7:8bea84f72e64 310 break;
yoshidayuito 7:8bea84f72e64 311 }
yoshidayuito 7:8bea84f72e64 312 cnt++;
yoshidayuito 7:8bea84f72e64 313 cnt%=26;
yoshidayuito 7:8bea84f72e64 314 }
yoshidayuito 7:8bea84f72e64 315
yoshidayuito 7:8bea84f72e64 316
yoshidayuito 7:8bea84f72e64 317
yoshidayuito 7:8bea84f72e64 318
yoshidayuito 7:8bea84f72e64 319 /*******************************************************************************
yoshidayuito 7:8bea84f72e64 320 * @概要 壁あて関数
yoshidayuito 7:8bea84f72e64 321 * @引数 なし
yoshidayuito 7:8bea84f72e64 322 * @返り値 なし
yoshidayuito 7:8bea84f72e64 323 *******************************************************************************/
yoshidayuito 7:8bea84f72e64 324 void ToWall(void)
yoshidayuito 7:8bea84f72e64 325 {
TakushimaYukimasa 14:18ec50f38993 326 /* 左側リミットに応じて左2輪の操作量を決める */
yoshidayuito 9:94112b5df540 327 if(In.Get(0)==0)
yoshidayuito 7:8bea84f72e64 328 for(int i=0; i<2; i++) MovMotor[i]-=10;
TakushimaYukimasa 14:18ec50f38993 329
TakushimaYukimasa 14:18ec50f38993 330 /* 右側リミットに応じて左2輪の操作量を決める */
yoshidayuito 9:94112b5df540 331 if(In.Get(1)==0)
yoshidayuito 7:8bea84f72e64 332 for(int i=2; i<4; i++) MovMotor[i]+=10;
yoshidayuito 10:9ee22b22e583 333 }
yoshidayuito 10:9ee22b22e583 334
yoshidayuito 10:9ee22b22e583 335
yoshidayuito 10:9ee22b22e583 336
yoshidayuito 10:9ee22b22e583 337 /*******************************************************************************
yoshidayuito 10:9ee22b22e583 338 * @概要 x座標移動関数
yoshidayuito 11:42310638e241 339 * @引数 Tar:目標値
yoshidayuito 10:9ee22b22e583 340 * @引数 Err :許容誤差
yoshidayuito 10:9ee22b22e583 341 * @返り値 val_p:残り距離
yoshidayuito 10:9ee22b22e583 342 *******************************************************************************/
yoshidayuito 11:42310638e241 343 int XCooMove(int Tar,int Err)
yoshidayuito 10:9ee22b22e583 344 {
TakushimaYukimasa 14:18ec50f38993 345 /* x座標移動初期化フラグ */
TakushimaYukimasa 14:18ec50f38993 346 static bool XCooMoveFlag=0;
TakushimaYukimasa 14:18ec50f38993 347
TakushimaYukimasa 14:18ec50f38993 348 /* x座標移動タイマー */
TakushimaYukimasa 14:18ec50f38993 349 static Timer CooCnt;
TakushimaYukimasa 14:18ec50f38993 350
TakushimaYukimasa 14:18ec50f38993 351 /* 速度 加速・減速・最終操作量 */
yoshidayuito 11:42310638e241 352 int val_p=0,val_t=0,val=0;
yoshidayuito 10:9ee22b22e583 353
TakushimaYukimasa 14:18ec50f38993 354 /* X座標移動最初だけ初期化 */
TakushimaYukimasa 14:18ec50f38993 355 if(XCooMoveFlag==0) {
yoshidayuito 13:c8af1467ba8b 356 /* タイマースタート */
yoshidayuito 13:c8af1467ba8b 357 CooCnt.start();
yoshidayuito 13:c8af1467ba8b 358 /* X座標移動用タイマーリセット */
yoshidayuito 13:c8af1467ba8b 359 CooCnt.reset();
TakushimaYukimasa 14:18ec50f38993 360 /* 初期化フラグ立てる */
TakushimaYukimasa 14:18ec50f38993 361 XCooMoveFlag=1;
yoshidayuito 13:c8af1467ba8b 362 }
yoshidayuito 10:9ee22b22e583 363
TakushimaYukimasa 14:18ec50f38993 364 /* 加速・減速それぞれ求める */
TakushimaYukimasa 14:18ec50f38993 365 val_p=(Tar-NowLoc.X)*CONST_ACC_X;
TakushimaYukimasa 14:18ec50f38993 366 val_t=CooCnt.read_ms()*CONST_DEC_X;
yoshidayuito 11:42310638e241 367
TakushimaYukimasa 14:18ec50f38993 368 /* 小さい方を操作量として採択 */
yoshidayuito 11:42310638e241 369 val=MIN(val_p,val_t);
yoshidayuito 10:9ee22b22e583 370
yoshidayuito 11:42310638e241 371 /* -100~100に調整 */
yoshidayuito 10:9ee22b22e583 372 if(val>100) val=100;
yoshidayuito 11:42310638e241 373 else if(val<-100) val=-100;
yoshidayuito 11:42310638e241 374
TakushimaYukimasa 14:18ec50f38993 375 /* X方向のみ操作量を与えてオムニ代入 */
yoshidayuito 16:ea3008995c41 376 SubOmuni(val,0,0);
yoshidayuito 11:42310638e241 377
TakushimaYukimasa 14:18ec50f38993 378 /* 残り距離求める */
yoshidayuito 13:c8af1467ba8b 379 int RemDis=Tar-NowLoc.X;
yoshidayuito 11:42310638e241 380
TakushimaYukimasa 14:18ec50f38993 381 /* 終了条件満たしているなら終了処理 */
TakushimaYukimasa 14:18ec50f38993 382 if(RemDis<=Err) {
TakushimaYukimasa 14:18ec50f38993 383 /* フラグ回収 */
TakushimaYukimasa 14:18ec50f38993 384 XCooMoveFlag=0;
TakushimaYukimasa 14:18ec50f38993 385 return 0;
TakushimaYukimasa 14:18ec50f38993 386 }
TakushimaYukimasa 14:18ec50f38993 387 /* 満たしてないなら残り距離返す */
TakushimaYukimasa 14:18ec50f38993 388 else
TakushimaYukimasa 14:18ec50f38993 389 return RemDis;
yoshidayuito 15:909c23ab7f0c 390 }
yoshidayuito 15:909c23ab7f0c 391
yoshidayuito 15:909c23ab7f0c 392
yoshidayuito 15:909c23ab7f0c 393
yoshidayuito 16:ea3008995c41 394
yoshidayuito 16:ea3008995c41 395 /*******************************************************************************
yoshidayuito 16:ea3008995c41 396 * @概要 オムニの値計算する(加算式)
yoshidayuito 16:ea3008995c41 397 * @引数 X,Y,Rotationそれぞれの操作量
yoshidayuito 16:ea3008995c41 398 * @返り値 なし
yoshidayuito 16:ea3008995c41 399 *******************************************************************************/
yoshidayuito 16:ea3008995c41 400 void SubOmuni(int X,int Y,int R)
yoshidayuito 16:ea3008995c41 401 {
yoshidayuito 16:ea3008995c41 402 /* 入力を100%に制限 */
yoshidayuito 16:ea3008995c41 403 X=Rest(X,100);
yoshidayuito 16:ea3008995c41 404 Y=Rest(Y,100);
yoshidayuito 16:ea3008995c41 405 R=Rest(R,100);
yoshidayuito 16:ea3008995c41 406
yoshidayuito 19:0840e182f96d 407
yoshidayuito 19:0840e182f96d 408
yoshidayuito 19:0840e182f96d 409
yoshidayuito 16:ea3008995c41 410 /* オムニ計算結果をtmpに */
yoshidayuito 16:ea3008995c41 411 int tmp[4]= {0};
yoshidayuito 19:0840e182f96d 412
yoshidayuito 19:0840e182f96d 413 /* 一度データをtmpに保存 */
yoshidayuito 19:0840e182f96d 414 for(int i=0;i<4;i++)
yoshidayuito 19:0840e182f96d 415 tmp[i]=MovMotor[i];
yoshidayuito 19:0840e182f96d 416
yoshidayuito 19:0840e182f96d 417 /* オムニ計算 */
yoshidayuito 16:ea3008995c41 418 omuni.XmarkOmni_Move(X,Y,R);
yoshidayuito 19:0840e182f96d 419
yoshidayuito 16:ea3008995c41 420 /* 計算結果を加算する */
yoshidayuito 19:0840e182f96d 421 for(int i=0; i<4; i++)
yoshidayuito 19:0840e182f96d 422 MovMotor[i]+=tmp[i];
yoshidayuito 16:ea3008995c41 423 }
yoshidayuito 16:ea3008995c41 424
yoshidayuito 16:ea3008995c41 425
yoshidayuito 16:ea3008995c41 426
yoshidayuito 16:ea3008995c41 427
yoshidayuito 15:909c23ab7f0c 428 /*******************************************************************************
yoshidayuito 15:909c23ab7f0c 429 * @概要 ライン補正関数
yoshidayuito 15:909c23ab7f0c 430 * @引数 なし
yoshidayuito 15:909c23ab7f0c 431 * @返り値 なし
yoshidayuito 15:909c23ab7f0c 432 *******************************************************************************/
yoshidayuito 17:5b4080915375 433 int LineCor(int Line_Dire)
yoshidayuito 15:909c23ab7f0c 434 {
yoshidayuito 15:909c23ab7f0c 435 /* xの操作量 */
yoshidayuito 16:ea3008995c41 436 int val_x=0;
yoshidayuito 15:909c23ab7f0c 437 /* 左側,右側の前回の値 */
yoshidayuito 17:5b4080915375 438 static int Last_val;
yoshidayuito 15:909c23ab7f0c 439 /* ライン補正初期化フラグ */
yoshidayuito 15:909c23ab7f0c 440 static bool LineFlag=0;
yoshidayuito 15:909c23ab7f0c 441
yoshidayuito 15:909c23ab7f0c 442 if(LineFlag==0) {
yoshidayuito 17:5b4080915375 443 /* 初期値設定 0…左 1…右 */
yoshidayuito 17:5b4080915375 444 Last_val=Line_Dire;
yoshidayuito 15:909c23ab7f0c 445 /* 初期化フラグ立てる */
yoshidayuito 15:909c23ab7f0c 446 LineFlag=1;
yoshidayuito 15:909c23ab7f0c 447 }
yoshidayuito 15:909c23ab7f0c 448
yoshidayuito 15:909c23ab7f0c 449 /* どちらもラインに乗っていない状態 */
yoshidayuito 15:909c23ab7f0c 450 if(In.Get(2)==0&&In.Get(3)==0) {
yoshidayuito 17:5b4080915375 451 if(Last_val==0) val_x-=10;
yoshidayuito 17:5b4080915375 452 if(Last_val==1) val_x+=10;
yoshidayuito 15:909c23ab7f0c 453 }
yoshidayuito 16:ea3008995c41 454
yoshidayuito 18:61d3a2b78aee 455 /* 左側のみ */
yoshidayuito 18:61d3a2b78aee 456 if(In.Get(2)==1&&In.Get(3)==0) {
yoshidayuito 18:61d3a2b78aee 457 val_x-=10;
yoshidayuito 18:61d3a2b78aee 458 Last_val=0;
yoshidayuito 18:61d3a2b78aee 459 }
yoshidayuito 18:61d3a2b78aee 460 /* 右側のみ */
yoshidayuito 18:61d3a2b78aee 461 else if(In.Get(2)==0&&In.Get(3)==1) {
yoshidayuito 18:61d3a2b78aee 462 val_x+=10;
yoshidayuito 18:61d3a2b78aee 463 Last_val=1;
yoshidayuito 18:61d3a2b78aee 464 }
yoshidayuito 18:61d3a2b78aee 465 /* どちらも乗っている */
yoshidayuito 18:61d3a2b78aee 466 else
yoshidayuito 18:61d3a2b78aee 467 /* フラグ回収 */
yoshidayuito 18:61d3a2b78aee 468 LineFlag=0;
yoshidayuito 15:909c23ab7f0c 469
yoshidayuito 15:909c23ab7f0c 470
yoshidayuito 16:ea3008995c41 471 SubOmuni(val_x,0,0);
yoshidayuito 17:5b4080915375 472
yoshidayuito 17:5b4080915375 473 /* 到達 */
yoshidayuito 17:5b4080915375 474 if(LineFlag==0) return 1;
yoshidayuito 17:5b4080915375 475 /* 未到達 */
yoshidayuito 17:5b4080915375 476 else return 0;
TakushimaYukimasa 0:9e851dc42cde 477 }