YKNCT / Mbed 2 deprecated Triton

Ver Tikutaikai

Dependencies:   mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ac0314naga 0:c527a14d991a 1 /* Includes ------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 2
ac0314naga 0:c527a14d991a 3 #ifndef MAIN_H
ac0314naga 0:c527a14d991a 4 #define MAIN_H
ac0314naga 0:c527a14d991a 5
ac0314naga 0:c527a14d991a 6 #include <mbed.h>
ac0314naga 0:c527a14d991a 7 #include <BNO055.h>
ac0314naga 0:c527a14d991a 8 #include <SBDBT.h>
ac0314naga 0:c527a14d991a 9 #include <YKNCT_I2C.h>
ac0314naga 0:c527a14d991a 10 #include <YKNCT_MD.h>
ac0314naga 0:c527a14d991a 11
ac0314naga 0:c527a14d991a 12 /* 型定義 --------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 13
ac0314naga 0:c527a14d991a 14 struct LOCATION {
ac0314naga 0:c527a14d991a 15 double X;
ac0314naga 0:c527a14d991a 16 double Y;
ac0314naga 0:c527a14d991a 17 double Theta;
ac0314naga 0:c527a14d991a 18
ac0314naga 0:c527a14d991a 19 public:
ac0314naga 0:c527a14d991a 20 /* 全部リセットしちゃうぞぉ♡ */
ac0314naga 0:c527a14d991a 21 void reset(void){
ac0314naga 0:c527a14d991a 22 X=-2000;
ac0314naga 0:c527a14d991a 23 Y=-2000;
ac0314naga 0:c527a14d991a 24 Theta=180;
ac0314naga 0:c527a14d991a 25 }
ac0314naga 0:c527a14d991a 26 };
ac0314naga 0:c527a14d991a 27
ac0314naga 0:c527a14d991a 28 struct COORDINATE {
ac0314naga 0:c527a14d991a 29 double X;
ac0314naga 0:c527a14d991a 30 double Y;
ac0314naga 0:c527a14d991a 31 double Theta;
ac0314naga 0:c527a14d991a 32 int Time;
ac0314naga 0:c527a14d991a 33 int optionInjection;
ac0314naga 0:c527a14d991a 34 bool optionRod;
ac0314naga 0:c527a14d991a 35 int optionRibbon;
ac0314naga 0:c527a14d991a 36 };
ac0314naga 0:c527a14d991a 37
ac0314naga 0:c527a14d991a 38
ac0314naga 0:c527a14d991a 39
ac0314naga 0:c527a14d991a 40 /* 射出管理構造体 */
ac0314naga 0:c527a14d991a 41 class INJECTION
ac0314naga 0:c527a14d991a 42 {
ac0314naga 0:c527a14d991a 43 public:
ac0314naga 0:c527a14d991a 44 INJECTION(PinName p1, PinName p2, PinName p3)
ac0314naga 0:c527a14d991a 45 : limitTop(p1), limitBottom(p2), limitMagazine(p3){
ac0314naga 0:c527a14d991a 46
ac0314naga 0:c527a14d991a 47 BallShotTim.start();
ac0314naga 0:c527a14d991a 48
ac0314naga 0:c527a14d991a 49 encDeg[0]=0;
ac0314naga 0:c527a14d991a 50 encDeg[1]=0;
ac0314naga 0:c527a14d991a 51 encDeg[2]=0;
ac0314naga 0:c527a14d991a 52 actNum=1;
ac0314naga 0:c527a14d991a 53
ac0314naga 0:c527a14d991a 54 targetRps[0]=-27;
ac0314naga 0:c527a14d991a 55 targetRps[1]=27;
ac0314naga 0:c527a14d991a 56 }
ac0314naga 0:c527a14d991a 57
ac0314naga 0:c527a14d991a 58 int actNum; //射出シーケンス
ac0314naga 0:c527a14d991a 59 int initalDeg; //装填機構の目標角度
ac0314naga 0:c527a14d991a 60 double encDeg[3]; //機体別の現在角度保存領域
ac0314naga 0:c527a14d991a 61 int moveMotor[3]; //機体別モーター値保存領域
ac0314naga 0:c527a14d991a 62 double updateVal[2][2]; //速度型PID
ac0314naga 0:c527a14d991a 63 int isBallLoading; //ボールが装填準備状態になっているか
ac0314naga 0:c527a14d991a 64 int isMove; //動作可能状態にあるか
ac0314naga 0:c527a14d991a 65 double EncDegDiff[2][2];//エンコーダー角度の差
ac0314naga 0:c527a14d991a 66 double velDev[2][3]; //速度PID　偏差の一時保存領域
ac0314naga 0:c527a14d991a 67 double loadDev[2]; //位置PID　偏差の一時保存領域
ac0314naga 0:c527a14d991a 68
ac0314naga 0:c527a14d991a 69 /* 目標回転数 */
ac0314naga 0:c527a14d991a 70 int targetRps[2];
ac0314naga 0:c527a14d991a 71
ac0314naga 0:c527a14d991a 72 DigitalIn limitTop;//上リミット
ac0314naga 0:c527a14d991a 73 DigitalIn limitBottom;//下リミット
ac0314naga 0:c527a14d991a 74 DigitalIn limitMagazine;//装填リミット
ac0314naga 0:c527a14d991a 75
ac0314naga 0:c527a14d991a 76 Timer BallLoadTim; //グルグルに乗るまで
ac0314naga 0:c527a14d991a 77 Timer BallShotTim; //グルグルからカップに乗るまで
ac0314naga 0:c527a14d991a 78
ac0314naga 0:c527a14d991a 79 void MainFunction();
ac0314naga 0:c527a14d991a 80 double RotationPerWhile(double degree, int num);
ac0314naga 0:c527a14d991a 81 double VelocityPID(double Target_value, double Current_value, int motorNum);
ac0314naga 0:c527a14d991a 82 double LoadingPID(double Target_value, double Current_value);
ac0314naga 0:c527a14d991a 83 };
ac0314naga 0:c527a14d991a 84
ac0314naga 0:c527a14d991a 85
ac0314naga 0:c527a14d991a 86 /* 定数定義 ------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 87
ac0314naga 0:c527a14d991a 88 /* π */
ac0314naga 0:c527a14d991a 89 #define PI 3.1415926535
ac0314naga 0:c527a14d991a 90
ac0314naga 0:c527a14d991a 91 /* モーターの最大数 */
ac0314naga 0:c527a14d991a 92 #define MotorMAX 11
ac0314naga 0:c527a14d991a 93
ac0314naga 0:c527a14d991a 94 /* エンコーダーの最大数 */
ac0314naga 0:c527a14d991a 95 #define EncoderMAX 8
ac0314naga 0:c527a14d991a 96
ac0314naga 0:c527a14d991a 97
ac0314naga 0:c527a14d991a 98 /* マクロ定義 ----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 99
ac0314naga 0:c527a14d991a 100 /* Enc角度から距離に変換 */
ac0314naga 0:c527a14d991a 101 #define DEG_TO_DIS(__DEGREE__) (100 * PI * (__DEGREE__) / 360)
ac0314naga 0:c527a14d991a 102
ac0314naga 0:c527a14d991a 103 /* radian degree 変換 */
ac0314naga 0:c527a14d991a 104 #define DEG_TO_RAD(__DEGREE__) ((__DEGREE__)*PI / 180)
ac0314naga 0:c527a14d991a 105 #define RAD_TO_DEG(__RADIAN__) ((__RADIAN__)*180 / PI)
ac0314naga 0:c527a14d991a 106
ac0314naga 0:c527a14d991a 107 #define ROUND(_DATA_) ((int)((_DATA_)*10)%10<=3?(_DATA_):(_DATA_)+1)
ac0314naga 0:c527a14d991a 108
ac0314naga 0:c527a14d991a 109 //350 800
ac0314naga 0:c527a14d991a 110 #define STEP(_STEP_) ((_STEP_ > 2) ? 400 : 800)
ac0314naga 0:c527a14d991a 111
ac0314naga 0:c527a14d991a 112
ac0314naga 0:c527a14d991a 113 /* コントローラー SerectとStartを実装 */
ac0314naga 0:c527a14d991a 114 //#define Select (DS3.LEFTkey+DS3.RIGHTkey==2)
ac0314naga 0:c527a14d991a 115 //#define Start (DS3.UPkey+DS3.DOWNkey==2)
ac0314naga 0:c527a14d991a 116
ac0314naga 0:c527a14d991a 117 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
ac0314naga 0:c527a14d991a 118 /* 変数定義 ------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 119 /* クラス定義 ----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 120 #endif /* MAIN_H */