File content as of revision 0:684b50f013f7:

#include "mbed.h"
#include <math.h>
#include <stdlib.h>

#define pi 3.14159265358979323846f
#define maximum_volt 12.0f
#define minimum_volt 0.45f // Need to test for different loads.

#define INPUT_VOLTAGE       12.5f 
#define PWM_FREQUENCY       50.0f   // 20kHz
#define PWM_STOP            0.5f    //the pwm dutycycle value is from 0~1 and 0.5 can let motor stop

#define FRICTION_VOLTAGE    0.45f
#define HALL_RESOLUTION     64.0f
#define GEAR_RATIO          56.0f      
#define VELOCITY_CMD        8.0f    // unit(voltage)



#define CONTROLLER          1     // 0 for transfer function 1 for control

Serial pc(USBTX,USBRX);
InterruptIn mybutton(USER_BUTTON);
Ticker main_function; //interrupt
PwmOut pwm1A(D7);
PwmOut pwm1B(D8);
PwmOut pwm2A(D11);
PwmOut pwm2B(A3);
DigitalOut led1(LED1);

//RX
int readcount = 0;
int RX_flag2 = 0;
char getData[6] = {0,0,0,0,0,0};
short data_received[2] = {0,0};

float dt = 0.01; // sec
float command = 0;
float velocityA = 0; //rpm
float velocityB = 0;
float positionA = 0;
float positionB = 0;
short EncoderPositionA;
short EncoderPositionB;
float last_voltA = 0;
float last_voltB = 0;
float errorA = 0;
float error_drA = 0;
float errorB = 0;
float error_drB = 0;
bool button_state = false;
float dutycycle = PWM_STOP;
float VELOCITY_SPEED_A = 0.0;
float VELOCITY_SPEED_B = 0.0;
int pub_count = 0;

void step_command();
void position_control();
float PD(float e, float last_e, float last_u, float P, float D);
float PDF(float e, float last_e, float last_u, float P, float D, float F);
void ReadVelocity();
void ReadPosition(float *positionA, float *positionB);
void motor_drive(float voltA, float voltB);
void InitMotor(float period_in_ms);
void InitEncoder(void);
void control_speed();


void RX_ITR();
void init_UART();


int main() {
    pc.baud(115200);
    
    InitEncoder();      //don't care
    InitMotor(PWM_FREQUENCY); // Set pwm period to 1ms.
    init_UART();
    mybutton.fall(&step_command);

    main_function.attach_us(&position_control, dt*1000000);
    
    while(1){}
}

void InitMotor(float period_in_us){
    pwm1A.period_us(period_in_us);
    pwm1B.period_us(period_in_us);
    pwm1A.write(PWM_STOP);
    pwm1B.write(PWM_STOP);
    TIM1->CCER |= 0x0044;
//    bool cc1ne_bit = (TIM1->CCER >> 2) & 0x0001;
//    pc.printf("CC1NE bit : %d\r",cc1ne_bit);
}


void step_command(){
    led1 = !led1;
    button_state = !button_state;
    
//    // Do what you want motor to do.
//    if(command == 0.0f){
////        command = 8.0f; // volts  used to open loop control
//        command = 90.0f; // deg     used to posiyion control
//    }
//    else{
////        motor_drive(0.0f,0.0f);
////        positionA = 0.0f;
//        command = 0.0f;
//    }
}

void position_control(){
#if CONTROLLER == 0
    if(button_state == true){
        ReadVelocity();
        command = VELOCITY_CMD;
        //printf("%.3f, %.3f\r\n",command, velocityA);
        motor_drive(command,0);
    }else{
        dutycycle = PWM_STOP;
        pwm1A.write(dutycycle);
        pwm1B.write(dutycycle);
        TIM1->CCER |= 0x0044;
        command = 0;
        //printf("%.3f, %.3f\r\n",command, velocityA);  // velocityA or velocityB
    }

    
#endif

#if CONTROLLER == 1
    if(button_state == true){
        pub_count++;
        ReadVelocity();
        control_speed();
        if (pub_count >= 10){
            printf("%.3f,%.3f\r\n",velocityA, velocityB);  // velocityA or velocityB
            //printf("CMD %.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
            pub_count = 0;
        }
    }else{
        dutycycle = PWM_STOP;
        pwm1A.write(dutycycle);
        pwm1B.write(dutycycle);
        TIM1->CCER |= 0x0044;
        command = 0;
        //printf("%.3f, %.3f\r\n",command, velocityA);  // velocityA or velocityB
    }
#endif
}


