YKNCT / Mbed 2 deprecated Triton

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?

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 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 }