YKNCT / Mbed 2 deprecated Triton

Ver Tikutaikai

Dependencies:   mbed SBDBT BNO055 YKNCT_MD YKNCT_I2C

main.h@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
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
TakushimaYukimasa 1:fa3652aca312 49 limitTop.mode(PullDown);
TakushimaYukimasa 1:fa3652aca312 50 limitBottom.mode(PullDown);
TakushimaYukimasa 1:fa3652aca312 51 limitMagazine.mode(PullDown);
TakushimaYukimasa 1:fa3652aca312 52
ac0314naga 0:c527a14d991a 53 encDeg[0]=0;
ac0314naga 0:c527a14d991a 54 encDeg[1]=0;
ac0314naga 0:c527a14d991a 55 encDeg[2]=0;
ac0314naga 0:c527a14d991a 56 actNum=1;
ac0314naga 0:c527a14d991a 57
ac0314naga 0:c527a14d991a 58 targetRps[0]=-27;
ac0314naga 0:c527a14d991a 59 targetRps[1]=27;
ac0314naga 0:c527a14d991a 60 }
ac0314naga 0:c527a14d991a 61
ac0314naga 0:c527a14d991a 62 int actNum; //射出シーケンス
ac0314naga 0:c527a14d991a 63 int initalDeg; //装填機構の目標角度
ac0314naga 0:c527a14d991a 64 double encDeg[3]; //機体別の現在角度保存領域
ac0314naga 0:c527a14d991a 65 int moveMotor[3]; //機体別モーター値保存領域
ac0314naga 0:c527a14d991a 66 double updateVal[2][2]; //速度型PID
ac0314naga 0:c527a14d991a 67 int isBallLoading; //ボールが装填準備状態になっているか
ac0314naga 0:c527a14d991a 68 int isMove; //動作可能状態にあるか
ac0314naga 0:c527a14d991a 69 double EncDegDiff[2][2];//エンコーダー角度の差
ac0314naga 0:c527a14d991a 70 double velDev[2][3]; //速度PID　偏差の一時保存領域
ac0314naga 0:c527a14d991a 71 double loadDev[2]; //位置PID　偏差の一時保存領域
ac0314naga 0:c527a14d991a 72
ac0314naga 0:c527a14d991a 73 /* 目標回転数 */
ac0314naga 0:c527a14d991a 74 int targetRps[2];
ac0314naga 0:c527a14d991a 75
ac0314naga 0:c527a14d991a 76 DigitalIn limitTop;//上リミット
ac0314naga 0:c527a14d991a 77 DigitalIn limitBottom;//下リミット
ac0314naga 0:c527a14d991a 78 DigitalIn limitMagazine;//装填リミット
ac0314naga 0:c527a14d991a 79
ac0314naga 0:c527a14d991a 80 Timer BallLoadTim; //グルグルに乗るまで
ac0314naga 0:c527a14d991a 81 Timer BallShotTim; //グルグルからカップに乗るまで
ac0314naga 0:c527a14d991a 82
ac0314naga 0:c527a14d991a 83 void MainFunction();
ac0314naga 0:c527a14d991a 84 double RotationPerWhile(double degree, int num);
ac0314naga 0:c527a14d991a 85 double VelocityPID(double Target_value, double Current_value, int motorNum);
ac0314naga 0:c527a14d991a 86 double LoadingPID(double Target_value, double Current_value);
ac0314naga 0:c527a14d991a 87 };
ac0314naga 0:c527a14d991a 88
ac0314naga 0:c527a14d991a 89
ac0314naga 0:c527a14d991a 90 /* 定数定義 ------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 91
ac0314naga 0:c527a14d991a 92 /* π */
ac0314naga 0:c527a14d991a 93 #define PI 3.1415926535
ac0314naga 0:c527a14d991a 94
ac0314naga 0:c527a14d991a 95 /* モーターの最大数 */
ac0314naga 0:c527a14d991a 96 #define MotorMAX 11
ac0314naga 0:c527a14d991a 97
ac0314naga 0:c527a14d991a 98 /* エンコーダーの最大数 */
ac0314naga 0:c527a14d991a 99 #define EncoderMAX 8
ac0314naga 0:c527a14d991a 100
ac0314naga 0:c527a14d991a 101
ac0314naga 0:c527a14d991a 102 /* マクロ定義 ----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 103
ac0314naga 0:c527a14d991a 104 /* Enc角度から距離に変換 */
ac0314naga 0:c527a14d991a 105 #define DEG_TO_DIS(__DEGREE__) (100 * PI * (__DEGREE__) / 360)
ac0314naga 0:c527a14d991a 106
ac0314naga 0:c527a14d991a 107 /* radian degree 変換 */
ac0314naga 0:c527a14d991a 108 #define DEG_TO_RAD(__DEGREE__) ((__DEGREE__)*PI / 180)
ac0314naga 0:c527a14d991a 109 #define RAD_TO_DEG(__RADIAN__) ((__RADIAN__)*180 / PI)
ac0314naga 0:c527a14d991a 110
ac0314naga 0:c527a14d991a 111 #define ROUND(_DATA_) ((int)((_DATA_)*10)%10<=3?(_DATA_):(_DATA_)+1)
ac0314naga 0:c527a14d991a 112
ac0314naga 0:c527a14d991a 113 //350 800
TakushimaYukimasa 1:fa3652aca312 114 #define STEP(_STEP_) ((_STEP_ > 2) ? 300 : 800)
ac0314naga 0:c527a14d991a 115
ac0314naga 0:c527a14d991a 116
ac0314naga 0:c527a14d991a 117 /* コントローラー SerectとStartを実装 */
ac0314naga 0:c527a14d991a 118 //#define Select (DS3.LEFTkey+DS3.RIGHTkey==2)
ac0314naga 0:c527a14d991a 119 //#define Start (DS3.UPkey+DS3.DOWNkey==2)
ac0314naga 0:c527a14d991a 120
ac0314naga 0:c527a14d991a 121 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
ac0314naga 0:c527a14d991a 122 /* 変数定義 ------------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 123 /* クラス定義 ----------------------------------------------------------------*/
ac0314naga 0:c527a14d991a 124 #endif /* MAIN_H */