void ReadVelocity(){
    /*
    The velocity is calculated by follow :
    velocity = EncoderPosition /Encoder CPR (Counts per round) /gear ratio *2pi /dt
    unit : rad/sec
    */
    
    EncoderPositionA = TIM2->CNT ;
    EncoderPositionB = TIM3->CNT ;
    TIM2->CNT = 0;
    TIM3->CNT = 0;
    // rad/s
    velocityA = EncoderPositionA /HALL_RESOLUTION /GEAR_RATIO /dt *60;
    velocityB = EncoderPositionB /HALL_RESOLUTION /GEAR_RATIO /dt *60;
    // RPM
//    *velocityA = EncoderPositionA /64.0 /90.0 /dt *60.0;
//    *velocityB = EncoderPositionB /64.0 /90.0 /dt *60.0;
}


void motor_drive(float voltA, float voltB){
    // Input voltage is in range -12.0V ~ 12.0V

    if(abs(voltA) <= minimum_volt){
        if(voltA > 0){ voltA = minimum_volt; }
        else{ voltA = -minimum_volt; }
    }
    
    // Convet volt to pwm
    
    float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE;
    float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE;
    pwm1A.write(dutycycleA);
    pwm1B.write(dutycycleB);
    TIM1->CCER |= 0x0044;
}

void control_speed(){
    float voltA;
    float voltB;
    errorA = (VELOCITY_SPEED_A - velocityA);
    voltA = last_voltA+0.4f*errorA-0.35f*error_drA;
    error_drA = errorA;
    last_voltA = voltA;
    if(abs(voltA) <= minimum_volt){
        if(voltA > 0){ voltA = minimum_volt; }
        else{ voltA = -minimum_volt; }
    }
    if(abs(voltA) > INPUT_VOLTAGE){
        if(voltA > 0){voltA = INPUT_VOLTAGE;}
        else{voltA = -INPUT_VOLTAGE;}    
    }
    
    errorB = (VELOCITY_SPEED_B - velocityB);
    voltB = last_voltB+0.4f*errorB-0.35f*error_drB;
    error_drB = errorB;
    last_voltB = voltB;
    if(abs(voltB) <= minimum_volt){
        if(voltB > 0){ voltB = minimum_volt; }
        else{ voltB = -minimum_volt; }
    }
    if(abs(voltB) > INPUT_VOLTAGE){
        if(voltB > 0){voltB = INPUT_VOLTAGE;}
        else{voltB = -INPUT_VOLTAGE;}    
    }
    
    float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE;
    float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE;
    pwm1A.write(dutycycleA);
    pwm1B.write(dutycycleB);
    TIM1->CCER |= 0x0044;
    //printf("%.3f, %.3f, %.3f\r\n",error1, last_error, voltA);
}




void InitEncoder(void) {
    // Hardware Quadrature Encoder AB for Nucleo F446RE
    // Output on debug port to host PC @ 9600 baud

    /* Connections
    PA_0 = Encoder1 A
    PA_1 = Encoder1 B
    PB_5 = Encoder2 A
    PB_4 = Encoder2 B
    */
    
    // configure GPIO PA0, PA1, PB5 & PB4 as inputs for Encoder
    RCC->AHB1ENR |= 0x00000003;  // Enable clock for GPIOA & GPIOB
 
    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           // PA0 & PA1 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_0 | GPIO_PUPDR_PUPDR1_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
    GPIOA->AFR[0]  |= 0x00000011 ;                                          // AF1 for PA0 & PA1                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
   
 
    GPIOB->MODER   |= GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 ;           // PB5 & PB4 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
    GPIOB->PUPDR   |= GPIO_PUPDR_PUPDR4_0 | GPIO_PUPDR_PUPDR5_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
    GPIOB->AFR[0]  |= 0x00220000 ;                                          // AF2 for PB5 & PB4                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
    GPIOB->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
   
    // configure TIM2 & TIM3 as Encoder input
    RCC->APB1ENR |= 0x00000003;  // Enable clock for TIM2 & TIM3

    TIM2->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1  
    TIM2->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
    TIM2->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
    TIM2->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
    TIM2->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
    TIM2->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
    TIM2->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
  
    TIM2->CNT = 0x0000;  //reset the counter before we use it
 
    TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1    
    TIM3->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
    TIM3->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
    TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
    TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
    TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
    TIM3->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
  
    TIM3->CNT = 0x0000;  //reset the counter before we use it
}

void init_UART()
{
    pc.baud(9600);  // baud rate設為9600
    pc.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
}


void RX_ITR()
{
    while(pc.readable()) {
        char uart_read;
        uart_read = pc.getc();
        if(uart_read == 115) {
            RX_flag2 = 1;
            readcount = 0;
            getData[5] = 0;
        } 
        if(RX_flag2 == 1) {
            getData[readcount] = uart_read;
            readcount++;
            if(readcount >= 6 & getData[5] == 101) {
                readcount = 0;
                RX_flag2 = 0;               
                ///code for decoding///
                data_received[0] = (getData[2] << 8) | getData[1];
                data_received[1] = (getData[4] << 8) | getData[3];    
                VELOCITY_SPEED_A = data_received[0]/100;
                VELOCITY_SPEED_B = data_received[1]/100;
                ///////////////////////
            }
        } 
    }
}