LDSC motor control

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
jkjk010695
Date:
Mon Apr 27 08:33:33 2020 +0000
Commit message:
LDSC motor control

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 684b50f013f7 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 27 08:33:33 2020 +0000
@@ -0,0 +1,312 @@
+#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;
+                ///////////////////////
+            }
+        } 
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 684b50f013f7 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 27 08:33:33 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file