![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LDSC motor control
main.cpp
- Committer:
- jkjk010695
- Date:
- 2020-04-27
- Revision:
- 0:684b50f013f7
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; /////////////////////// } } } }