File content as of revision 8:d36996d8b4a0:

/************************************************
MotorDriver ver4.1 farmware
author : shundai miyawaki
2019/06/14 ~ 
Twitter : @WaatH5
Copyright shundai miyawaki. All rights reserved.

************************************************/

/*

// target board select checker of stm32f042k6t6
//未検証
#if !defined(TARGET_STM32F042K6)
#error This code is only for NUCLEO-F042K6 or STM32F042K6T6. Reselect a Platform.
#endif
*/

// target board select checker of stm32f303k8t6
//検証済
#if !defined(TARGET_STM32F303K8)
#error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform.
#endif

// debug switch
//#define DEBUG   //debug ON
#undef DEBUG  //debug OFF

// preproseccer
#include "dbg.h"
#include "mbed.h"
#include "PID.h"
#include "QEI.h"
//#include <CANformat.h>


// ゲインはモータードライバー固有のパラメータなのでflashに保存する，
// 初期値としてデフォルトの値をdefineで定義する
// theta feedback PID gain
#define theta_Kp 9.0f
#define theta_Ki 0.0f
#define theta_Kd 0.00001f

// omega feedback PID gain
#define omega_Kp 0.05f
#define omega_Ki 0.05f
#define omega_Kd 0.0f


#define ANGLE_THRESHOLD 2.0f //under 2.0 degree is decreaced

// CAN PID define

#define THETA_FEEDBACK_CCW 10
#define THETA_FEEDBACK_CW 11
#define OMEGA_FEEDBACK_CCW 20
#define OMEGA_FEEDBACK_CW 21
#define CURRENT_FEEDBACK_CCW 30
#define CURRENT_FEEDBACK_CW 31

#define MOTOR_STOP 0
#define THETA_FEEDBACK 10
#define OMEGA_FEEDBACK 20
#define CURRENT_FEEDBACK 30

// math value define 
#define M_PI 3.141593f

// constant value
#define ABLE 0
#define DISABLE 1
#define CCW 0
#define CW 1

// threshold value define
#define MOTOR_TARGET_OMEGA_THRESHOLD    0.5f   //[deg/s]
#define MOTOR_TARGET_THETA_THRESHOLD    0.5f   //[rpm]
#define MOTOR_TARGET_CURRENT_THRESHOLD  0.1f   //[A]

// stm32f303k8t6 unique serial address 
#define uniqueSerialAdd_kl_freescale (unsigned long *)0x40048058
#define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8
#define uniqueSerialAddr uniqueSerialAddr_stm32

// controll value define
#define PID_CONTROLL_PERIOD     8  //[ms]
#define ENCODER_CONTROLL_PERIOD  8  //[ms]
#define CAN_SEND_PERIOD 100 //[ms]
#define MOTOR_FREUENCY 8000 //[Hz]
#define CAN_FREQUENCY 100 //[kHz]
#define SERIAL_BAUDLATE 115200 //[bps]
#define CURRENT_SENSOR_OFFSET_COUNT 1000 // [count]

// current sensor define
#define CURRENT_SENSOR_VCC   3.3 //[V]
#define CURRENT_SENSOR_RANGE 105 //[mv/A]
#define CURRENT_LIMIT 1.5    //[A]

// turning by toukai robocon
#define MOTOR1_ID 393246
#define MOTOR2_ID -2147418100
#define MOTOR3_ID 196631
#define MOTOR4_ID 786469
#define MOTOR5_ID 1966107
#define MOTOR6_ID 1638418


//standerd define
Serial pc(PB_6, PB_7, SERIAL_BAUDLATE);
DigitalOut myled(LED1); //PB_3
DigitalIn userSW(PB_5);

//motor define
PwmOut PWM1(PA_4);
PwmOut PWM2(PA_6);
PwmOut PHASE(PB_0);
DigitalOut SR(PA_7);
PID thetaPID(theta_Kp, theta_Ki, theta_Kd, PID_CONTROLL_PERIOD*0.001f);
PID omegaPID(omega_Kp, omega_Ki, omega_Kd, PID_CONTROLL_PERIOD*0.001f);

