File content as of revision 5:58c684e2ee47:

/***************************************
MotorDriver Ver4 or Ver5 or Ver6 select
***************************************/
//<注意>プログラム上でのバージョンです 書き込む基板に合わせてここだけ変更してください．
//Ver4 2019Bチーム手動機で使っていたMD(CAN通信のコネクタとPWM入力用のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが４つ)
//Ver5 2019Bチーム自動機で使っていたMD(CAN通信のコネクタとロータリーエンコーダ入力用のコネクタがあります　全部でコネクタが３つ)
//Ver6 センサ入力に対応したMD
#define MOTOR_DRIVER_VERSION 5
/***************************************
        SMB or LAP select
***************************************/
//駆動方式の選択 選択する方のコメントアウトを外してください．
//<default> SMBモード
//<option>  LAPモード
#define MOTOR_DRIVER_MODE_SMB
//#define MOTOR_DRIVER_MODE_LAP
/*************************************************************************************************************************/
//                                              souce code
/***************************
     include file
***************************/
#include "mbed.h"
#include "can_network_motordriver_definition.h"
#include "bitCommunication.h"
#include "buzzer.h"
#include "eeprom.h"
#include "eeprom_initalize.h"
//#include "QEI.h"
#include "qeihw.h"
#include "Pid.h"
#include "motorDrive_SMB.h"
#include "motorDrive_LAP.h"
#include "MedianFilter.h"
#include "definition.h"
/******************************
    definition instance
******************************/
CAN can(CAN_PIN[CAN_RD],CAN_PIN[CAN_TD]);
Serial pc(SERIAL_PIN[SERIAL_TX],SERIAL_PIN[SERIAL_RX],communication_baud::PC);
BusIn idSwitch(ID_PIN[SWITCH1],ID_PIN[SWITCH2],ID_PIN[SWITCH3],ID_PIN[SWITCH4]);
//QEI encoder(ENCODER_PIN[ENCODER_A],ENCODER_PIN[ENCODER_B],NC,ENCODER_PPR);
QEIHW qei(QEI_DIRINV_NONE,QEI_SIGNALMODE_QUAD,QEI_CAPMODE_4X,QEI_INVINX_NONE);      //QEI_hw
buzzer led(LED_PIN[FREE]);
PwmOut pwmLed[2] = {
    PwmOut(LED_PIN[CW]),
    PwmOut(LED_PIN[CCW]),
};
Pid pid;
Ticker controlCycle;
Timer errorTimer;
#ifdef MOTOR_DRIVER_MODE_LAP
motorDriveLAP motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
#else
motorDriveSMB motor(A3921_PIN[PWMH],A3921_PIN[PHASE]);
#endif
#if MOTOR_DRIVER_VERSION == 4
DigitalOut pwml(PWML_PIN,true);
#endif
/******************************
      global variable
******************************/
float parameter[PARAMETER_NUMBER] = {0.0f,0.0f,0.0f,0.0f};
float outputDuty = 0.0f;
float targetRpm = 0.0f;
float rps = 0.0f;
float rpm = 0.0f;
float rotation[2] = {0.0f,0.0f};
int powerFilterCount;
int powerFilterArray[10];
bool enable_motor_output = false;    //モーターへの出力許可を管理(true : 出力可能 , false : 出力を強制停止)
bool outputMode = false;
bool printfMode = false;
bool receiveParameterStatus[4] = {false,false,false,false};
/******************************
    definition function
******************************/
void initalize();
void mainLoop();
void outputIndicator(float control_duty);
void safetyControl(float duty);
void canReceiveHandler();
void debag();
/******************************
      function content
******************************/
int main() {
    initalize();
    while(1){
    }
}

