File content as of revision 1:fa3652aca312:

/* 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();

        limitTop.mode(PullDown);
        limitBottom.mode(PullDown);
        limitMagazine.mode(PullDown);
        
        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) ? 300 : 800)


/* コントローラー SerectとStartを実装 */
//#define Select (DS3.LEFTkey+DS3.RIGHTkey==2)
//#define Start (DS3.UPkey+DS3.DOWNkey==2)

/* 関数プロトタイプ宣言 -------------------------------------------------------*/
/* 変数定義 ------------------------------------------------------------------*/
/* クラス定義 ----------------------------------------------------------------*/
#endif /* MAIN_H */