File content as of revision 4:40a764acc4ec:

/************************************************
MotorDriver ver4.1 farmware
author : shundai miyawaki
2019/06/14 ~ 

Copyright shundai miyawaki. All rights reserved.

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

#if !defined(TARGET_STM32F303K8)
#error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform.
#endif

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

#include "dbg.h"
#include "mbed.h"
//#include <CANformat.h>

#include "PID.h"
#include "QEI.h"

#define Kp2 0.9f
#define Ki2 0.0f
#define Kd2 0.004f

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

#define Kp 0.4f
#define Ki 0.8f
#define Kd 0.0f

#define THETA_FEEDBACK 1
#define OMEGA_FEEDBACK 2
#define CURRENT_FEEDBACK 3

#define SERIAL_BAUDLATE 115200
#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
#define CURRENT_SENSOR_OFFSET_COUNT 1000

#define M_PI 3.141593f

#define ABLE 0
#define DISABLE 1

#define CW 1
#define CCW 0
#define MOTOR_FREUENCY 10000 //[Hz]
#define CAN_FREQUENCY 500 //[kHz]

#define MOTOR_TARGET_OMEGA_THRESHOLD    0.5f   //[deg/s]
#define MOTOR_TARGET_THETA_THRESHOLD    0.5f   //[rad]
#define MOTOR_TARGET_CURRENT_THRESHOLD  0.1f   //[A]

#define uniqueSerialAdd_kl_freescale (unsigned long *)0x40048058
#define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8
#define uniqueSerialAddr uniqueSerialAddr_stm32

//Controll Period
#define PID_CONTROLL_PERIOD     10  //[ms]
#define ENCODER_CONTROLL_PERIOD  8  //[ms]

//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 0000000004
#define MOTOR5_ID 0000000005
#define MOTOR6_ID 0000000006

/*
readしたらCAN.readDATAを0にする
read時は割込み禁止で，CAN timeoutの割込みで0のままなら，すべて停止
*/
/*
#define CAN_TIMEOUT 3000 //[ms]
#define ENCODER_PPR 100.0f
*/

//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(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
PID omegaPID(Kp2, Ki2, Kd2, PID_CONTROLL_PERIOD*0.001f);
QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
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 CANsend();
//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;
Timer ENCODER_timer;

struct state
{
    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 CurrentFunction();

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

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.
    unsigned long *id = (unsigned 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\r\n");
            }
    }
    
    DBG("deviceID : %d \r\n", MDCAN.deviceID);
        
    myled = 1;
    wait(1.0);
    myled = 0;
    
    // CAN setup 
    can.frequency(500000); //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
    omegaPID.setInputLimits(-360.0f, 360.0f);
    omegaPID.setOutputLimits(-3.68f, 3.68f); 
    omegaPID.setBias(0.0);
    omegaPID.setMode(AUTO_MODE);
    
    // PID omega setup
    thetaPID.setInputLimits(0.0f, 3.68f);
    thetaPID.setOutputLimits(0.0f, 1.0f); 
    thetaPID.setBias(0.0);
    thetaPID.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);
    ENCODER_timer.start();
    Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
    //NVIC_SetPriority(TIMER3_IRQn, 1);
    
    //initial target
    Motor.ThetaTarget = 90.0f; //initial theta[deg]
    Motor.OmegaTarget = 0.0f;  //initial omega[rps]

    while(1)
    { 
        //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);
        pc.printf("%6.3f,%6.3f\r\n", Motor.theta, Motor.omega);
        
        //MotorDrive(THETA_FEEDBACK, Motor.ThetaTarget);
        MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget);
        
        //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 'q': 
                            
                Motor.OmegaTarget += 0.5f;
                DBG("get 'q' \r\n");    
                
                break;
            
            case 'a':
    
                Motor.OmegaTarget -= 0.5f;
                DBG("get 'a' \r\n");
                
                break;
            
            case 'p': 
                
                Motor.ThetaTarget += 30.0f;
                DBG("get 'p' \r\n");    
                
                break;
            
            case 'l':
        
                Motor.ThetaTarget -= 30.0f;
                DBG("get 'l' \r\n");
                
                break;
            
                
            case 'r':
                
                DBG("\r\nSystem Reset! \r\n");
                NVIC_SystemReset();
                
                break;
            
            default:
                
                DBG("The others key recieve \r\n");
                
                break;
        }
    }
}

