File content as of revision 2:50e50ee8e08a:

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

#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 1.3f
#define Ki2 0.0f
#define Kd2 0.002f

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

#define THETA_FEEDBACK 0
#define OMEGA_FEEDBACK 1
#define CURRENT_FEEDBACK 2

//#define hostID 0

#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 8000 //[Hz]

#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]


/*
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 motor(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
PID motor2(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();



void MotorDrive(int mode, double target);

//void CurrentFunction();

InterruptIn userButton(PB_5);

Ticker Encoder;

Timer ENCODER_timer;

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



int main()
{
    // MPU setup
    pc.printf("MotorDriver_ver4.1 start! \r\n");
    pc.printf("/** MPU INFO **/ \r\n");
    pc.printf("Target PlatForm: STM32F303K8T6 \r\n");
    pc.printf("System Frequency: %d Hz \r\n", SystemCoreClock);
    pc.printf("Program File: %s \r\n", __FILE__);
    pc.printf("Compile Date: %s \r\n", __DATE__);
    pc.printf("Compile Time: %s \r\n", __TIME__);
    
    //Unique ID starting addresses for most of STM32 microcontrollers.
    unsigned long *id = (unsigned long *)0x1FFFF7AC; 
    
    pc.printf("%d, %d, %d \r\n", id[0], id[1], id[2]);
        
    
    myled = 1;
    wait(1.0);
    myled = 0;
    
    //load Flash Data
    int deviceID = 10;
    
    // CAN setup 
    can.frequency(1000000); //ROS default bps:100kbps -> 500kbps
    //can.filter(deviceID, 0b11111111, CANAny, 0);
    can.reset(); //delete receive buffer
    //can.monitor();
    //can.mode();
    
    // 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 omega setup
    motor.setInputLimits(0.0f, 3.68f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
    motor.setOutputLimits(0.0f, 1.0f); 
    motor.setBias(0.0);
    motor.setMode(AUTO_MODE);    
    //motor.setSetPoint(1.0f);  //PID target value
    
    // PID position setup
    motor2.setInputLimits(-360.0f, 360.0f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
    motor2.setOutputLimits(-3.68f, 3.68f); 
    motor2.setBias(0.0);
    motor2.setMode(AUTO_MODE);
    //motor.setSetPoint(1.0f);  //PID target value
    
    // 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;
    pc.printf("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);
    
    Motor.target = 250.0f; //[deg]
    //Motor.target = 1.0f;  //[rps]
    
    while(1)
    { 
        
        
        //Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
        
        //pc.printf("motor2.compute : %6.3f motor.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", motor2.compute(), motor.compute(), Motor.theta, Motor.omega);
        pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
        
        MotorDrive(THETA_FEEDBACK, Motor.target);
        //MotorDrive(OMEGA_FEEDBACK, Motor.target);
        
        //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());  
        //CurrentFunction();
    }
}

void SerialFunction()
{
    if(pc.readable() == 1)
    {
        char data = pc.getc();
        pc.printf("get data:%c \r\n", data);
        
        
        switch (data)
        {
            case 'q': 
                
                //motor.setSetPoint(1.5f);  //PID target value
                
                Motor.target += 0.5f;
                pc.printf("get 'q' \r\n");    
                
                break;
            
            case 'a':
        
                //motor.setSetPoint(0.5f);  //PID target value
                
                Motor.target -= 0.5f;
                pc.printf("get 'a' \r\n");
                
                break;
            
            case 'p': 
                
                //motor.setSetPoint(1.5f);  //PID target value
                
                Motor.target += 30.0f;
                pc.printf("get 'p' \r\n");    
                
                break;
            
            case 'l':
        
                //motor.setSetPoint(0.5f);  //PID target value
                
                Motor.target -= 30.0f;
                pc.printf("get 'l' \r\n");
                
                break;
            
                
            case 'r':
        
                
                
                pc.printf("\r\nSystem Reset! \r\n");
                NVIC_SystemReset();
                
                break;
            
            default:
                
                pc.printf("default \r\n");
                
                break;
        }
    }
}

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

void CANread()
{
    __disable_irq(); //All Interrupt disabled
    Encoder.detach();
    
    
    if(can.read(msg) != 0)
    {
        pc.printf("CAN recieve \r\n");    
        myled = !myled;
            
        for(int i = 0 ; i < 8 ; i++)
        {
            MDCAN.recvDATA[i] = msg.data[i];
            pc.printf("[%c] ", MDCAN.recvDATA[i]);
        }
    
        pc.printf("\r\n");
    
        switch(MDCAN.recvDATA[1]) //check PID
        {
            case 0: //MD data send
            
                MDCAN.sendDATA[0] = 1; //PID
                MDCAN.sendDATA[1] = 0; //dataA
                MDCAN.sendDATA[2] = 0; //dataB
                MDCAN.sendDATA[3] = 0; //dataC
                MDCAN.sendDATA[4] = 0; //dataD
                MDCAN.sendDATA[5] = 0; //dataE
                MDCAN.sendDATA[6] = 0; //dataF
                MDCAN.sendDATA[7] = 15;//dataG

                //CANsend(hostID);
            
                //pc.printf("reply to host(PID=%d)\r\n",PID);
            
                break;
            
            case 9: //MD drive
            
                ///MD4.Drive();
            
                break;
        
            default:
                
                //CAN.recvDATA[1] = 0b00000000;
            
                break;
        }
    }
            
    motor.setSetPoint((double)MDCAN.recvDATA[1]);     //motor target value velocity
    __enable_irq();
    Encoder.attach(Velocity, 0.01); //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
    
    //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
}

void CANsend(int id)
{
    __disable_irq();
    Encoder.detach();
        
    if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended))) 
    {
        pc.printf("CAN send error! \r\n"); 
    }

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

