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