YKNCT / Mbed 2 deprecated Triton

Ver Tikutaikai

Dependencies:   mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C

main.cpp@0:c527a14d991a, 2021-10-12 (annotated)

Committer:
ac0314naga
Date:
Tue Oct 12 03:57:59 2021 +0000
Revision:
0:c527a14d991a
Child:
1:fa3652aca312
triton

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ac0314naga 0:c527a14d991a 1 /* Includes -----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 2 #include <main.h>
ac0314naga 0:c527a14d991a 3 #include <target.h>
ac0314naga 0:c527a14d991a 4
ac0314naga 0:c527a14d991a 5 /* 型定義
ac0314naga 0:c527a14d991a 6 * ---------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 7
ac0314naga 0:c527a14d991a 8 /* ロボットの加速度 */
ac0314naga 0:c527a14d991a 9 LOCATION NowAcc;
ac0314naga 0:c527a14d991a 10 /* ロボットの座標 */
ac0314naga 0:c527a14d991a 11 LOCATION NowLoc;
ac0314naga 0:c527a14d991a 12
ac0314naga 0:c527a14d991a 13 /* 定数定義
ac0314naga 0:c527a14d991a 14 * -------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 15
ac0314naga 0:c527a14d991a 16
ac0314naga 0:c527a14d991a 17 /* 関数プロトタイプ宣言
ac0314naga 0:c527a14d991a 18 * --------------------------------------------------------*/
ac0314naga 0:c527a14d991a 19 void uartRX(void);
ac0314naga 0:c527a14d991a 20
ac0314naga 0:c527a14d991a 21 void IT_CallBack(void);
ac0314naga 0:c527a14d991a 22
ac0314naga 0:c527a14d991a 23 /* PID */
ac0314naga 0:c527a14d991a 24 double LocationPID(double, double);
ac0314naga 0:c527a14d991a 25
ac0314naga 0:c527a14d991a 26 /* 値制限 */
ac0314naga 0:c527a14d991a 27 double Range(double, double);
ac0314naga 0:c527a14d991a 28
ac0314naga 0:c527a14d991a 29 /* 角速度算出関数 */
ac0314naga 0:c527a14d991a 30 int RotationPerWhile(int, int, int);
ac0314naga 0:c527a14d991a 31
ac0314naga 0:c527a14d991a 32 /* 足回り */
ac0314naga 0:c527a14d991a 33 void InverseTriOmni(int *, double, double, double);
ac0314naga 0:c527a14d991a 34
ac0314naga 0:c527a14d991a 35 /* 自己位置推定関数 */
ac0314naga 0:c527a14d991a 36 void LocEstimate(void);
ac0314naga 0:c527a14d991a 37
ac0314naga 0:c527a14d991a 38 /* 変数定義
ac0314naga 0:c527a14d991a 39 * -------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 40
ac0314naga 0:c527a14d991a 41 /* 足回り値保存変数 */
ac0314naga 0:c527a14d991a 42 int MovMotor[MotorMAX] = {0};
ac0314naga 0:c527a14d991a 43
ac0314naga 0:c527a14d991a 44 /* 周期保存用 */
ac0314naga 0:c527a14d991a 45 int whileTime_us = 0;
ac0314naga 0:c527a14d991a 46
ac0314naga 0:c527a14d991a 47 /* コントローラ */
ac0314naga 0:c527a14d991a 48 int operate = 0;
ac0314naga 0:c527a14d991a 49
ac0314naga 0:c527a14d991a 50 /* ジャグリング機構 動かすか否か */
ac0314naga 0:c527a14d991a 51 int isMoveInjection = 0;
ac0314naga 0:c527a14d991a 52
ac0314naga 0:c527a14d991a 53 /* リボン回転機構 動かすか否か */
ac0314naga 0:c527a14d991a 54 int isMoveRibbon = 0;
ac0314naga 0:c527a14d991a 55
ac0314naga 0:c527a14d991a 56 /* 足回りエンコーダー */
ac0314naga 0:c527a14d991a 57 double EncoderDeg[EncoderMAX - 6] = {0};
ac0314naga 0:c527a14d991a 58
ac0314naga 0:c527a14d991a 59 /* 手動オムニ目標角 */
ac0314naga 0:c527a14d991a 60 double TarTheta = 0;
ac0314naga 0:c527a14d991a 61
ac0314naga 0:c527a14d991a 62 /* 目標番号 */
ac0314naga 0:c527a14d991a 63 int targetNum = 0;
ac0314naga 0:c527a14d991a 64
ac0314naga 0:c527a14d991a 65 /* タイマー関連 */
ac0314naga 0:c527a14d991a 66 /* 自身の状況 1-warning 2-normal */
ac0314naga 0:c527a14d991a 67 int statious = 2;
ac0314naga 0:c527a14d991a 68
ac0314naga 0:c527a14d991a 69 /* 時間[ms] */
ac0314naga 0:c527a14d991a 70 double msTimer = 0;
ac0314naga 0:c527a14d991a 71
ac0314naga 0:c527a14d991a 72 /* メインタイマー制御 0-pause 1-start */
ac0314naga 0:c527a14d991a 73 bool timerMode = 0;
ac0314naga 0:c527a14d991a 74
ac0314naga 0:c527a14d991a 75 /* クラス定義 ----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 76
ac0314naga 0:c527a14d991a 77 /* 割り込み用クラス */
ac0314naga 0:c527a14d991a 78 Ticker flipper;
ac0314naga 0:c527a14d991a 79
ac0314naga 0:c527a14d991a 80 /* UART (Tx,Rx) */
ac0314naga 0:c527a14d991a 81 Serial pc(USBTX, USBRX, 115200);
ac0314naga 0:c527a14d991a 82
ac0314naga 0:c527a14d991a 83 /* XBee送受信 */
ac0314naga 0:c527a14d991a 84 Serial UART(PC_10, PC_11, 9600);
ac0314naga 0:c527a14d991a 85
ac0314naga 0:c527a14d991a 86 /* コントローラー */
ac0314naga 0:c527a14d991a 87 SBDBT DS3(PA_0, PA_1, 9600);
ac0314naga 0:c527a14d991a 88
ac0314naga 0:c527a14d991a 89 /* I2C MDのクラス定義 */
ac0314naga 0:c527a14d991a 90 YKNCT_MD_I2C MD(PB_9, PB_8);
ac0314naga 0:c527a14d991a 91
ac0314naga 0:c527a14d991a 92 /* ジャイロのピン設定 */
ac0314naga 0:c527a14d991a 93 BNO055 bno(PB_9, PB_8);
ac0314naga 0:c527a14d991a 94
ac0314naga 0:c527a14d991a 95
ac0314naga 0:c527a14d991a 96 DigitalOut led(LED2);
ac0314naga 0:c527a14d991a 97 /* リボン射出ピン */
ac0314naga 0:c527a14d991a 98 DigitalOut Rod(PC_2);
ac0314naga 0:c527a14d991a 99
ac0314naga 0:c527a14d991a 100 /* エンコーダーピン設定 */
ac0314naga 0:c527a14d991a 101 DigitalOut CS(PB_4);
ac0314naga 0:c527a14d991a 102 DigitalOut CL(PA_10);
ac0314naga 0:c527a14d991a 103 DigitalIn Do[EncoderMAX] = {PB_12, PB_2, PB_1, PB_15, PB_14, PB_13, PC_4, PB_5};
ac0314naga 0:c527a14d991a 104 //DigitalIn Do[EncoderMAX] = {PB_15, PB_14, PB_13, PB_12, PB_2, PB_1, PC_4, PB_5};
ac0314naga 0:c527a14d991a 105
ac0314naga 0:c527a14d991a 106 /* カタパルト二台分の領域確保 */
ac0314naga 0:c527a14d991a 107 INJECTION injection[2] = {
ac0314naga 0:c527a14d991a 108 INJECTION(PB_10, PA_8, PA_9),
ac0314naga 0:c527a14d991a 109 INJECTION(PA_4, PB_0, PC_1)
ac0314naga 0:c527a14d991a 110 }; // pa_6,pa_7
ac0314naga 0:c527a14d991a 111 //PA_8,PB_0
ac0314naga 0:c527a14d991a 112
ac0314naga 0:c527a14d991a 113 DigitalIn limitRibbonTop(PC_0);
ac0314naga 0:c527a14d991a 114 DigitalIn limitRibbonBottom(PC_3);
ac0314naga 0:c527a14d991a 115
ac0314naga 0:c527a14d991a 116 Timer WaitNextShot;
ac0314naga 0:c527a14d991a 117
ac0314naga 0:c527a14d991a 118 /*----------------------------------- main ----------------------------------*/
ac0314naga 0:c527a14d991a 119 int main()
ac0314naga 0:c527a14d991a 120 {
ac0314naga 0:c527a14d991a 121 /* I2CMDの設定 */
ac0314naga 0:c527a14d991a 122 for (int i = 0; i < MotorMAX; i++) MD.Init(i, MD_SMB);
ac0314naga 0:c527a14d991a 123 for (int i = 0; i < EncoderMAX; i++) Do[i].mode(PullDown);
ac0314naga 0:c527a14d991a 124
ac0314naga 0:c527a14d991a 125 /* 割り込みの設定
ac0314naga 0:c527a14d991a 126 * IT_CallBack関数を0.1msで割り込み */
ac0314naga 0:c527a14d991a 127 flipper.attach_us(&IT_CallBack, 100);
ac0314naga 0:c527a14d991a 128
ac0314naga 0:c527a14d991a 129 /* uart受信割込み設定 */
ac0314naga 0:c527a14d991a 130 UART.attach(uartRX, Serial::RxIrq);
ac0314naga 0:c527a14d991a 131
ac0314naga 0:c527a14d991a 132 /* ジャイロの設定 */
ac0314naga 0:c527a14d991a 133 wait(0.5);
ac0314naga 0:c527a14d991a 134 bno.setmode(OPERATION_MODE_IMUPLUS);
ac0314naga 0:c527a14d991a 135
ac0314naga 0:c527a14d991a 136 /* 動作時間計測用 */
ac0314naga 0:c527a14d991a 137 Timer whileTimer;
ac0314naga 0:c527a14d991a 138 Timer yawCnt;
ac0314naga 0:c527a14d991a 139
ac0314naga 0:c527a14d991a 140 /* 計測開始 */
ac0314naga 0:c527a14d991a 141 whileTimer.start();
ac0314naga 0:c527a14d991a 142 yawCnt.start();
ac0314naga 0:c527a14d991a 143 WaitNextShot.start();
ac0314naga 0:c527a14d991a 144
ac0314naga 0:c527a14d991a 145 /* 小さくすると深くなる */
ac0314naga 0:c527a14d991a 146 injection[0].initalDeg = 29;
ac0314naga 0:c527a14d991a 147 injection[1].initalDeg = 52;
ac0314naga 0:c527a14d991a 148
ac0314naga 0:c527a14d991a 149 injection[0].targetRps[0] = -28;
ac0314naga 0:c527a14d991a 150 injection[0].targetRps[1] = 28;
ac0314naga 0:c527a14d991a 151 injection[1].targetRps[0] = -25;
ac0314naga 0:c527a14d991a 152 injection[1].targetRps[1] = 25;
ac0314naga 0:c527a14d991a 153
ac0314naga 0:c527a14d991a 154
ac0314naga 0:c527a14d991a 155 /* メインループ ----------------------------------------------------------*/
ac0314naga 0:c527a14d991a 156 while (1) {
ac0314naga 0:c527a14d991a 157
ac0314naga 0:c527a14d991a 158 led = !led;
ac0314naga 0:c527a14d991a 159
ac0314naga 0:c527a14d991a 160 for (int i = 0; i < MotorMAX; i++) {
ac0314naga 0:c527a14d991a 161 MD.Set(i, MovMotor[i]);
ac0314naga 0:c527a14d991a 162 MovMotor[i] = 0;
ac0314naga 0:c527a14d991a 163 }
ac0314naga 0:c527a14d991a 164 for (int i = 0; i < 3; i++) {
ac0314naga 0:c527a14d991a 165 injection[0].moveMotor[i] = injection[1].moveMotor[i] = 0;
ac0314naga 0:c527a14d991a 166 }
ac0314naga 0:c527a14d991a 167 MD.Exe();
ac0314naga 0:c527a14d991a 168
ac0314naga 0:c527a14d991a 169 /* 自己位置計算 */
ac0314naga 0:c527a14d991a 170 LocEstimate();
ac0314naga 0:c527a14d991a 171
ac0314naga 0:c527a14d991a 172 /* 操縦権変更 ×停止 〇射出自動 △足回り手動 */
ac0314naga 0:c527a14d991a 173 if (DS3.CROSS) operate = 0;
ac0314naga 0:c527a14d991a 174 if (DS3.CIRCLE) operate = 1;
ac0314naga 0:c527a14d991a 175 if (DS3.TRIANGLE) operate = 2;
ac0314naga 0:c527a14d991a 176 if (DS3.SQUARE) operate = 3;
ac0314naga 0:c527a14d991a 177
ac0314naga 0:c527a14d991a 178 /* STARTで座標系リセット */
ac0314naga 0:c527a14d991a 179 if (DS3.START) {
ac0314naga 0:c527a14d991a 180 NowLoc.reset();
ac0314naga 0:c527a14d991a 181 TarTheta=0.0;
ac0314naga 0:c527a14d991a 182 }
ac0314naga 0:c527a14d991a 183
ac0314naga 0:c527a14d991a 184 /* CS送信ステータス更新 */
ac0314naga 0:c527a14d991a 185 statious=(operate==1)+1;
ac0314naga 0:c527a14d991a 186
ac0314naga 0:c527a14d991a 187 /* 停止以外でジャグリングとリボンの処理 */
ac0314naga 0:c527a14d991a 188 if (operate != 0) {
ac0314naga 0:c527a14d991a 189
ac0314naga 0:c527a14d991a 190 static int nextCnt = 0;
ac0314naga 0:c527a14d991a 191
ac0314naga 0:c527a14d991a 192 if(isMoveInjection && WaitNextShot.read_ms() > STEP(targetNum) && !injection[(nextCnt+1)%2].isMove) {
ac0314naga 0:c527a14d991a 193 injection[nextCnt%2].isMove = 1;
ac0314naga 0:c527a14d991a 194 WaitNextShot.stop();
ac0314naga 0:c527a14d991a 195 nextCnt++;
ac0314naga 0:c527a14d991a 196 }
ac0314naga 0:c527a14d991a 197
ac0314naga 0:c527a14d991a 198
ac0314naga 0:c527a14d991a 199 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 200 /* 動作状態を決める */
ac0314naga 0:c527a14d991a 201 // injection[i].isMove = isMoveInjection && injection[!i].WaitNextShot.read_ms() > step;
ac0314naga 0:c527a14d991a 202 injection[i].MainFunction();
ac0314naga 0:c527a14d991a 203 }
ac0314naga 0:c527a14d991a 204
ac0314naga 0:c527a14d991a 205 /* 出力 */
ac0314naga 0:c527a14d991a 206 for (int i = 0; i < 6; i++) {
ac0314naga 0:c527a14d991a 207 MovMotor[i] = Range(injection[i / 3].moveMotor[i % 3], 100);
ac0314naga 0:c527a14d991a 208 }
ac0314naga 0:c527a14d991a 209
ac0314naga 0:c527a14d991a 210 /* リボン */
ac0314naga 0:c527a14d991a 211 if(isMoveRibbon==1) MovMotor[9] = 70;//70
ac0314naga 0:c527a14d991a 212 else if(isMoveRibbon==2) {
ac0314naga 0:c527a14d991a 213 if(limitRibbonTop) MovMotor[10] = 50;
ac0314naga 0:c527a14d991a 214 else MovMotor[9] = 70;
ac0314naga 0:c527a14d991a 215 } else if(!limitRibbonBottom)MovMotor[9] = -60;
ac0314naga 0:c527a14d991a 216
ac0314naga 0:c527a14d991a 217 // if(isMoveRibbon) {
ac0314naga 0:c527a14d991a 218 // if(limitRibbonTop) MovMotor[10] = 50;
ac0314naga 0:c527a14d991a 219 // else MovMotor[9] = 70;
ac0314naga 0:c527a14d991a 220 // } else {
ac0314naga 0:c527a14d991a 221 // if(!limitRibbonBottom) MovMotor[9] = -60;
ac0314naga 0:c527a14d991a 222 // }
ac0314naga 0:c527a14d991a 223 }
ac0314naga 0:c527a14d991a 224
ac0314naga 0:c527a14d991a 225 /* 停止処理 */
ac0314naga 0:c527a14d991a 226 if (operate == 0) {
ac0314naga 0:c527a14d991a 227 for (int i = 0; i < MotorMAX; i++) MovMotor[i] = 0;
ac0314naga 0:c527a14d991a 228 for (int i = 0; i < 2; i++)
ac0314naga 0:c527a14d991a 229 injection[0].moveMotor[i] = injection[1].moveMotor[i] = 0;
ac0314naga 0:c527a14d991a 230 for (int i = 0; i < 2; i++)
ac0314naga 0:c527a14d991a 231 injection[0].updateVal[i][1] = injection[1].updateVal[i][1] = 0;
ac0314naga 0:c527a14d991a 232 Rod = 0;
ac0314naga 0:c527a14d991a 233 TarTheta=NowLoc.Theta;
ac0314naga 0:c527a14d991a 234 }
ac0314naga 0:c527a14d991a 235
ac0314naga 0:c527a14d991a 236 /* 足回り自動 */
ac0314naga 0:c527a14d991a 237 else if (operate == 1) {
ac0314naga 0:c527a14d991a 238 targetNum = 1;
ac0314naga 0:c527a14d991a 239 while (Target[targetNum].Time < msTimer && targetNum < TargetMAX)
ac0314naga 0:c527a14d991a 240 targetNum++;
ac0314naga 0:c527a14d991a 241
ac0314naga 0:c527a14d991a 242 if (targetNum == TargetMAX)
ac0314naga 0:c527a14d991a 243 operate = 0;
ac0314naga 0:c527a14d991a 244 else {
ac0314naga 0:c527a14d991a 245 double divTim = 0;
ac0314naga 0:c527a14d991a 246 divTim = (double)(msTimer - Target[targetNum - 1].Time) / (double)(Target[targetNum].Time - Target[targetNum - 1].Time);
ac0314naga 0:c527a14d991a 247
ac0314naga 0:c527a14d991a 248 /* 時間毎の目標を算出する */
ac0314naga 0:c527a14d991a 249 LOCATION NowTar;
ac0314naga 0:c527a14d991a 250 NowTar.X = Target[targetNum - 1].X+(Target[targetNum].X - Target[targetNum - 1].X) * divTim;
ac0314naga 0:c527a14d991a 251 NowTar.Y = Target[targetNum - 1].Y+(Target[targetNum].Y - Target[targetNum - 1].Y) * divTim;
ac0314naga 0:c527a14d991a 252 NowTar.Theta = Target[targetNum - 1].Theta+(Target[targetNum].Theta - Target[targetNum - 1].Theta) * divTim;
ac0314naga 0:c527a14d991a 253
ac0314naga 0:c527a14d991a 254 /* 目標までの必要諸量 */
ac0314naga 0:c527a14d991a 255 LOCATION moveGain;
ac0314naga 0:c527a14d991a 256 moveGain.X = LocationPID(NowTar.X, NowLoc.X);
ac0314naga 0:c527a14d991a 257 moveGain.Y = LocationPID(NowTar.Y, NowLoc.Y);
ac0314naga 0:c527a14d991a 258 moveGain.Theta = (NowTar.Theta - NowLoc.Theta)*-4;
ac0314naga 0:c527a14d991a 259
ac0314naga 0:c527a14d991a 260 /* 三角オムニに出力する */
ac0314naga 0:c527a14d991a 261 InverseTriOmni(MovMotor, moveGain.X, moveGain.Y, moveGain.Theta);
ac0314naga 0:c527a14d991a 262 for(int i = 6; i < 9; i++)
ac0314naga 0:c527a14d991a 263 MovMotor[i] = Range(MovMotor[i],100);
ac0314naga 0:c527a14d991a 264
ac0314naga 0:c527a14d991a 265 isMoveInjection = Target[targetNum].optionInjection;
ac0314naga 0:c527a14d991a 266 isMoveRibbon = Target[targetNum].optionRibbon;
ac0314naga 0:c527a14d991a 267 Rod = Target[targetNum].optionRod;
ac0314naga 0:c527a14d991a 268
ac0314naga 0:c527a14d991a 269
ac0314naga 0:c527a14d991a 270 // pc.printf("num:%d ",targetNum);
ac0314naga 0:c527a14d991a 271 // pc.printf("rod:%d ",Target[targetNum].optionRod);
ac0314naga 0:c527a14d991a 272 // pc.printf("Tar:%4.0f %4.0f %1.2f ",Target[targetNum].X,Target[targetNum].Y,Target[targetNum].Theta);
ac0314naga 0:c527a14d991a 273 // pc.printf("NowTar:%4.0f %4.0f %1.2f ",NowTar.X,NowTar.Y,NowTar.Theta);
ac0314naga 0:c527a14d991a 274 }
ac0314naga 0:c527a14d991a 275 }
ac0314naga 0:c527a14d991a 276
ac0314naga 0:c527a14d991a 277 /* 手動 */
ac0314naga 0:c527a14d991a 278 else if (operate == 2) {
ac0314naga 0:c527a14d991a 279
ac0314naga 0:c527a14d991a 280 int x_val = (DS3.LX-64) * 100/64;
ac0314naga 0:c527a14d991a 281 int y_val = (64-DS3.LY) * 100/64;
ac0314naga 0:c527a14d991a 282 int r_val = (DS3.RX-64) * 100/64;
ac0314naga 0:c527a14d991a 283
ac0314naga 0:c527a14d991a 284 /* 目標角更新 */
ac0314naga 0:c527a14d991a 285 if (DS3.RX != 64) yawCnt.reset();
ac0314naga 0:c527a14d991a 286 if (yawCnt.read_ms() < 1000) TarTheta = NowLoc.Theta;
ac0314naga 0:c527a14d991a 287 /* gyro値による補正 */
ac0314naga 0:c527a14d991a 288 r_val += (TarTheta - NowLoc.Theta) * -4.0;
ac0314naga 0:c527a14d991a 289
ac0314naga 0:c527a14d991a 290 InverseTriOmni(MovMotor, x_val, y_val, r_val);
ac0314naga 0:c527a14d991a 291
ac0314naga 0:c527a14d991a 292 isMoveInjection = DS3.R1;
ac0314naga 0:c527a14d991a 293 isMoveRibbon = DS3.R2+DS3.L2;
ac0314naga 0:c527a14d991a 294 Rod = DS3.L1;
ac0314naga 0:c527a14d991a 295 }
ac0314naga 0:c527a14d991a 296
ac0314naga 0:c527a14d991a 297
ac0314naga 0:c527a14d991a 298 else if(operate == 3) {
ac0314naga 0:c527a14d991a 299 isMoveInjection = DS3.R1;
ac0314naga 0:c527a14d991a 300 isMoveRibbon = DS3.R2+DS3.L2;
ac0314naga 0:c527a14d991a 301 Rod = DS3.L1;
ac0314naga 0:c527a14d991a 302 }
ac0314naga 0:c527a14d991a 303
ac0314naga 0:c527a14d991a 304 /* 改行を入れる */
ac0314naga 0:c527a14d991a 305 pc.printf("\n\r");
ac0314naga 0:c527a14d991a 306
ac0314naga 0:c527a14d991a 307 // pc.printf("Loc:%4.0f %4.0f %1.2f ",NowLoc.X,NowLoc.Y,NowLoc.Theta);
ac0314naga 0:c527a14d991a 308 // pc.printf("isRibbon:%d ", isMoveRibbon);
ac0314naga 0:c527a14d991a 309 // pc.printf("isIn:%d ", isMoveInjection);
ac0314naga 0:c527a14d991a 310 /* データチェック */
ac0314naga 0:c527a14d991a 311 // pc.printf("ope:%d ", operate);
ac0314naga 0:c527a14d991a 312 // pc.printf("tim:%6d ",msTimer);
ac0314naga 0:c527a14d991a 313 // pc.printf("tim:%6d ",whileTime_us);
ac0314naga 0:c527a14d991a 314 // pc.printf("act[0]:%4d %4d", injection[0].actNum,
ac0314naga 0:c527a14d991a 315 // injection[0].WaitNextShot.read_ms());
ac0314naga 0:c527a14d991a 316 // pc.printf("act[1]:%4d %4d", injection[1].actNum,
ac0314naga 0:c527a14d991a 317 // injection[1].WaitNextShot.read_ms());
ac0314naga 0:c527a14d991a 318
ac0314naga 0:c527a14d991a 319 // pc.printf("BallShotTim:%d ", injection[1].BallShotTim.read_ms());
ac0314naga 0:c527a14d991a 320 // pc.printf("lim:%4d ",injection[1].limitMagazine!=0);
ac0314naga 0:c527a14d991a 321 // pc.printf("isBallLoading:%d ", injection[1].isBallLoading);
ac0314naga 0:c527a14d991a 322 pc.printf("Motor %d %d %d ", MovMotor[6], MovMotor[7], MovMotor[8]);
ac0314naga 0:c527a14d991a 323
ac0314naga 0:c527a14d991a 324 // pc.printf("Motor %d %d ", MovMotor[9], MovMotor[10]);
ac0314naga 0:c527a14d991a 325
ac0314naga 0:c527a14d991a 326 // pc.printf("Enc %3.0f %3.0f ",EncoderDeg[0],EncoderDeg[1]);
ac0314naga 0:c527a14d991a 327 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 328 // pc.printf("inj[%d]is:%d act:%d ", i, injection[i].isMove,
ac0314naga 0:c527a14d991a 329 // injection[i].actNum);
ac0314naga 0:c527a14d991a 330 // pc.printf("BallLoad:%4d %d ", injection[i].BallLoadTim.read_ms(),
ac0314naga 0:c527a14d991a 331 // injection[i].isBallLoading);
ac0314naga 0:c527a14d991a 332 // pc.printf("Enc %3.0f %3.0f %3.0f ", injection[i].encDeg[0],
ac0314naga 0:c527a14d991a 333 // injection[i].encDeg[1], injection[i].encDeg[2]);
ac0314naga 0:c527a14d991a 334 // pc.printf("Enc %3.0f %3.0f ", injection[i].encDeg[0], injection[i].encDeg[0]);
ac0314naga 0:c527a14d991a 335 // pc.printf("Lim %d %d ",
ac0314naga 0:c527a14d991a 336 // injection[i].limitTop!=0,injection[i].limitBottom!=0);
ac0314naga 0:c527a14d991a 337 // pc.printf("Mo %d %d %d ", injection[i].moveMotor[0], injection[i].moveMotor[1], injection[i].moveMotor[2]);
ac0314naga 0:c527a14d991a 338 // pc.printf("is %d ", injection[i].BallLoadTim.read_ms());
ac0314naga 0:c527a14d991a 339 // pc.printf("Ribbon %d ",isMoveRibbon);
ac0314naga 0:c527a14d991a 340 }
ac0314naga 0:c527a14d991a 341
ac0314naga 0:c527a14d991a 342 /* 周期を保存 */
ac0314naga 0:c527a14d991a 343 whileTime_us = whileTimer.read_us();
ac0314naga 0:c527a14d991a 344 if(timerMode) msTimer+=(double)whileTimer.read()*1000.0;
ac0314naga 0:c527a14d991a 345 // if(timerMode) msTimer+=whileTimer.read_ms()+1;
ac0314naga 0:c527a14d991a 346 if(whileTime_us>50000) I2C hoge(PB_9, PB_8);
ac0314naga 0:c527a14d991a 347 /* 周期リセット */
ac0314naga 0:c527a14d991a 348 whileTimer.reset();
ac0314naga 0:c527a14d991a 349 } // while
ac0314naga 0:c527a14d991a 350 } // main
ac0314naga 0:c527a14d991a 351
ac0314naga 0:c527a14d991a 352
ac0314naga 0:c527a14d991a 353
ac0314naga 0:c527a14d991a 354
ac0314naga 0:c527a14d991a 355 /*******************************************************************************
ac0314naga 0:c527a14d991a 356 * @概要　　射出機構のメイン関数
ac0314naga 0:c527a14d991a 357 * @引数　　なし
ac0314naga 0:c527a14d991a 358 * @返り値　なし
ac0314naga 0:c527a14d991a 359 *******************************************************************************/
ac0314naga 0:c527a14d991a 360 void INJECTION::MainFunction(void)
ac0314naga 0:c527a14d991a 361 {
ac0314naga 0:c527a14d991a 362 /* 上のリミットと接触したらEncリセット */
ac0314naga 0:c527a14d991a 363 if (limitTop) encDeg[0] = encDeg[1] = 0;
ac0314naga 0:c527a14d991a 364
ac0314naga 0:c527a14d991a 365 /* 下のリミットと接触したらエンコーダー角度を合わせる */
ac0314naga 0:c527a14d991a 366 if(limitBottom) {
ac0314naga 0:c527a14d991a 367 if(abs(encDeg[0]) > abs(encDeg[1])) encDeg[1] = -encDeg[0];
ac0314naga 0:c527a14d991a 368 else if(abs(encDeg[0]) < abs(encDeg[1])) encDeg[0] = -encDeg[1];
ac0314naga 0:c527a14d991a 369 }
ac0314naga 0:c527a14d991a 370
ac0314naga 0:c527a14d991a 371 /* 装填機構のリミットと接触したら装填処理*/
ac0314naga 0:c527a14d991a 372 if (limitMagazine != 0) BallLoadTim.start();
ac0314naga 0:c527a14d991a 373
ac0314naga 0:c527a14d991a 374 /* 装填機構動作 */
ac0314naga 0:c527a14d991a 375 moveMotor[2] = LoadingPID((double)initalDeg,encDeg[2]);
ac0314naga 0:c527a14d991a 376
ac0314naga 0:c527a14d991a 377 switch (actNum) {
ac0314naga 0:c527a14d991a 378 case 0:
ac0314naga 0:c527a14d991a 379
ac0314naga 0:c527a14d991a 380 /* 上昇したら終了 */
ac0314naga 0:c527a14d991a 381 if (encDeg[1] > 800) {
ac0314naga 0:c527a14d991a 382 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 383 velDev[i][0] = velDev[i][1] = velDev[i][2] = 0;
ac0314naga 0:c527a14d991a 384 moveMotor[i] = 0;
ac0314naga 0:c527a14d991a 385 updateVal[i][0] = updateVal[i][1] = 0;
ac0314naga 0:c527a14d991a 386 EncDegDiff[i][0]=EncDegDiff[i][1]=0;
ac0314naga 0:c527a14d991a 387 }
ac0314naga 0:c527a14d991a 388 actNum++;
ac0314naga 0:c527a14d991a 389
ac0314naga 0:c527a14d991a 390 } else if (BallShotTim.read_ms() > 250) {//350 //250
ac0314naga 0:c527a14d991a 391 /* 左右それぞれ一秒あたりの回転数を計算する */
ac0314naga 0:c527a14d991a 392 double rps[2]= {0};
ac0314naga 0:c527a14d991a 393 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 394 double angularVel = RotationPerWhile(encDeg[i], i); //差分
ac0314naga 0:c527a14d991a 395 rps[i] = (angularVel * 1000000.0 / whileTime_us) / 360.0;
ac0314naga 0:c527a14d991a 396 }
ac0314naga 0:c527a14d991a 397
ac0314naga 0:c527a14d991a 398 /* 操作量を更新 */
ac0314naga 0:c527a14d991a 399 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 400 updateVal[i][0] = updateVal[i][1];
ac0314naga 0:c527a14d991a 401 updateVal[i][1] =
ac0314naga 0:c527a14d991a 402 updateVal[i][0] + VelocityPID(targetRps[i], rps[i], i);
ac0314naga 0:c527a14d991a 403 /* 値を制限する */
ac0314naga 0:c527a14d991a 404 moveMotor[i] = Range(updateVal[i][1], 100);
ac0314naga 0:c527a14d991a 405
ac0314naga 0:c527a14d991a 406 }
ac0314naga 0:c527a14d991a 407 }
ac0314naga 0:c527a14d991a 408 break;
ac0314naga 0:c527a14d991a 409
ac0314naga 0:c527a14d991a 410 case 1:
ac0314naga 0:c527a14d991a 411 /* 目標更新，装填機構動作 */
ac0314naga 0:c527a14d991a 412 if (BallLoadTim.read_ms() >400 && isMove) {//450 200
ac0314naga 0:c527a14d991a 413 isBallLoading = 1;
ac0314naga 0:c527a14d991a 414 initalDeg -= 120;
ac0314naga 0:c527a14d991a 415 isMove = 0;
ac0314naga 0:c527a14d991a 416 WaitNextShot.reset();
ac0314naga 0:c527a14d991a 417 WaitNextShot.start();
ac0314naga 0:c527a14d991a 418 }
ac0314naga 0:c527a14d991a 419
ac0314naga 0:c527a14d991a 420 /* 装填機構を動作させる */
ac0314naga 0:c527a14d991a 421 if (isBallLoading) {
ac0314naga 0:c527a14d991a 422 BallLoadTim.stop();
ac0314naga 0:c527a14d991a 423 BallLoadTim.reset();
ac0314naga 0:c527a14d991a 424
ac0314naga 0:c527a14d991a 425 if (limitTop) {
ac0314naga 0:c527a14d991a 426 /* 誤差範囲なら終了フラグ立て */
ac0314naga 0:c527a14d991a 427 /* 装填機構の目標角度更新 */
ac0314naga 0:c527a14d991a 428 if (abs(initalDeg - encDeg[2]) < 5.0) {
ac0314naga 0:c527a14d991a 429 isBallLoading = 0;
ac0314naga 0:c527a14d991a 430 BallShotTim.reset();
ac0314naga 0:c527a14d991a 431 actNum++;
ac0314naga 0:c527a14d991a 432 }
ac0314naga 0:c527a14d991a 433 }
ac0314naga 0:c527a14d991a 434 /* 射出機構戻ってなかったら途中までで上書き */
ac0314naga 0:c527a14d991a 435 else {
ac0314naga 0:c527a14d991a 436 /* 下降中に途中まで回す */
ac0314naga 0:c527a14d991a 437 moveMotor[2] = LoadingPID(initalDeg+40, encDeg[2]);
ac0314naga 0:c527a14d991a 438 }
ac0314naga 0:c527a14d991a 439 }
ac0314naga 0:c527a14d991a 440
ac0314naga 0:c527a14d991a 441 /* リミットに触れるまで下降 */
ac0314naga 0:c527a14d991a 442 if (limitTop) {
ac0314naga 0:c527a14d991a 443 for (int i = 0; i < 2; i++) moveMotor[i] = 0;
ac0314naga 0:c527a14d991a 444 } else if (encDeg[1] > 500) {
ac0314naga 0:c527a14d991a 445 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 446 moveMotor[i] = -400 - encDeg[i];
ac0314naga 0:c527a14d991a 447 moveMotor[i] = Range(moveMotor[i], 70);
ac0314naga 0:c527a14d991a 448 }
ac0314naga 0:c527a14d991a 449 } else {
ac0314naga 0:c527a14d991a 450 /* 固定値で一番下まで下降 */
ac0314naga 0:c527a14d991a 451 for (int i = 0; i < 2; i++) moveMotor[i] = -10 * (i * 2 - 1);
ac0314naga 0:c527a14d991a 452 }
ac0314naga 0:c527a14d991a 453 break;
ac0314naga 0:c527a14d991a 454
ac0314naga 0:c527a14d991a 455 default:
ac0314naga 0:c527a14d991a 456 actNum = 0;
ac0314naga 0:c527a14d991a 457 break;
ac0314naga 0:c527a14d991a 458 } // switch
ac0314naga 0:c527a14d991a 459 }
ac0314naga 0:c527a14d991a 460
ac0314naga 0:c527a14d991a 461 /*******************************************************************************
ac0314naga 0:c527a14d991a 462 * @概要　　速度型PID制御
ac0314naga 0:c527a14d991a 463 * @引数　　目標値, 現在値, モーター番号
ac0314naga 0:c527a14d991a 464 * @返り値　操作量
ac0314naga 0:c527a14d991a 465 *******************************************************************************/
ac0314naga 0:c527a14d991a 466 double INJECTION::VelocityPID(double Target_value, double Current_value, int motorNum)
ac0314naga 0:c527a14d991a 467 {
ac0314naga 0:c527a14d991a 468 /* 比例定数,積分定数,微分定数 */
ac0314naga 0:c527a14d991a 469 const double kp = 1.0;
ac0314naga 0:c527a14d991a 470 const double ki = 0.3;
ac0314naga 0:c527a14d991a 471 const double kd = 0.05;
ac0314naga 0:c527a14d991a 472 /* 比例項,積分項,微分項の計算結果を保存する変数*/
ac0314naga 0:c527a14d991a 473
ac0314naga 0:c527a14d991a 474 /* 偏差を更新 */
ac0314naga 0:c527a14d991a 475 velDev[motorNum][2] = velDev[motorNum][1];
ac0314naga 0:c527a14d991a 476 velDev[motorNum][1] = velDev[motorNum][0];
ac0314naga 0:c527a14d991a 477 velDev[motorNum][0] = Target_value - Current_value;
ac0314naga 0:c527a14d991a 478
ac0314naga 0:c527a14d991a 479 /* 比例項,積分項,微分項をそれぞれ求める */
ac0314naga 0:c527a14d991a 480 double P = (velDev[motorNum][0] - velDev[motorNum][1]) * kp;
ac0314naga 0:c527a14d991a 481 double I = velDev[motorNum][0] * ki;
ac0314naga 0:c527a14d991a 482 double D = ((velDev[motorNum][0] - velDev[motorNum][1]) -
ac0314naga 0:c527a14d991a 483 (velDev[motorNum][1] - velDev[motorNum][2])) *
ac0314naga 0:c527a14d991a 484 kd;
ac0314naga 0:c527a14d991a 485
ac0314naga 0:c527a14d991a 486 /* 合計値を操作量として返す */
ac0314naga 0:c527a14d991a 487 return P + I + D;
ac0314naga 0:c527a14d991a 488 }
ac0314naga 0:c527a14d991a 489
ac0314naga 0:c527a14d991a 490 /*******************************************************************************
ac0314naga 0:c527a14d991a 491 * @概要　　位置型PID制御
ac0314naga 0:c527a14d991a 492 * @引数　　目標値, 現在値
ac0314naga 0:c527a14d991a 493 * @返り値　操作量
ac0314naga 0:c527a14d991a 494 *******************************************************************************/
ac0314naga 0:c527a14d991a 495 double LocationPID(double Target_value, double Current_value)
ac0314naga 0:c527a14d991a 496 {
ac0314naga 0:c527a14d991a 497 /* 比例定数,積分定数,微分定数 */
ac0314naga 0:c527a14d991a 498 const double kp = 0.5;
ac0314naga 0:c527a14d991a 499 const double ki = 0;
ac0314naga 0:c527a14d991a 500 const double kd = 0;
ac0314naga 0:c527a14d991a 501 /* 比例項,積分項,微分項の計算結果を保存する変数*/
ac0314naga 0:c527a14d991a 502 static double P = 0;
ac0314naga 0:c527a14d991a 503 static double I = 0;
ac0314naga 0:c527a14d991a 504 static double D = 0;
ac0314naga 0:c527a14d991a 505 /* 偏差の一時保存領域 */
ac0314naga 0:c527a14d991a 506 static double dev[2] = {0};
ac0314naga 0:c527a14d991a 507
ac0314naga 0:c527a14d991a 508 /* 偏差を更新 */
ac0314naga 0:c527a14d991a 509 dev[1] = dev[0];
ac0314naga 0:c527a14d991a 510 dev[0] = Target_value - Current_value;
ac0314naga 0:c527a14d991a 511
ac0314naga 0:c527a14d991a 512 /* 比例項,積分項,微分項をそれぞれ求める */
ac0314naga 0:c527a14d991a 513 P = dev[0] * kp;
ac0314naga 0:c527a14d991a 514 I += dev[0] * ki;
ac0314naga 0:c527a14d991a 515 D = (dev[0] - dev[1]) * kd;
ac0314naga 0:c527a14d991a 516
ac0314naga 0:c527a14d991a 517 /* ワインドアップ */
ac0314naga 0:c527a14d991a 518 if ((dev[0] < 0 && dev[1] > 0) || (dev[0] > 0 && dev[1] < 0)) I = 0;
ac0314naga 0:c527a14d991a 519
ac0314naga 0:c527a14d991a 520 /* 合計値を操作量として返す */
ac0314naga 0:c527a14d991a 521 return P + I + D;
ac0314naga 0:c527a14d991a 522 }
ac0314naga 0:c527a14d991a 523
ac0314naga 0:c527a14d991a 524 /*******************************************************************************
ac0314naga 0:c527a14d991a 525 * @概要　　位置型PID制御(装填機構)
ac0314naga 0:c527a14d991a 526 * @引数　　目標値, 現在値
ac0314naga 0:c527a14d991a 527 * @返り値　操作量
ac0314naga 0:c527a14d991a 528 *******************************************************************************/
ac0314naga 0:c527a14d991a 529 double INJECTION::LoadingPID(double Target_value, double Current_value)
ac0314naga 0:c527a14d991a 530 {
ac0314naga 0:c527a14d991a 531 /* 比例定数,積分定数,微分定数 */ //0.2 0.02 0.25 | 0.2 0.04 0.25
ac0314naga 0:c527a14d991a 532 const double kp = 1.0;
ac0314naga 0:c527a14d991a 533 const double ki = 0.06;
ac0314naga 0:c527a14d991a 534 const double kd = 0.9;
ac0314naga 0:c527a14d991a 535 /* 比例項,積分項,微分項の計算結果を保存する変数*/
ac0314naga 0:c527a14d991a 536 static double P = 0;
ac0314naga 0:c527a14d991a 537 static double I = 0;
ac0314naga 0:c527a14d991a 538 static double D = 0;
ac0314naga 0:c527a14d991a 539
ac0314naga 0:c527a14d991a 540 /* 偏差を更新 */
ac0314naga 0:c527a14d991a 541 loadDev[1] = loadDev[0];
ac0314naga 0:c527a14d991a 542 loadDev[0] = Target_value - Current_value;
ac0314naga 0:c527a14d991a 543
ac0314naga 0:c527a14d991a 544 /* 比例項,積分項,微分項をそれぞれ求める */
ac0314naga 0:c527a14d991a 545 P = loadDev[0] * kp;
ac0314naga 0:c527a14d991a 546 I += loadDev[0] * ki;
ac0314naga 0:c527a14d991a 547 D = (loadDev[0] - loadDev[1]) * kd;
ac0314naga 0:c527a14d991a 548
ac0314naga 0:c527a14d991a 549
ac0314naga 0:c527a14d991a 550 /* ワインドアップ */
ac0314naga 0:c527a14d991a 551 if ((loadDev[0] < 0 && loadDev[1] > 0) || (loadDev[0] > 0 && loadDev[1] < 0)) I = 0;
ac0314naga 0:c527a14d991a 552
ac0314naga 0:c527a14d991a 553 /* 合計値を操作量として返す */
ac0314naga 0:c527a14d991a 554 return Range(P + Range(I,6) + D, 80);
ac0314naga 0:c527a14d991a 555 }
ac0314naga 0:c527a14d991a 556
ac0314naga 0:c527a14d991a 557 /*******************************************************************************
ac0314naga 0:c527a14d991a 558 * @概要　　制限する
ac0314naga 0:c527a14d991a 559 * @引数　　対象,限界値
ac0314naga 0:c527a14d991a 560 * @返り値　制限後の値
ac0314naga 0:c527a14d991a 561 *******************************************************************************/
ac0314naga 0:c527a14d991a 562 double Range(double value, double limit)
ac0314naga 0:c527a14d991a 563 {
ac0314naga 0:c527a14d991a 564 if (value > limit)
ac0314naga 0:c527a14d991a 565 return limit;
ac0314naga 0:c527a14d991a 566 else if (value < -limit)
ac0314naga 0:c527a14d991a 567 return -limit;
ac0314naga 0:c527a14d991a 568 else
ac0314naga 0:c527a14d991a 569 return value;
ac0314naga 0:c527a14d991a 570 }
ac0314naga 0:c527a14d991a 571
ac0314naga 0:c527a14d991a 572 /*******************************************************************************
ac0314naga 0:c527a14d991a 573 * @概要　　回転角度を求める関数
ac0314naga 0:c527a14d991a 574 * @引数　　角度, エンコーダー番号, 機体番号
ac0314naga 0:c527a14d991a 575 * @返り値　周期あたりの角速度
ac0314naga 0:c527a14d991a 576 *******************************************************************************/
ac0314naga 0:c527a14d991a 577 double INJECTION::RotationPerWhile(double degree, int num)
ac0314naga 0:c527a14d991a 578 {
ac0314naga 0:c527a14d991a 579 /* 前回の値更新 今回の値更新 */
ac0314naga 0:c527a14d991a 580 EncDegDiff[num][1] = EncDegDiff[num][0];
ac0314naga 0:c527a14d991a 581 EncDegDiff[num][0] = degree;
ac0314naga 0:c527a14d991a 582
ac0314naga 0:c527a14d991a 583 /* 前回と現在の値を比べる */
ac0314naga 0:c527a14d991a 584 return EncDegDiff[num][0] - EncDegDiff[num][1];
ac0314naga 0:c527a14d991a 585 }
ac0314naga 0:c527a14d991a 586
ac0314naga 0:c527a14d991a 587 /*******************************************************************************
ac0314naga 0:c527a14d991a 588 * @概要 自己位置推定関数
ac0314naga 0:c527a14d991a 589 * @引数 なし
ac0314naga 0:c527a14d991a 590 * @返り値 なし
ac0314naga 0:c527a14d991a 591 *******************************************************************************/
ac0314naga 0:c527a14d991a 592 void LocEstimate(void)
ac0314naga 0:c527a14d991a 593 {
ac0314naga 0:c527a14d991a 594 static double GyroDeg[2] = {0};
ac0314naga 0:c527a14d991a 595 static double EncDeg[2][2] = {0};
ac0314naga 0:c527a14d991a 596 static double disp[3] = {0};
ac0314naga 0:c527a14d991a 597
ac0314naga 0:c527a14d991a 598 /* ジャイロの値取得 */
ac0314naga 0:c527a14d991a 599 bno.get_angles();
ac0314naga 0:c527a14d991a 600 GyroDeg[1] = GyroDeg[0];
ac0314naga 0:c527a14d991a 601 GyroDeg[0] = bno.euler.yaw;
ac0314naga 0:c527a14d991a 602
ac0314naga 0:c527a14d991a 603 if (GyroDeg[0] != 0) {
ac0314naga 0:c527a14d991a 604 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
ac0314naga 0:c527a14d991a 605 if (GyroDeg[1] < 90 && GyroDeg[0] > 270) GyroDeg[1] += 360;
ac0314naga 0:c527a14d991a 606 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
ac0314naga 0:c527a14d991a 607 else if (GyroDeg[1] > 270 && GyroDeg[0] < 90)
ac0314naga 0:c527a14d991a 608 GyroDeg[1] -= 360;
ac0314naga 0:c527a14d991a 609 /* 差を求める*/
ac0314naga 0:c527a14d991a 610 disp[2] = GyroDeg[1] - GyroDeg[0];
ac0314naga 0:c527a14d991a 611 }
ac0314naga 0:c527a14d991a 612 /* Enc2つの差分求める */
ac0314naga 0:c527a14d991a 613 for (int i = 0; i < 2; i++) {
ac0314naga 0:c527a14d991a 614 EncDeg[i][1] = EncDeg[i][0];
ac0314naga 0:c527a14d991a 615 EncDeg[i][0] = EncoderDeg[i];
ac0314naga 0:c527a14d991a 616 disp[i] = DEG_TO_DIS(EncDeg[i][1] - EncDeg[i][0]);
ac0314naga 0:c527a14d991a 617 }
ac0314naga 0:c527a14d991a 618 /* 差分を加速度として保存 */
ac0314naga 0:c527a14d991a 619 NowAcc.Theta = disp[2];
ac0314naga 0:c527a14d991a 620 NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.Theta)) +
ac0314naga 0:c527a14d991a 621 disp[1] * sin(DEG_TO_RAD(NowLoc.Theta));
ac0314naga 0:c527a14d991a 622 NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.Theta)) -
ac0314naga 0:c527a14d991a 623 disp[1] * cos(DEG_TO_RAD(NowLoc.Theta));
ac0314naga 0:c527a14d991a 624 /* 差分を累積して現在位置を保存 */
ac0314naga 0:c527a14d991a 625 NowLoc.X += NowAcc.X / 2.0;
ac0314naga 0:c527a14d991a 626 NowLoc.Y += NowAcc.Y / 2.0;
ac0314naga 0:c527a14d991a 627 NowLoc.Theta += NowAcc.Theta;
ac0314naga 0:c527a14d991a 628 }
ac0314naga 0:c527a14d991a 629
ac0314naga 0:c527a14d991a 630 /*******************************************************************************
ac0314naga 0:c527a14d991a 631 * @概要　　回転角度を求める関数
ac0314naga 0:c527a14d991a 632 * @引数　　角度, エンコーダー番号, 機体番号
ac0314naga 0:c527a14d991a 633 * @返り値　周期あたりの角速度
ac0314naga 0:c527a14d991a 634 *******************************************************************************/
ac0314naga 0:c527a14d991a 635 int Rotation(int degree, int num, int robotNum)
ac0314naga 0:c527a14d991a 636 {
ac0314naga 0:c527a14d991a 637 static int EncDeg[2][2][2] = {0};
ac0314naga 0:c527a14d991a 638 static int NowRotate[2][2] = {0};
ac0314naga 0:c527a14d991a 639
ac0314naga 0:c527a14d991a 640 /* 前回の値更新 今回の値更新 */
ac0314naga 0:c527a14d991a 641 EncDeg[robotNum][num][1] = EncDeg[robotNum][num][0];
ac0314naga 0:c527a14d991a 642 EncDeg[robotNum][num][0] = degree;
ac0314naga 0:c527a14d991a 643
ac0314naga 0:c527a14d991a 644 /* 前回と現在の値を比べる */
ac0314naga 0:c527a14d991a 645 NowRotate[robotNum][num] =
ac0314naga 0:c527a14d991a 646 EncDeg[robotNum][num][0] - EncDeg[robotNum][num][1];
ac0314naga 0:c527a14d991a 647 return NowRotate[robotNum][num];
ac0314naga 0:c527a14d991a 648 }
ac0314naga 0:c527a14d991a 649
ac0314naga 0:c527a14d991a 650 /*******************************************************************************
ac0314naga 0:c527a14d991a 651 * @概要　　逆三角オムニ関数
ac0314naga 0:c527a14d991a 652 * @引数　　モータの配列・x,y,r ベクトル
ac0314naga 0:c527a14d991a 653 * @返り値　なし
ac0314naga 0:c527a14d991a 654 ******************************************************************************/
ac0314naga 0:c527a14d991a 655 void InverseTriOmni(int *duty, double x, double y, double r)
ac0314naga 0:c527a14d991a 656 {
ac0314naga 0:c527a14d991a 657 int duty_max = 0;
ac0314naga 0:c527a14d991a 658
ac0314naga 0:c527a14d991a 659 double rad = DEG_TO_RAD(NowLoc.Theta);
ac0314naga 0:c527a14d991a 660 duty[6] = x * cos(rad) + y * sin(rad);
ac0314naga 0:c527a14d991a 661 duty[7] = x * cos(rad + PI / 3.0 * 2) + y * sin(rad + PI / 3.0 * 2);
ac0314naga 0:c527a14d991a 662 duty[8] = -x * cos(rad + PI / 3.0) - y * sin(rad + PI / 3.0);
ac0314naga 0:c527a14d991a 663
ac0314naga 0:c527a14d991a 664 duty_max = 0;
ac0314naga 0:c527a14d991a 665 for (int i = 6; i < 9; i++) {
ac0314naga 0:c527a14d991a 666 duty[i] += r;
ac0314naga 0:c527a14d991a 667 duty_max = max(duty_max, abs(duty[i]));
ac0314naga 0:c527a14d991a 668 }
ac0314naga 0:c527a14d991a 669 for (int i = 6; i < 9; i++)
ac0314naga 0:c527a14d991a 670 if (duty_max > 99) duty[i] = duty[i] * 99 / duty_max;
ac0314naga 0:c527a14d991a 671 }
ac0314naga 0:c527a14d991a 672
ac0314naga 0:c527a14d991a 673
ac0314naga 0:c527a14d991a 674 /* UART受信割り込み *************************************************************/
ac0314naga 0:c527a14d991a 675 void uartRX()
ac0314naga 0:c527a14d991a 676 {
ac0314naga 0:c527a14d991a 677 static int num = 0;
ac0314naga 0:c527a14d991a 678 static uint8_t buf[3] = {0};
ac0314naga 0:c527a14d991a 679
ac0314naga 0:c527a14d991a 680 /* 受信データ */
ac0314naga 0:c527a14d991a 681 uint8_t data = UART.getc();
ac0314naga 0:c527a14d991a 682 if((data&0x80) != 0) {
ac0314naga 0:c527a14d991a 683 timerMode = (data & 0x40)!=0;
ac0314naga 0:c527a14d991a 684
ac0314naga 0:c527a14d991a 685 //ビット結合
ac0314naga 0:c527a14d991a 686 uint32_t tmp = 0;
ac0314naga 0:c527a14d991a 687 for(int i=0; i<3; i++)
ac0314naga 0:c527a14d991a 688 tmp |= buf[i]<<(14-i*7);
ac0314naga 0:c527a14d991a 689
ac0314naga 0:c527a14d991a 690 //チェックサム計算
ac0314naga 0:c527a14d991a 691 uint8_t checkSum = 0;
ac0314naga 0:c527a14d991a 692 for (int i = 0; i < 7; i++)
ac0314naga 0:c527a14d991a 693 checkSum += tmp / (int)pow(10.0, (double)i) % 10;
ac0314naga 0:c527a14d991a 694
ac0314naga 0:c527a14d991a 695 //誤り検知陰性
ac0314naga 0:c527a14d991a 696 if(checkSum == (data&0x3F))
ac0314naga 0:c527a14d991a 697 msTimer = tmp;
ac0314naga 0:c527a14d991a 698
ac0314naga 0:c527a14d991a 699 num = 0;
ac0314naga 0:c527a14d991a 700 } else {
ac0314naga 0:c527a14d991a 701 buf[num] = data;
ac0314naga 0:c527a14d991a 702 num++;
ac0314naga 0:c527a14d991a 703 }
ac0314naga 0:c527a14d991a 704
ac0314naga 0:c527a14d991a 705
ac0314naga 0:c527a14d991a 706 /* 送信データ */
ac0314naga 0:c527a14d991a 707 char sendData = 2<<2;
ac0314naga 0:c527a14d991a 708 sendData|=(statious&0b11);
ac0314naga 0:c527a14d991a 709 UART.putc(sendData);
ac0314naga 0:c527a14d991a 710 }
ac0314naga 0:c527a14d991a 711
ac0314naga 0:c527a14d991a 712 /* 割り込み(100us) *************************************************************/
ac0314naga 0:c527a14d991a 713 void IT_CallBack(void)
ac0314naga 0:c527a14d991a 714 {
ac0314naga 0:c527a14d991a 715 static int cnt = 0;
ac0314naga 0:c527a14d991a 716 static int data[EncoderMAX] = {0};
ac0314naga 0:c527a14d991a 717 static double EncDeg[EncoderMAX][2] = {0};
ac0314naga 0:c527a14d991a 718
ac0314naga 0:c527a14d991a 719 switch (cnt) {
ac0314naga 0:c527a14d991a 720 /* 最初の処理 */
ac0314naga 0:c527a14d991a 721 case 0:
ac0314naga 0:c527a14d991a 722 for (int i = 0; i < EncoderMAX; i++) data[i] = 0;
ac0314naga 0:c527a14d991a 723 CS = 0;
ac0314naga 0:c527a14d991a 724 CL = 1;
ac0314naga 0:c527a14d991a 725 break;
ac0314naga 0:c527a14d991a 726 /* 最後の処理 */
ac0314naga 0:c527a14d991a 727 case 25:
ac0314naga 0:c527a14d991a 728 CS = 1;
ac0314naga 0:c527a14d991a 729
ac0314naga 0:c527a14d991a 730 for (int i = 0; i < EncoderMAX; i++) {
ac0314naga 0:c527a14d991a 731 /* 前回の値更新
ac0314naga 0:c527a14d991a 732 * 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
ac0314naga 0:c527a14d991a 733 EncDeg[i][1] = EncDeg[i][0];
ac0314naga 0:c527a14d991a 734 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
ac0314naga 0:c527a14d991a 735 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
ac0314naga 0:c527a14d991a 736 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
ac0314naga 0:c527a14d991a 737 EncDeg[i][1] -= 360;
ac0314naga 0:c527a14d991a 738 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
ac0314naga 0:c527a14d991a 739 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
ac0314naga 0:c527a14d991a 740 EncDeg[i][1] += 360;
ac0314naga 0:c527a14d991a 741 /* 差を求め，適当な変数に代入 */
ac0314naga 0:c527a14d991a 742 if (i < 6)
ac0314naga 0:c527a14d991a 743 injection[i / 3].encDeg[i % 3] +=
ac0314naga 0:c527a14d991a 744 EncDeg[i][0] - EncDeg[i][1];
ac0314naga 0:c527a14d991a 745 else
ac0314naga 0:c527a14d991a 746 EncoderDeg[i - 6] -= EncDeg[i][0] - EncDeg[i][1];
ac0314naga 0:c527a14d991a 747 }
ac0314naga 0:c527a14d991a 748 break;
ac0314naga 0:c527a14d991a 749 /* 通常の処理 */
ac0314naga 0:c527a14d991a 750 default:
ac0314naga 0:c527a14d991a 751 CL = !CL;
ac0314naga 0:c527a14d991a 752 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
ac0314naga 0:c527a14d991a 753
ac0314naga 0:c527a14d991a 754 for (int i = 0; i < EncoderMAX; i++)
ac0314naga 0:c527a14d991a 755 if (cnt != 1 && cnt % 2) {
ac0314naga 0:c527a14d991a 756 data[i] |= (Do[i] == 1);
ac0314naga 0:c527a14d991a 757 data[i] = data[i] << 1;
ac0314naga 0:c527a14d991a 758 }
ac0314naga 0:c527a14d991a 759 break;
ac0314naga 0:c527a14d991a 760 }
ac0314naga 0:c527a14d991a 761 cnt++;
ac0314naga 0:c527a14d991a 762 cnt %= 26;
ac0314naga 0:c527a14d991a 763 }