Diff: main.h

Revision:

0:c527a14d991a

Child:

1:fa3652aca312

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Tue Oct 12 03:57:59 2021 +0000
@@ -0,0 +1,120 @@
+/* Includes ------------------------------------------------------------------*/
+
+#ifndef MAIN_H
+#define MAIN_H
+
+#include <mbed.h>
+#include <BNO055.h>
+#include <SBDBT.h>
+#include <YKNCT_I2C.h>
+#include <YKNCT_MD.h>
+
+/* 型定義 --------------------------------------------------------------------*/
+
+struct LOCATION {
+    double X;
+    double Y;
+    double Theta;
+    
+public:
+    /* 全部リセットしちゃうぞぉ♡ */
+    void reset(void){
+        X=-2000;
+        Y=-2000;
+        Theta=180;
+    }
+};
+
+struct COORDINATE {
+    double X;
+    double Y;
+    double Theta;
+    int Time;
+    int optionInjection;
+    bool optionRod;
+    int optionRibbon;
+};
+
+
+
+/* 射出管理構造体 */
+class INJECTION
+{
+public:
+    INJECTION(PinName p1, PinName p2, PinName p3) 
+    : limitTop(p1), limitBottom(p2), limitMagazine(p3){
+
+        BallShotTim.start();
+
+        encDeg[0]=0;
+        encDeg[1]=0;
+        encDeg[2]=0;
+        actNum=1;
+        
+        targetRps[0]=-27;
+        targetRps[1]=27;
+    }
+    
+    int actNum;             //射出シーケンス
+    int initalDeg;          //装填機構の目標角度
+    double encDeg[3];       //機体別の現在角度保存領域
+    int moveMotor[3];       //機体別モーター値保存領域
+    double updateVal[2][2]; //速度型PID
+    int isBallLoading;      //ボールが装填準備状態になっているか
+    int isMove;             //動作可能状態にあるか
+    double EncDegDiff[2][2];//エンコーダー角度の差
+    double velDev[2][3];    //速度PID　偏差の一時保存領域
+    double loadDev[2];      //位置PID　偏差の一時保存領域
+    
+    /* 目標回転数 */
+    int targetRps[2];
+
+    DigitalIn limitTop;//上リミット
+    DigitalIn limitBottom;//下リミット
+    DigitalIn limitMagazine;//装填リミット
+
+    Timer BallLoadTim;  //グルグルに乗るまで
+    Timer BallShotTim;  //グルグルからカップに乗るまで
+    
+    void MainFunction();
+    double RotationPerWhile(double degree, int num);
+    double VelocityPID(double Target_value, double Current_value, int motorNum);
+    double LoadingPID(double Target_value, double Current_value);
+};
+
+
+/* 定数定義 ------------------------------------------------------------------*/
+
+/* π */
+#define PI 3.1415926535
+
+/* モーターの最大数 */
+#define MotorMAX 11
+
+/* エンコーダーの最大数 */
+#define EncoderMAX 8
+
+
+/* マクロ定義 ----------------------------------------------------------------*/
+
+/* Enc角度から距離に変換 */
+#define DEG_TO_DIS(__DEGREE__) (100 * PI * (__DEGREE__) / 360)
+
+/* radian degree 変換 */
+#define DEG_TO_RAD(__DEGREE__) ((__DEGREE__)*PI / 180)
+#define RAD_TO_DEG(__RADIAN__) ((__RADIAN__)*180 / PI)
+
+#define ROUND(_DATA_) ((int)((_DATA_)*10)%10<=3?(_DATA_):(_DATA_)+1)
+
+//350 800
+#define STEP(_STEP_) ((_STEP_ > 2) ? 400 : 800)
+
+
+/* コントローラー SerectとStartを実装 */
+//#define Select (DS3.LEFTkey+DS3.RIGHTkey==2)
+//#define Start (DS3.UPkey+DS3.DOWNkey==2)
+
+/* 関数プロトタイプ宣言 -------------------------------------------------------*/
+/* 変数定義 ------------------------------------------------------------------*/
+/* クラス定義 ----------------------------------------------------------------*/
+#endif /* MAIN_H */
\ No newline at end of file