void initalize() {
    /***************************EEPROM　読み取り処理*******************************/
    can_flame eepromData;
    Chip_EEPROM_init();
    __disable_irq();
    read_eeprom((char*)0, eepromData.c, 4);
    parameter[KP] = eepromData.f;
    read_eeprom((char*)4, eepromData.c, 4);
    parameter[KI] = eepromData.f;
    read_eeprom((char*)8, eepromData.c, 4);
    parameter[KD] = eepromData.f;
    __enable_irq();
    /***************************************************************************/
    
    /******************************割り込み優先度設定******************************/
    NVIC_SetPriority(C_CAN0_IRQn,0);
    NVIC_SetPriority(QEI_IRQn,1);
    /***************************************************************************/
    
    /*******************************CAN通信の設定*********************************/
    can.frequency(communication_baud::CAN);
    can.attach(&canReceiveHandler,CAN::RxIrq);
    /***************************************************************************/
    
    /***************************MotorDrive 設定**********************************/
    motor.setupFrequency(motor_option::FREQUENCY);
    motor.setupLimitDuty(motor_option::LOWER_LIMIT,motor_option::UPPER_LIMIT);
    /****************************************************************************/
    
    /*********************************SpeedPID設定*******************************/
    pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
    /***************************************************************************/
    
    /**********************************その他************************************/
    qei.SetDigiFilter(480UL);                   //QEI_hw filter設定
    qei.SetMaxPosition(0xFFFFFFFF);             //QEI_hw filter設定
    qei.SetVelocityTimerReload_us(10000);       //QEI_hw filter設定
    
    errorTimer.start();                                     //CAN通信 タイムアウト検知用計測開始
    pc.printf("\r\nInitalize MotorDriver!\r\n");            //initalize終了出力
    pc.printf("Firmware:robocon2021_TBCMotorDriver( https://os.mbed.com/users/YutaTogashi/code/robocon2021_TBCMotorDriver/ )\r\n");  //Firmwareをシリアル出力
    pc.printf("UsingLibrary:can_network_motordriver ( https://os.mbed.com/users/YutaTogashi/code/can_network_motordriver/ )\r\n");   //MDを使用しやすくしたライブラリ　シリアル出力
    controlCycle.attach(&mainLoop,period::CONTROL_CYCLE);   //制御ループ設定
    /***************************************************************************/
}

void mainLoop() {   
    //RPS,RPMの計算
    rotation[0] = static_cast<float>(static_cast<int32_t>(qei.GetPosition())) / (static_cast<float>(ENCODER_PPR) * 4.0f);
    rps = (rotation[0] - rotation[1]) / period::CONTROL_CYCLE;
    rpm = rps * 60.0f;
    rotation[1] = rotation[0];
    
    //速度PIDモードの挙動
    if(outputMode) {
        pid.setup(parameter[KP],parameter[KI],parameter[KD],SPEED_PID,period::CONTROL_CYCLE);
        pid.calculate(targetRpm,/*encoder.getRPM()*/rpm);
        outputDuty = pid.getDuty();
    }
    
    //TIMEOUT判定
    if(errorTimer.read() > TIMEOUT_VALUE)   enable_motor_output = false;
    else                                    enable_motor_output = true;
    
    //出力処理
    if(enable_motor_output) {
        led.output(outputMode);     //速度PIDモードの場合は，LEDインジゲータが点灯
    } else {
        led.output(0.5f);           //TIMEOUT時は，LEDインジゲータが点滅
        safetyControl(0.05f);       //徐々に出力を下げる
        pid.reset();                //速度PIDの計算をリセット
    }
    
    outputIndicator(outputDuty);    //Duty,Phaseインジゲータを制御
    motor.output(outputDuty);       //A3921制御

    debag();                        //デバッグ関数
}

void debag() {
    if(!(printfMode)) {
        pc.printf("SWITCH:%d\t",idSwitch.read());
        pc.printf("CAN_ID:%d\t", (MD_0_MCU + idSwitch.read()));
        pc.printf("DUTY:%.2f\t",outputDuty);
        pc.printf("RPM:%.2f\t",/*encoder.getRPM()*/rpm);
        
        if(outputMode) {
            pc.printf("TARGET:%.1f\t",targetRpm);
            pc.printf("Kp:%f\t",parameter[KP]);
            pc.printf("Ki:%f\t",parameter[KI]);
            pc.printf("Kd:%f\t",parameter[KD]);
        }
        pc.printf("TIME:%f",errorTimer.read());
        pc.printf("\r\n");
    } else {
        pc.printf("%f",targetRpm);
        pc.printf("%f,",/*encoder.getRPM()*/rpm);
        pc.printf(",%f",outputDuty);
        pc.printf("\r\n");
    }
}

void outputIndicator(float duty) {
    if(fabs(duty) > 0.0f) {
        if(duty > 0.0f) {
            pwmLed[CW].write(fabs(duty));
            pwmLed[CCW].write(0.0f);
        } else {
            pwmLed[CW].write(0.0f);
            pwmLed[CCW].write(fabs(duty));
        }
    } else {
        pwmLed[CW].write(0.0f);
        pwmLed[CCW].write(0.0f);
    }
}

