![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LDSC motor control
Diff: main.cpp
- Revision:
- 0:684b50f013f7
--- /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