//use encoder define

/*
    pulse per revolution : 1000
    phase : A, B, Z
    voltage : 10.8V - 20.0V
*/
//#define ENCODER_TRD_S1000B
#define ENCODER_E6A2_CW5C

#if defined ENCODER_TRD_S1000B
#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
#endif

#if defined ENCODER_E6A2_CW5C
#define ENCODER_PULSE_PER_REVOLUTION 2000 //(500*4)
QEI encoder(PA_3, PA_1, NC, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
#endif

//AnalogIn CPUtemp(ADC_TEMP);
AnalogIn Current(PB_1);

//CAN define
CAN can(PA_11, PA_12); //RD(PA_11,D10),TD(PA_12,D2)
CANMessage msg;

struct CANdata
{
    //int timeout;
    int deviceID;
    char recvDATA[8];
    char sendDATA[8];
};

struct CANdata MDCAN;



//void CANtimeout();
//void CANtest();

/*
int mode : select THETA_FEEDBACK(=1) or OMEGA_FEEDBACK(=2)
double target : enter the input value corresponding to the mode 
*/
void MotorDrive(int mode, double target);

//void CurrentFunction();
InterruptIn userButton(PB_5);
Ticker Encoder;
Ticker SEND;
Timer ENCODER_timer;

struct state
{
    int PID;
    double ThetaTarget;
    double OmegaTarget;
    double CurrentTarget;
    double theta;
    double omega;
    double current;
};

struct state Motor;

//Current Sensor setup   
double current_bias = 0.0;

void Velocity();
void ButtonFunction();
void SerialFunction();
void CANread();
void CANsend();
void CurrentFunction();
char SerialGet();

// 電流センサーをタイマー割込みにして，規定量を超えたら角速度の目標値を０にする．



void loadFlash(uint32_t address, uint8_t *data, uint32_t size);
void writeFlash(uint32_t address, uint8_t *data, uint32_t size);
void eraseFlash(uint32_t address);

float buf = 0.0f;

int main()
{
    // MPU setup
    DBG("MotorDriver_ver4.1 start! \r\n");
    DBG("/** MPU INFO **/ \r\n");
    DBG("Target PlatForm: STM32F303K8T6 \r\n");
    DBG("System Frequency: %d Hz \r\n", SystemCoreClock);
    DBG("Program File: %s \r\n", __FILE__);
    DBG("Compile Date: %s \r\n", __DATE__);
    DBG("Compile Time: %s \r\n", __TIME__);
    
    //Unique ID starting addresses for most of STM32 microcontrollers.
    long *id = (long *)0x1FFFF7AC; 
    
    DBG("%d, %d, %d \r\n", id[0], id[1], id[2]);
    
    switch (id[0])
    {
        case MOTOR1_ID:
            MDCAN.deviceID = 11;
            DBG("\r\n ==MOTOR1== \r\n\r\n");
            break;
        
        case MOTOR2_ID:
            MDCAN.deviceID = 12;
            DBG("\r\n ==MOTOR2== \r\n\r\n");
            break;
        
        case MOTOR3_ID:
            MDCAN.deviceID = 13;
            DBG("\r\n ==MOTOR3== \r\n\r\n");
            break;
        
        case MOTOR4_ID:
            MDCAN.deviceID = 14;
            DBG("\r\n ==MOTOR4== \r\n\r\n");
            break;
        
        case MOTOR5_ID:
            MDCAN.deviceID = 15;
            DBG("\r\n ==MOTOR5== \r\n\r\n");
            break;
        
        case MOTOR6_ID:
            MDCAN.deviceID = 16;
            DBG("\r\n ==MOTOR6== \r\n\r\n");
            break;

         default:
            
            while(1)
            {
                DBG("deviceID none define. It does not work!\r\n");
            }
    }
    
    DBG("deviceID : %d \r\n", MDCAN.deviceID);
    
    // user LED Indicator
    myled = 1;
    wait(1.0);
    myled = 0;
    
    // CAN setup 
    can.frequency(CAN_FREQUENCY*1000); //ROS default bps:100kbps -> 500kbps
    can.mode(CAN::Normal);
    //can.filter(MDCAN.deviceID, 0x0000000F, CANStandard, 14);
    //can.reset(); //delete receive buffer

    // motor signal setup
    PWM1.period(1.0f/MOTOR_FREUENCY);
    PWM2.period(1.0f/MOTOR_FREUENCY);
    PHASE.period(1.0f/MOTOR_FREUENCY);    
    PWM1 = 0.0f;
    PWM2 = 0.0f;
    PHASE = 0.0f;
    SR = 0;

    // PID position setup
    thetaPID.setInputLimits(-360.0f*100.0f, 360.0f*100.0f); //[deg]
    thetaPID.setOutputLimits(-240.0f, 240.0f);  //[rpm]
    thetaPID.setBias(0.0);
    thetaPID.setMode(AUTO_MODE);
    
    // PID omega setup
    omegaPID.setInputLimits(0.0, 240.0f*0.9f); //[rpm]
    omegaPID.setOutputLimits(0.0f, 1.0f); //[duty] 
    omegaPID.setBias(0.0);
    omegaPID.setMode(AUTO_MODE);

    // current sensor setup
    for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
    {
        current_bias += Current.read();    
    }
    
    current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
    DBG("current_bias_average: %f \r\n", current_bias);

    // interrupt start function
    userButton.mode(PullUp);
    userButton.fall(ButtonFunction);
    pc.attach(SerialFunction, Serial::RxIrq);
    can.attach(CANread, CAN::RxIrq);
    SEND.attach(CANsend, CAN_SEND_PERIOD*0.001f);
    ENCODER_timer.start();
    Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
    //NVIC_SetPriority(TIMER3_IRQn, 1);
    
    //initial target
    Motor.PID = OMEGA_FEEDBACK;     // initial drive mode is stop.
    Motor.ThetaTarget = 0.0f; // initial theta[deg]
    Motor.OmegaTarget = 0.0f;  // initial omega[rpm]

    while(1)
    { 
        pc.printf("%f \r\n", buf);
        
        if(buf > 0.0f)
        {
            PWM1 = 0.0f;
            PWM2 = buf;
        }
        
        else if(buf <= 0.0f)
        {
            PWM1 = buf;
            PWM2 = 0.0f;
        }
        
        //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
        
        //DBG("omegaPID.compute : %6.3f thetaPID.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", omegaPID.compute(), thetaPID.compute(), Motor.theta, Motor.omega);
        
        switch(Motor.PID)
        {
            case THETA_FEEDBACK:
            
                //DBG("THETA_FEEDBACK \r\n");
                MotorDrive(THETA_FEEDBACK, Motor.ThetaTarget);
                break;
            
            case OMEGA_FEEDBACK:
            
                //DBG("OMEGA_FEEDBACK \r\n");
                MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget);
                break;
                
            case CURRENT_FEEDBACK:
                
                //DBG("CURRENT_FEEDBACK \r\n");
                MotorDrive(CURRENT_FEEDBACK, Motor.CurrentTarget);
        
            case MOTOR_STOP:
            
                //DBG("MOTOR_STOP \r\n");
                MotorDrive(OMEGA_FEEDBACK, 0.0f);
                break;
                
            default:    // the others PID is stop.
            
                MotorDrive(OMEGA_FEEDBACK, 0.0f);
                break;        
        }
        
        //pc.printf("PID[%d], TT[%6.3f deg], OT[%6.3f rpm], T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.PID, Motor.ThetaTarget, Motor.OmegaTarget, Motor.theta, Motor.omega);
        pc.printf("T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.theta, Motor.omega);
        
        //DBG("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,thetaPID.compute());  
        //CurrentFunction();
        
    }
}

void SerialFunction()
{
    if(pc.readable() == 1)
    {
        char data = pc.getc();
        DBG("get data:%c \r\n", data);

        switch (data)
        {
            case 'p': 
                //Motor.PID = THETA_FEEDBACK;
                //Motor.ThetaTarget += 30.0f;
                buf += 0.05f;
                DBG("get 'p' \r\n");    
                
                break;
            
            case 'l':
                //Motor.PID = THETA_FEEDBACK;
                //Motor.ThetaTarget -= 30.0f;
                buf -= 0.05f;
                DBG("get 'l' \r\n");
                
                break;
                
            case 'o': 
                Motor.PID = OMEGA_FEEDBACK;  
                Motor.OmegaTarget += 10.0f;
                DBG("get 'q' \r\n");    
                
                break;
            
            case 'k':
                Motor.PID = OMEGA_FEEDBACK;
                Motor.OmegaTarget -= 10.0f;
                DBG("get 'a' \r\n");
                
                break;
    
            case 'r':
                
                DBG("\r\nSystem Reset! \r\n");
                NVIC_SystemReset();
                
                break;
                
            case 'f': //PID free (PWM only)
                
                
                /*
                while(1)
                {
                    SR = ABLE;
                    PHASE = (float)CW;
                    PWM1 = 0.05f;
                    PWM2 = 1.0f;
                }
                */
                
            default:
                
                DBG("The others key recieve \r\n");
                
                break;
        }
    }
}

void ButtonFunction()
{
    //serial menu printf
    DBG("setup open \r\n");
    DBG("1: Theta Controll\r\n");
    DBG("2: Omega Controll\r\n");
    DBG("3: Exit setup menu...\r\n");
    
    DBG("Send to MD11 test CAN data\r\n");
    
    MDCAN.sendDATA[0] = 21; // omega FB CCw
    MDCAN.sendDATA[1] = 60; // 60 rpm
    MDCAN.sendDATA[2] = 0;
    MDCAN.sendDATA[3] = 0;
    MDCAN.sendDATA[4] = 0;
    MDCAN.sendDATA[5] = 0;
    MDCAN.sendDATA[6] = 0;
    MDCAN.sendDATA[7] = 0;
    
    if(!can.write(CANMessage(12, MDCAN.sendDATA, 8, CANData, CANStandard))) 
    {
        DBG("CAN send error! \r\n"); 
    }
    
    //CANtest();  //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
    
    //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
    
    //ボタン情報を割込みで入力させる関数を用意してボタン情報を入力する．
    
    //モータが駆動し始める電流を測定する．（パルスが増えるまでＰＷＭを微小増加させる）
    /*
    Motor.OmegaTarget = 60.0f; //[rpm]
    
    while(1)
    {
        // motor stop
        PWM1 = 0.0f;
        PWM2 = 0.0f;
    }
    */
}

void CANread()
{
    // 1. when data received.
    // 2. when CAN data ID equal my ID
    // 3. when CAN data format is Standerd
    // 4. Not the same as the one received in the past
    // 同じものは受信しないプログラムを消したら通信できた446re
    if(can.read(msg) != 0  &&
       MDCAN.deviceID == msg.id && 
       msg.format == 0 )
    {
        DBG("CAN recieve \r\n");    
        myled = !myled;
        
        DBG("recv ID : %d", msg.id);
            
        for(int i = 0 ; i < 8 ; i++)
        {
            MDCAN.recvDATA[i] = msg.data[i];
            DBG("[%d] ", MDCAN.recvDATA[i]);
        }
    
        DBG("\r\n");
    
        switch(MDCAN.recvDATA[0]) //check PID
        {
            //送られてくるデータはかならず正なので１バイト目のCCW，CWじょうほうを目標値に付け加える．
            case THETA_FEEDBACK_CW: //MD data send
            
                Motor.PID = THETA_FEEDBACK;
                Motor.ThetaTarget = (double)MDCAN.recvDATA[1];
                DBG("theta CW FB CAN \r\n");
                break;
            
            case THETA_FEEDBACK_CCW: //MD data send    
                
                Motor.PID = THETA_FEEDBACK;
                Motor.ThetaTarget = -1.0f*(double)MDCAN.recvDATA[1];
                DBG("theta CCW FB CAN \r\n");
                break;
            
            case OMEGA_FEEDBACK_CW:
            
                // 1 rpm(0.0166 rps)
                Motor.PID = OMEGA_FEEDBACK;
                Motor.OmegaTarget = (double)MDCAN.recvDATA[1];
                DBG("omega CW FB CAN \r\n");  
                break;
            
            case OMEGA_FEEDBACK_CCW:
            
                // 1 rpm(0.0166 rps)
                Motor.PID = OMEGA_FEEDBACK;
                Motor.OmegaTarget = -1.0f*(double)MDCAN.recvDATA[1];
                DBG("omega CCW FB CAN \r\n");  
                break;
        
            default:
            
                DBG("recv CAN PID error \r\n");
                
                break;
        }
    }
}

void Velocity()
{
    static int pulse; //old pulse

    Motor.omega = 60.0f*((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
    pulse = encoder.getPulses(); //pulse update
    
    Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
    
    ENCODER_timer.reset(); //timer reset:t=0s
    if(Motor.omega > 0)
    {
        MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CW;
    }
    
    else
    {
        MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CCW;   
    }
    //pc.printf("%d \r\n", abs(int(Motor.omega)));
    //MDCAN.sendDATA[1] = abs(int(Motor.omega));
    //MDCAN.sendDATA[2] = 0;
    //MDCAN.sendDATA[3] = 0;
    //MDCAN.sendDATA[4] = 0;
    //MDCAN.sendDATA[5] = 0;
    //MDCAN.sendDATA[6] = 0;
    //MDCAN.sendDATA[7] = 0;
    
    /*
    if(!can.write(CANMessage(MDCAN.deviceID*10, MDCAN.sendDATA, 8, CANData, CANExtended))) 
    {
        DBG("CAN send error! \r\n"); 
    }
    */
    
    //DBG("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
}

void CANsend()
{
    //CANSenｄがデータ集めて送信（50mstoka）
    //__disable_irq();
    //Encoder.detach();
        
    //data -> Can data
    
    switch (MDCAN.sendDATA[0])
    {
        case THETA_FEEDBACK:
            MDCAN.sendDATA[1] = abs(int(Motor.theta));
            break;

        case OMEGA_FEEDBACK:
            MDCAN.sendDATA[1] = abs(int(Motor.omega));
            break;
    }
    
    
    MDCAN.sendDATA[2] = 0;
    MDCAN.sendDATA[3] = 0;
    MDCAN.sendDATA[4] = 0;
    MDCAN.sendDATA[5] = 0;
    MDCAN.sendDATA[6] = 0;
    MDCAN.sendDATA[7] = 0;
    
    //余りを計算して，端数は削除するとCANデータに変換できる
    if(!can.write(CANMessage((char)((int)MDCAN.deviceID-10)*10+100, MDCAN.sendDATA, 8, CANData, CANStandard))) 
    {
        DBG("CAN send error! \r\n"); 
    }
    
    else 
    {
        pc.printf("send !!");
    }

    //__enable_irq();
    //Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 30ms
    //pc.attach(SerialFunction, Serial::RxIrq);
    //userButton.fall(ButtonFunction);
    //can.attach(CANread, CAN::RxIrq);
}

void CurrentFunction()
{
        
}

void MotorDrive(int mode, double target)
{   
    //theta feebback(mode 1)
    //omega feedback(mode 2)
    //current feedback(mode 3)

    //static int old_direction;
    //static double old_target;

    switch (mode)
    {
        case THETA_FEEDBACK:
        
            //DBG("THETA_FEEDBACK!! \r\n");
        
            thetaPID.setSetPoint(target); 
            thetaPID.setProcessValue(Motor.theta);
            
            if(target >= 0)
            {
                // Friction compensation
                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止中
                {
                    //SR = ABLE;
                    //PHASE = (float)CW;
                    //PWM1 = 0.9f;
                    //PWM2 = 1.0f;  
                }
                  
                if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
                {
                    MotorDrive(OMEGA_FEEDBACK, thetaPID.compute());
                }
                
                else //誤差の範囲内ならモータのPWMを停止させる
                {
                    MotorDrive(OMEGA_FEEDBACK, 0.0); 
                    PWM1 = 0.0f;
                    PWM2 = 1.0f;  
                }    
            }
        
            else
            {   
                // Friction compensation
                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止時
                {
                    //SR = ABLE;
                    //PHASE = (float)CCW;
                    //PWM1 = 0.9f;
                    //PWM2 = 1.0f;  
                }
                
                if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
                {
                    MotorDrive(OMEGA_FEEDBACK, thetaPID.compute());
                }
                
                else //誤差の範囲内ならモータのPWMを停止させる
                {
                    MotorDrive(OMEGA_FEEDBACK, 0.0);
                    PWM1 = 0.0f;
                    PWM2 = 1.0f;  
                }
            }
            
            break;
        
        case OMEGA_FEEDBACK:
            
            //DBG("OMEGA_FEEDBACK  ");
            
            if(target >= 0.0f)
            {  
                omegaPID.setSetPoint(abs(target)); //目標値入力
                omegaPID.setProcessValue(abs(Motor.omega));//センサー入力(絶対値いる？)
                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CW;
                PWM1 = omegaPID.compute();
                PWM2 = 1.0f;   
                
                //DBG("%f \r\n", (float)direction);   
            }
                
            else
            {
                omegaPID.setSetPoint(abs(target)); 
                omegaPID.setProcessValue(abs(Motor.omega));
                //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CCW;
                PWM1 = omegaPID.compute();
                PWM2 = 1.0f;       
            }                  
                    
            break;
        
        case CURRENT_FEEDBACK:
            
            DBG("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
            
            break;
        
        default:
            
            DBG("Function:MotorDriver mode error! \r\n");
            
            break;
    }
    
    //old_direction = direction;
    //old_target = target;
}

void eraseFlash(uint32_t address)
{
    FLASH_EraseInitTypeDef erase;
    erase.TypeErase = FLASH_TYPEERASE_PAGES;    /* ページの消去を選択 */
    erase.PageAddress = address;                /* ページの先頭アドレスを指定 */
    erase.NbPages = 1;      /* 消すページの数．今回は1つだけ */

    uint32_t pageError = 0;

    HAL_FLASHEx_Erase(&erase, &pageError);  /* HAL_FLASHExの関数で消去 */
}

void writeFlash(uint32_t address, uint8_t *data, uint32_t size)
{
    uint32_t *dataWord = (uint32_t*)data;   /* 書き込むデータへのポインタ(4Byteごと) */
    uint32_t sizeWord = (size+3)/4;    /* データのサイズ(4Byteで1) */
    
    HAL_FLASH_Unlock();        /* フラッシュをアンロック */
    eraseFlash(address);            /* 指定したアドレスのページを消去 */
    do {
        /* 4Byte(Word)ずつフラッシュに書き込む */
        HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, *dataWord);
    } while (address+=4, ++dataWord, --sizeWord);
     HAL_FLASH_Lock();        /* フラッシュをロック */
}

void loadFlash(uint32_t address, uint8_t *data, uint32_t size)
{
    memcpy(data, (uint8_t*)address, size);
}