void safetyControl(float control_duty) {
    if(fabs(outputDuty - 0.0f) > 0.10f) {
        if(outputDuty > 0.0f)   outputDuty -= control_duty;
        else                    outputDuty += control_duty;
    } else {
        outputDuty = 0.0f;   
    }
}

void canReceiveHandler() {
    can_flame message,returnMessage;
    CANMessage sendMsg,receiveMessage;
    
    can.read(receiveMessage);
    
    sendMsg.id = MAIN_MCU;

    if((MD_0_MCU + idSwitch.read()) == receiveMessage.id) {
        errorTimer.reset();
        switch ((int)receiveMessage.data[0]) {
            case md_can_flame::SEND_KP:
                for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                __disable_irq();
                write_eeprom(message.c,(char*)0,4);
                __enable_irq();
                parameter[KP] = message.f;
                receiveParameterStatus[0] = true;
                break;
            case md_can_flame::SEND_KI:
                for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                __disable_irq();
                write_eeprom(message.c,(char*)4,4);
                __enable_irq();
                parameter[KI] = message.f;
                receiveParameterStatus[1] = true;
                break;
            case md_can_flame::SEND_KD:
                for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                __disable_irq();
                write_eeprom(message.c,(char*)8,4);
                __enable_irq();
                parameter[KD] = message.f;
                receiveParameterStatus[2] = true;
                break;
            case md_can_flame::SEND_ACCELERATION:
                for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                parameter[ACCELERATION] = message.f;
                receiveParameterStatus[3] = true;
                break;
            case md_can_flame::SEND_CONTROL_DUTY:
                for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                outputDuty = message.f;
                outputMode = false;
                break;
            case md_can_flame::SEND_CONTROL_RPM:
                if(bitRead((int)receiveMessage.data[1],0)) {
                    for(int i=2;i<6;i++)    message.c[i-2] = receiveMessage.data[i];
                    targetRpm = message.f;
                    outputMode = true;
                } else {
                    targetRpm = 0.0f;
                    outputDuty = 0.0f;
                    pid.reset();
                    outputMode = false;
                }
                break;
            case md_can_flame::REQUEST_RECEIVE_CURRENT:
                //no code
                break;
            case md_can_flame::REQUEST_RECEIVE_ERROR:
                int parameterStatus = 0;
                for(int i=0;i<4;i++) bitWrite(&parameterStatus,i,receiveParameterStatus[i]);
                sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ERROR;
                sendMsg.data[2] = parameterStatus;
                can.write(sendMsg);
                break;
            case md_can_flame::REQUEST_RECEIVE_POSITION:        //dont useing
                //returnMessage.i = encoder.getPosition();
                returnMessage.i = qei.GetPosition();
                sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_POSITION;
                for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
                can.write(sendMsg);
                break;
            case md_can_flame::REQUEST_RECEIVE_ROTATION:
                //returnMessage.f = static_cast<float>(encoder.getRotation());
                returnMessage.f = rotation[0];
                sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_ROTATION;
                for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
                can.write(sendMsg);
                break;
            case md_can_flame::REQUEST_RECEIVE_RPS:
                //returnMessage.f = encoder.getRPS();
                returnMessage.f = rps;
                sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPS;
                for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
                can.write(sendMsg);
                break;
            case md_can_flame::REQUEST_RECEIVE_RPM:
                //returnMessage.f = encoder.getRPM();
                returnMessage.f = rpm;
                sendMsg.data[0] = md_can_flame::REQUEST_RECEIVE_RPM;
                for(int i=0;i<4;i++)    sendMsg.data[i+2] = returnMessage.c[i];
                can.write(sendMsg);
                break;
            case md_can_flame::REQUEST_PRINTF_DEBAG:
                printfMode = false;
                break;
            case md_can_flame::REQUEST_PRINTF_FORMAT_CSV:
                printfMode = true;
                break;
            case md_can_flame::REQUEST_RESET_QEI:
                //encoder.reset();
                qei.Reset(QEI_RESET_POS);
                break;
            case md_can_flame::REQUEST_RESET_MCU:
                NVIC_SystemReset();
                break;
            default:
                break;
        }
    } else if(ALL_MCU == receiveMessage.id) {
        NVIC_SystemReset();
    } else {
        
    }
}