void ButtonFunction()
{
    Motor.OmegaTarget = 0.0f; //motor stop
     
    DBG("setup open \r\n");
    DBG("1:change the CAN device ID \r\n");
    DBG("2:change the CAN device ID \r\n");
    
    //CANtest();  //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
    
    //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
    
    while(1)
    {
        if(!pc.readable())
        {
           // pc.scanf("%d", cmd);
        }
        
        else
        {
            DBG("old device ID:\r\n");
            DBG("new device ID:\r\n");
            
            while(1)
            {
                
            }    
        }
    }
}

void CANread()
{
    //__disable_irq(); //All Interrupt disabled
    //Encoder.detach();
    
    
    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
        {
            case THETA_FEEDBACK: //MD data send
                
                /*
                int sum = 0;
                
                for(int i = 0 ; i < 8 ; i++)
                {
                    sum += MDCAN.recvDATA[i];
                }
                */
                
                //現在の分解能は 1 deg
                Motor.ThetaTarget = (double)MDCAN.recvDATA[1];
                DBG("theta FB can \r\n");
            
                break;
            
            case OMEGA_FEEDBACK:
            
                //現在の分解能は 1 rps
                Motor.OmegaTarget = (double)MDCAN.recvDATA[1];
                DBG("omega FB can \r\n");  
                break;
        
            default:
            
                DBG("recv CAN PID error \r\n");
                
                break;
        }
    }
            
    //thetaPID.setSetPoint((double)MDCAN.recvDATA[1]);     //motor target value velocity
    //__enable_irq();
    //Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 10ms
    //pc.attach(SerialFunction, Serial::RxIrq);
    //userButton.fall(ButtonFunction);
    //can.attach(CANread, CAN::RxIrq);
}

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

    Motor.omega = ((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
    /*
    CANsend(OMEGA_FEEDBACK, );
    
    MDCAN.sendDATA[0] = OMEGA_FEEDBACK;
    MDCAN.sendDATA[1] = ;
    MDCAN.sendDATA[2] = ;
    MDCAN.sendDATA[3] = ;
    MDCAN.sendDATA[4] = ;
    MDCAN.sendDATA[5] = ;
    MDCAN.sendDATA[6] = ;
    MDCAN.sendDATA[7] = ;
    */
    //DBG("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
}

void CANsend(int id, int PID)
{
    //__disable_irq();
    //Encoder.detach();
        
    //data -> Can data
    
    //余りを計算して，端数は削除するとCANデータに変換できる
    if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended))) 
    {
        DBG("CAN send error! \r\n"); 
    }

    //__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");
        
            omegaPID.setSetPoint(target); 
            omegaPID.setProcessValue(Motor.theta);
            
            if(target >= 0)
            {
                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止中
                {
                    SR = ABLE;
                    PHASE = (float)CW;
                    PWM1 = 0.3f;
                    PWM2 = 1.0f;  
                }
                  
                if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
                {
                    MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
                }
                
                else //誤差の範囲内ならモータのPWMを停止させる
                {
                    MotorDrive(OMEGA_FEEDBACK, 0.0); 
                    PWM1 = 0.0f;
                    PWM2 = 1.0f;  
                }    
            }
        
            else
            {
                
                if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止時
                {
                    SR = ABLE;
                    PHASE = (float)CCW;
                    PWM1 = 0.3f;
                    PWM2 = 1.0f;  
                }
                
                if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
                {
                    MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
                }
                
                else //誤差の範囲内ならモータのPWMを停止させる
                {
                    MotorDrive(OMEGA_FEEDBACK, 0.0);
                    PWM1 = 0.0f;
                    PWM2 = 1.0f;  
                }
            }
            
            break;
        
        case OMEGA_FEEDBACK:
            
            /*
            if(direction != old_direction)  //Prevent sudden turn
            {        
                thetaPID.setProcessValue(0.0f);   
            
                if(thetaPID.compute() < 0.05f)
                {
                    old_direction = direction;
                }
            }
            */
            
            
            DBG("OMEGA_FEEDBACK  ");
            
            if(target >= 0.0f)
            {  
                thetaPID.setSetPoint(abs(target)); 
                thetaPID.setProcessValue(abs(Motor.omega));
                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CW;
                PWM1 = thetaPID.compute();
                PWM2 = 1.0f;   
                
                //DBG("%f \r\n", (float)direction);   
            }
                
            else
            {
                thetaPID.setSetPoint(abs(target)); 
                thetaPID.setProcessValue(abs(Motor.omega));
                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CCW;
                PWM1 = thetaPID.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;
}