志鈴 謝
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