File content as of revision 9:c1dc00ede75d:

/***************************************
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 "Can_network_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を使用しやすくしたライブラリ　シリアル出力
    #if MOTOR_DRIVER_VERSION == 4
    pc.printf("MotorDriverVersion:4\r\n");
    #elif MOTOR_DRIVER_VERSION == 5
    pc.printf("MotorDriverVersion:5\r\n");
    #elif MOTOR_DRIVER_VERSION == 6
    pc.printf("MotorDriverVersion:6\r\n");
    #endif
    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("ID:%d\t", (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;
    
    float value = 0.0f;
    CANMessage msg;
    can.read(msg);
    
    if((getCanIdData(msg.id,0) == can_protcol::device_type::MOTOR_DRIVER) && (getCanIdData(msg.id,1) == idSwitch.read())) {           //MD基板宛のメッセージのみ抽出
        errorTimer.reset();                                                    //TIMEOUT時間管理用タイマーをリセット
        switch (((static_cast<uint8_t>(msg.data[0])) >> 4)) {
            case can_protcol::device_type::CONTROL_MCU:                        //メイン制御基板からきたデータを抽出
            case can_protcol::device_type::PC:                                 //PCからきたデータを抽出
                switch (getCanIdData(msg.id,2)) {       //DataTypeを確認
                    case can_protcol::data_type::motor_driver::DEBAG_MODE:                  //debag mode
                        break;
                    case can_protcol::data_type::motor_driver::PARAMETER:                   //パラメータ受信
                        switch (static_cast<uint8_t>(msg.data[1])) {
                            case 0:             //PositionPid Kp
                                break;
                            case 1:             //PositoinPid Ki
                                break;
                            case 2:             //PositionPid Kd
                                break;
                            case 3:             //VelocityPid Kp
                                memcpy(&value,msg.data+2,4);
                                parameter[KP] = value;
                                receiveParameterStatus[0] = true;
                                break;
                            case 4:             //VelocityPid Ki
                                memcpy(&value,msg.data+2,4);
                                parameter[KI] = value;
                                receiveParameterStatus[1] = true;
                                break;
                            case 5:             //VelocityPid Kd
                                memcpy(&value,msg.data+2,4);
                                parameter[KD] = value;
                                receiveParameterStatus[2] = true;
                                break;
                            case 6:             //Acceleration
                                break;
                            case 7:             //Timeout Time
                                break;
                            case 8:             //Max Duty
                                break;
                            case 9:             //Max Velocity(RPM)
                                break;
                            case 10:            //Encoder PPR
                                break;
                            case 11:            //AbsEncoder Reset
                                break;
                            default:
                                break;   
                        }
                        break;
                    case can_protcol::data_type::motor_driver::CONTROL_INFORMATION:         //制御量受信
                        switch (static_cast<uint8_t>(msg.data[1])) {
                            case 0:                     //duty駆動
                                memcpy(&outputDuty,msg.data+2,4);
                                outputMode = false;
                                break;
                            case 1:                     //速度制御                                
                                if((static_cast<uint8_t>(msg.data[6]) & 0b1) == 0b1) {      //駆動電源のフィードバック情報より駆動電源が入力されている場合の挙動
                                    memcpy(&targetRpm,msg.data+2,4);
                                    outputMode = true;
                                } else {                                                    //駆動電源のフィードバック情報より駆動電源が入力されていない場合の挙動
                                    targetRpm = 0.0f;
                                    outputDuty = 0.0f;
                                    pid.reset();
                                    outputMode = false;
                                }
                                break;
                            default:
                                break;   
                        }
                        break;
                    case can_protcol::data_type::motor_driver::STEER_CONTROL_INFORMATION:   //(ステア用)制御量受信
                        break;
                    case can_protcol::data_type::motor_driver::PRINTF_FORMAT:               //printf出力形式の指定情報を受信
                        break;
                    default:
                        break;
                }
                break;
            default:
                break;
        }
    }
    
    
    //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 {
        
    }
    */
}