/*
void CANtimeout()
{
    if(MDCAN.recvDATA[2] == 0)
    {
        pc.printf("CAN CONNECTION TIMEOUT \r\n");
        //MotorDrive();
    }
}

void CurrentFunction()
{
    static int noneDrive_count;
    
    
    pc.printf("current(raw) : %f \r\n",Current.read());    
    
    Motor.current = (Current.read() - current_bias);
    
    if(noneDrive_count >= 2)
    {
        CANsend();  //Power capacity lack. check power supply output current
    }
    
    //if(MAX_LIMIT_CURRENT)
    
}
*/

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

    //static int old_direction;
    static double old_target;

    switch (mode)
    {
        case THETA_FEEDBACK:
        
            //pc.printf("THETA_FEEDBACK!! \r\n");
        
            motor2.setSetPoint(target); 
            motor2.setProcessValue(Motor.theta);
            
            if(target >= 0)
            {
                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
                {
                    SR = ABLE;
                    PHASE = (float)CW;
                    PWM1 = 0.4f;
                    PWM2 = 1.0f;  
                    //MotorDrive(CURRENT_FEEDBACK, CCW, 1.2f);
                }
                  
                else
                {
                    MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                }
                
                //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
            }
        
            else
            {
                
                if(abs(Motor.omega) < 0.01f) //モータの回転が停止時
                {
                    SR = ABLE;
                    PHASE = (float)CCW;
                    PWM1 = 0.4f;
                    PWM2 = 1.0f;  
                    //MotorDrive(CURRENT_FEEDBACK, CW, 1.2f);
                }
                
                else
                {
                    MotorDrive(OMEGA_FEEDBACK, motor2.compute());
                }
                
                //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
            }
            
            /*
            SR = ABLE;
            PHASE = (float)direction;
            PWM1 = motor.compute();
            PWM2 = 1.0f;   
            */        
            break;
        
        case OMEGA_FEEDBACK:
            
            /*
            if(direction != old_direction)  //Prevent sudden turn
            {        
                motor.setProcessValue(0.0f);   
            
                if(motor.compute() < 0.05f)
                {
                    old_direction = direction;
                }
            }
            */
            
            
            //pc.printf("OMEGA_FEEDBACK  ");
            
            if(target >= 0.0f)
            {  
                motor.setSetPoint(abs(target)); 
                motor.setProcessValue(abs(Motor.omega));
                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CW;
                PWM1 = motor.compute();
                PWM2 = 1.0f;   
                
                //pc.printf("%f \r\n", (float)direction);   
            }
                
            else
            {
                motor.setSetPoint(abs(target)); 
                motor.setProcessValue(abs(Motor.omega));
                    //ここにtargetの正負によって回転方向を帰るプログラムを書く
                SR = ABLE;
                PHASE = (float)CCW;
                PWM1 = motor.compute();
                PWM2 = 1.0f;       
            }                  
                    
            break;
        
        case CURRENT_FEEDBACK:
            
            pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
            
            break;
        
        default:
            
            pc.printf("Function:MotorDriver mode error! \r\n");
            
            break;
    }
    
    
    //motor.setProcessValue(omega);

    //old_direction = direction;
    old_target = target;
}


/*
void CAN2DATA()

void DATA2CAN()
*/