志鈴 謝
/
Motor_Control
LDSC motor control
main.cpp@0:684b50f013f7, 2020-04-27 (annotated)
- Committer:
- jkjk010695
- Date:
- Mon Apr 27 08:33:33 2020 +0000
- Revision:
- 0:684b50f013f7
LDSC motor control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jkjk010695 | 0:684b50f013f7 | 1 | #include "mbed.h" |
jkjk010695 | 0:684b50f013f7 | 2 | #include <math.h> |
jkjk010695 | 0:684b50f013f7 | 3 | #include <stdlib.h> |
jkjk010695 | 0:684b50f013f7 | 4 | |
jkjk010695 | 0:684b50f013f7 | 5 | #define pi 3.14159265358979323846f |
jkjk010695 | 0:684b50f013f7 | 6 | #define maximum_volt 12.0f |
jkjk010695 | 0:684b50f013f7 | 7 | #define minimum_volt 0.45f // Need to test for different loads. |
jkjk010695 | 0:684b50f013f7 | 8 | |
jkjk010695 | 0:684b50f013f7 | 9 | #define INPUT_VOLTAGE 12.5f |
jkjk010695 | 0:684b50f013f7 | 10 | #define PWM_FREQUENCY 50.0f // 20kHz |
jkjk010695 | 0:684b50f013f7 | 11 | #define PWM_STOP 0.5f //the pwm dutycycle value is from 0~1 and 0.5 can let motor stop |
jkjk010695 | 0:684b50f013f7 | 12 | |
jkjk010695 | 0:684b50f013f7 | 13 | #define FRICTION_VOLTAGE 0.45f |
jkjk010695 | 0:684b50f013f7 | 14 | #define HALL_RESOLUTION 64.0f |
jkjk010695 | 0:684b50f013f7 | 15 | #define GEAR_RATIO 56.0f |
jkjk010695 | 0:684b50f013f7 | 16 | #define VELOCITY_CMD 8.0f // unit(voltage) |
jkjk010695 | 0:684b50f013f7 | 17 | |
jkjk010695 | 0:684b50f013f7 | 18 | |
jkjk010695 | 0:684b50f013f7 | 19 | |
jkjk010695 | 0:684b50f013f7 | 20 | #define CONTROLLER 1 // 0 for transfer function 1 for control |
jkjk010695 | 0:684b50f013f7 | 21 | |
jkjk010695 | 0:684b50f013f7 | 22 | Serial pc(USBTX,USBRX); |
jkjk010695 | 0:684b50f013f7 | 23 | InterruptIn mybutton(USER_BUTTON); |
jkjk010695 | 0:684b50f013f7 | 24 | Ticker main_function; //interrupt |
jkjk010695 | 0:684b50f013f7 | 25 | PwmOut pwm1A(D7); |
jkjk010695 | 0:684b50f013f7 | 26 | PwmOut pwm1B(D8); |
jkjk010695 | 0:684b50f013f7 | 27 | PwmOut pwm2A(D11); |
jkjk010695 | 0:684b50f013f7 | 28 | PwmOut pwm2B(A3); |
jkjk010695 | 0:684b50f013f7 | 29 | DigitalOut led1(LED1); |
jkjk010695 | 0:684b50f013f7 | 30 | |
jkjk010695 | 0:684b50f013f7 | 31 | //RX |
jkjk010695 | 0:684b50f013f7 | 32 | int readcount = 0; |
jkjk010695 | 0:684b50f013f7 | 33 | int RX_flag2 = 0; |
jkjk010695 | 0:684b50f013f7 | 34 | char getData[6] = {0,0,0,0,0,0}; |
jkjk010695 | 0:684b50f013f7 | 35 | short data_received[2] = {0,0}; |
jkjk010695 | 0:684b50f013f7 | 36 | |
jkjk010695 | 0:684b50f013f7 | 37 | float dt = 0.01; // sec |
jkjk010695 | 0:684b50f013f7 | 38 | float command = 0; |
jkjk010695 | 0:684b50f013f7 | 39 | float velocityA = 0; //rpm |
jkjk010695 | 0:684b50f013f7 | 40 | float velocityB = 0; |
jkjk010695 | 0:684b50f013f7 | 41 | float positionA = 0; |
jkjk010695 | 0:684b50f013f7 | 42 | float positionB = 0; |
jkjk010695 | 0:684b50f013f7 | 43 | short EncoderPositionA; |
jkjk010695 | 0:684b50f013f7 | 44 | short EncoderPositionB; |
jkjk010695 | 0:684b50f013f7 | 45 | float last_voltA = 0; |
jkjk010695 | 0:684b50f013f7 | 46 | float last_voltB = 0; |
jkjk010695 | 0:684b50f013f7 | 47 | float errorA = 0; |
jkjk010695 | 0:684b50f013f7 | 48 | float error_drA = 0; |
jkjk010695 | 0:684b50f013f7 | 49 | float errorB = 0; |
jkjk010695 | 0:684b50f013f7 | 50 | float error_drB = 0; |
jkjk010695 | 0:684b50f013f7 | 51 | bool button_state = false; |
jkjk010695 | 0:684b50f013f7 | 52 | float dutycycle = PWM_STOP; |
jkjk010695 | 0:684b50f013f7 | 53 | float VELOCITY_SPEED_A = 0.0; |
jkjk010695 | 0:684b50f013f7 | 54 | float VELOCITY_SPEED_B = 0.0; |
jkjk010695 | 0:684b50f013f7 | 55 | int pub_count = 0; |
jkjk010695 | 0:684b50f013f7 | 56 | |
jkjk010695 | 0:684b50f013f7 | 57 | void step_command(); |
jkjk010695 | 0:684b50f013f7 | 58 | void position_control(); |
jkjk010695 | 0:684b50f013f7 | 59 | float PD(float e, float last_e, float last_u, float P, float D); |
jkjk010695 | 0:684b50f013f7 | 60 | float PDF(float e, float last_e, float last_u, float P, float D, float F); |
jkjk010695 | 0:684b50f013f7 | 61 | void ReadVelocity(); |
jkjk010695 | 0:684b50f013f7 | 62 | void ReadPosition(float *positionA, float *positionB); |
jkjk010695 | 0:684b50f013f7 | 63 | void motor_drive(float voltA, float voltB); |
jkjk010695 | 0:684b50f013f7 | 64 | void InitMotor(float period_in_ms); |
jkjk010695 | 0:684b50f013f7 | 65 | void InitEncoder(void); |
jkjk010695 | 0:684b50f013f7 | 66 | void control_speed(); |
jkjk010695 | 0:684b50f013f7 | 67 | |
jkjk010695 | 0:684b50f013f7 | 68 | |
jkjk010695 | 0:684b50f013f7 | 69 | void RX_ITR(); |
jkjk010695 | 0:684b50f013f7 | 70 | void init_UART(); |
jkjk010695 | 0:684b50f013f7 | 71 | |
jkjk010695 | 0:684b50f013f7 | 72 | |
jkjk010695 | 0:684b50f013f7 | 73 | int main() { |
jkjk010695 | 0:684b50f013f7 | 74 | pc.baud(115200); |
jkjk010695 | 0:684b50f013f7 | 75 | |
jkjk010695 | 0:684b50f013f7 | 76 | InitEncoder(); //don't care |
jkjk010695 | 0:684b50f013f7 | 77 | InitMotor(PWM_FREQUENCY); // Set pwm period to 1ms. |
jkjk010695 | 0:684b50f013f7 | 78 | init_UART(); |
jkjk010695 | 0:684b50f013f7 | 79 | mybutton.fall(&step_command); |
jkjk010695 | 0:684b50f013f7 | 80 | |
jkjk010695 | 0:684b50f013f7 | 81 | main_function.attach_us(&position_control, dt*1000000); |
jkjk010695 | 0:684b50f013f7 | 82 | |
jkjk010695 | 0:684b50f013f7 | 83 | while(1){} |
jkjk010695 | 0:684b50f013f7 | 84 | } |
jkjk010695 | 0:684b50f013f7 | 85 | |
jkjk010695 | 0:684b50f013f7 | 86 | void InitMotor(float period_in_us){ |
jkjk010695 | 0:684b50f013f7 | 87 | pwm1A.period_us(period_in_us); |
jkjk010695 | 0:684b50f013f7 | 88 | pwm1B.period_us(period_in_us); |
jkjk010695 | 0:684b50f013f7 | 89 | pwm1A.write(PWM_STOP); |
jkjk010695 | 0:684b50f013f7 | 90 | pwm1B.write(PWM_STOP); |
jkjk010695 | 0:684b50f013f7 | 91 | TIM1->CCER |= 0x0044; |
jkjk010695 | 0:684b50f013f7 | 92 | // bool cc1ne_bit = (TIM1->CCER >> 2) & 0x0001; |
jkjk010695 | 0:684b50f013f7 | 93 | // pc.printf("CC1NE bit : %d\r",cc1ne_bit); |
jkjk010695 | 0:684b50f013f7 | 94 | } |
jkjk010695 | 0:684b50f013f7 | 95 | |
jkjk010695 | 0:684b50f013f7 | 96 | |
jkjk010695 | 0:684b50f013f7 | 97 | void step_command(){ |
jkjk010695 | 0:684b50f013f7 | 98 | led1 = !led1; |
jkjk010695 | 0:684b50f013f7 | 99 | button_state = !button_state; |
jkjk010695 | 0:684b50f013f7 | 100 | |
jkjk010695 | 0:684b50f013f7 | 101 | // // Do what you want motor to do. |
jkjk010695 | 0:684b50f013f7 | 102 | // if(command == 0.0f){ |
jkjk010695 | 0:684b50f013f7 | 103 | //// command = 8.0f; // volts used to open loop control |
jkjk010695 | 0:684b50f013f7 | 104 | // command = 90.0f; // deg used to posiyion control |
jkjk010695 | 0:684b50f013f7 | 105 | // } |
jkjk010695 | 0:684b50f013f7 | 106 | // else{ |
jkjk010695 | 0:684b50f013f7 | 107 | //// motor_drive(0.0f,0.0f); |
jkjk010695 | 0:684b50f013f7 | 108 | //// positionA = 0.0f; |
jkjk010695 | 0:684b50f013f7 | 109 | // command = 0.0f; |
jkjk010695 | 0:684b50f013f7 | 110 | // } |
jkjk010695 | 0:684b50f013f7 | 111 | } |
jkjk010695 | 0:684b50f013f7 | 112 | |
jkjk010695 | 0:684b50f013f7 | 113 | void position_control(){ |
jkjk010695 | 0:684b50f013f7 | 114 | #if CONTROLLER == 0 |
jkjk010695 | 0:684b50f013f7 | 115 | if(button_state == true){ |
jkjk010695 | 0:684b50f013f7 | 116 | ReadVelocity(); |
jkjk010695 | 0:684b50f013f7 | 117 | command = VELOCITY_CMD; |
jkjk010695 | 0:684b50f013f7 | 118 | //printf("%.3f, %.3f\r\n",command, velocityA); |
jkjk010695 | 0:684b50f013f7 | 119 | motor_drive(command,0); |
jkjk010695 | 0:684b50f013f7 | 120 | }else{ |
jkjk010695 | 0:684b50f013f7 | 121 | dutycycle = PWM_STOP; |
jkjk010695 | 0:684b50f013f7 | 122 | pwm1A.write(dutycycle); |
jkjk010695 | 0:684b50f013f7 | 123 | pwm1B.write(dutycycle); |
jkjk010695 | 0:684b50f013f7 | 124 | TIM1->CCER |= 0x0044; |
jkjk010695 | 0:684b50f013f7 | 125 | command = 0; |
jkjk010695 | 0:684b50f013f7 | 126 | //printf("%.3f, %.3f\r\n",command, velocityA); // velocityA or velocityB |
jkjk010695 | 0:684b50f013f7 | 127 | } |
jkjk010695 | 0:684b50f013f7 | 128 | |
jkjk010695 | 0:684b50f013f7 | 129 | |
jkjk010695 | 0:684b50f013f7 | 130 | #endif |
jkjk010695 | 0:684b50f013f7 | 131 | |
jkjk010695 | 0:684b50f013f7 | 132 | #if CONTROLLER == 1 |
jkjk010695 | 0:684b50f013f7 | 133 | if(button_state == true){ |
jkjk010695 | 0:684b50f013f7 | 134 | pub_count++; |
jkjk010695 | 0:684b50f013f7 | 135 | ReadVelocity(); |
jkjk010695 | 0:684b50f013f7 | 136 | control_speed(); |
jkjk010695 | 0:684b50f013f7 | 137 | if (pub_count >= 10){ |
jkjk010695 | 0:684b50f013f7 | 138 | printf("%.3f,%.3f\r\n",velocityA, velocityB); // velocityA or velocityB |
jkjk010695 | 0:684b50f013f7 | 139 | //printf("CMD %.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B); |
jkjk010695 | 0:684b50f013f7 | 140 | pub_count = 0; |
jkjk010695 | 0:684b50f013f7 | 141 | } |
jkjk010695 | 0:684b50f013f7 | 142 | }else{ |
jkjk010695 | 0:684b50f013f7 | 143 | dutycycle = PWM_STOP; |
jkjk010695 | 0:684b50f013f7 | 144 | pwm1A.write(dutycycle); |
jkjk010695 | 0:684b50f013f7 | 145 | pwm1B.write(dutycycle); |
jkjk010695 | 0:684b50f013f7 | 146 | TIM1->CCER |= 0x0044; |
jkjk010695 | 0:684b50f013f7 | 147 | command = 0; |
jkjk010695 | 0:684b50f013f7 | 148 | //printf("%.3f, %.3f\r\n",command, velocityA); // velocityA or velocityB |
jkjk010695 | 0:684b50f013f7 | 149 | } |
jkjk010695 | 0:684b50f013f7 | 150 | #endif |
jkjk010695 | 0:684b50f013f7 | 151 | } |
jkjk010695 | 0:684b50f013f7 | 152 | |
jkjk010695 | 0:684b50f013f7 | 153 | |
jkjk010695 | 0:684b50f013f7 | 154 | void ReadVelocity(){ |
jkjk010695 | 0:684b50f013f7 | 155 | /* |
jkjk010695 | 0:684b50f013f7 | 156 | The velocity is calculated by follow : |
jkjk010695 | 0:684b50f013f7 | 157 | velocity = EncoderPosition /Encoder CPR (Counts per round) /gear ratio *2pi /dt |
jkjk010695 | 0:684b50f013f7 | 158 | unit : rad/sec |
jkjk010695 | 0:684b50f013f7 | 159 | */ |
jkjk010695 | 0:684b50f013f7 | 160 | |
jkjk010695 | 0:684b50f013f7 | 161 | EncoderPositionA = TIM2->CNT ; |
jkjk010695 | 0:684b50f013f7 | 162 | EncoderPositionB = TIM3->CNT ; |
jkjk010695 | 0:684b50f013f7 | 163 | TIM2->CNT = 0; |
jkjk010695 | 0:684b50f013f7 | 164 | TIM3->CNT = 0; |
jkjk010695 | 0:684b50f013f7 | 165 | // rad/s |
jkjk010695 | 0:684b50f013f7 | 166 | velocityA = EncoderPositionA /HALL_RESOLUTION /GEAR_RATIO /dt *60; |
jkjk010695 | 0:684b50f013f7 | 167 | velocityB = EncoderPositionB /HALL_RESOLUTION /GEAR_RATIO /dt *60; |
jkjk010695 | 0:684b50f013f7 | 168 | // RPM |
jkjk010695 | 0:684b50f013f7 | 169 | // *velocityA = EncoderPositionA /64.0 /90.0 /dt *60.0; |
jkjk010695 | 0:684b50f013f7 | 170 | // *velocityB = EncoderPositionB /64.0 /90.0 /dt *60.0; |
jkjk010695 | 0:684b50f013f7 | 171 | } |
jkjk010695 | 0:684b50f013f7 | 172 | |
jkjk010695 | 0:684b50f013f7 | 173 | |
jkjk010695 | 0:684b50f013f7 | 174 | void motor_drive(float voltA, float voltB){ |
jkjk010695 | 0:684b50f013f7 | 175 | // Input voltage is in range -12.0V ~ 12.0V |
jkjk010695 | 0:684b50f013f7 | 176 | |
jkjk010695 | 0:684b50f013f7 | 177 | if(abs(voltA) <= minimum_volt){ |
jkjk010695 | 0:684b50f013f7 | 178 | if(voltA > 0){ voltA = minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 179 | else{ voltA = -minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 180 | } |
jkjk010695 | 0:684b50f013f7 | 181 | |
jkjk010695 | 0:684b50f013f7 | 182 | // Convet volt to pwm |
jkjk010695 | 0:684b50f013f7 | 183 | |
jkjk010695 | 0:684b50f013f7 | 184 | float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE; |
jkjk010695 | 0:684b50f013f7 | 185 | float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE; |
jkjk010695 | 0:684b50f013f7 | 186 | pwm1A.write(dutycycleA); |
jkjk010695 | 0:684b50f013f7 | 187 | pwm1B.write(dutycycleB); |
jkjk010695 | 0:684b50f013f7 | 188 | TIM1->CCER |= 0x0044; |
jkjk010695 | 0:684b50f013f7 | 189 | } |
jkjk010695 | 0:684b50f013f7 | 190 | |
jkjk010695 | 0:684b50f013f7 | 191 | void control_speed(){ |
jkjk010695 | 0:684b50f013f7 | 192 | float voltA; |
jkjk010695 | 0:684b50f013f7 | 193 | float voltB; |
jkjk010695 | 0:684b50f013f7 | 194 | errorA = (VELOCITY_SPEED_A - velocityA); |
jkjk010695 | 0:684b50f013f7 | 195 | voltA = last_voltA+0.4f*errorA-0.35f*error_drA; |
jkjk010695 | 0:684b50f013f7 | 196 | error_drA = errorA; |
jkjk010695 | 0:684b50f013f7 | 197 | last_voltA = voltA; |
jkjk010695 | 0:684b50f013f7 | 198 | if(abs(voltA) <= minimum_volt){ |
jkjk010695 | 0:684b50f013f7 | 199 | if(voltA > 0){ voltA = minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 200 | else{ voltA = -minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 201 | } |
jkjk010695 | 0:684b50f013f7 | 202 | if(abs(voltA) > INPUT_VOLTAGE){ |
jkjk010695 | 0:684b50f013f7 | 203 | if(voltA > 0){voltA = INPUT_VOLTAGE;} |
jkjk010695 | 0:684b50f013f7 | 204 | else{voltA = -INPUT_VOLTAGE;} |
jkjk010695 | 0:684b50f013f7 | 205 | } |
jkjk010695 | 0:684b50f013f7 | 206 | |
jkjk010695 | 0:684b50f013f7 | 207 | errorB = (VELOCITY_SPEED_B - velocityB); |
jkjk010695 | 0:684b50f013f7 | 208 | voltB = last_voltB+0.4f*errorB-0.35f*error_drB; |
jkjk010695 | 0:684b50f013f7 | 209 | error_drB = errorB; |
jkjk010695 | 0:684b50f013f7 | 210 | last_voltB = voltB; |
jkjk010695 | 0:684b50f013f7 | 211 | if(abs(voltB) <= minimum_volt){ |
jkjk010695 | 0:684b50f013f7 | 212 | if(voltB > 0){ voltB = minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 213 | else{ voltB = -minimum_volt; } |
jkjk010695 | 0:684b50f013f7 | 214 | } |
jkjk010695 | 0:684b50f013f7 | 215 | if(abs(voltB) > INPUT_VOLTAGE){ |
jkjk010695 | 0:684b50f013f7 | 216 | if(voltB > 0){voltB = INPUT_VOLTAGE;} |
jkjk010695 | 0:684b50f013f7 | 217 | else{voltB = -INPUT_VOLTAGE;} |
jkjk010695 | 0:684b50f013f7 | 218 | } |
jkjk010695 | 0:684b50f013f7 | 219 | |
jkjk010695 | 0:684b50f013f7 | 220 | float dutycycleA = 0.5f - 0.5f *voltA /INPUT_VOLTAGE; |
jkjk010695 | 0:684b50f013f7 | 221 | float dutycycleB = 0.5f - 0.5f *voltB /INPUT_VOLTAGE; |
jkjk010695 | 0:684b50f013f7 | 222 | pwm1A.write(dutycycleA); |
jkjk010695 | 0:684b50f013f7 | 223 | pwm1B.write(dutycycleB); |
jkjk010695 | 0:684b50f013f7 | 224 | TIM1->CCER |= 0x0044; |
jkjk010695 | 0:684b50f013f7 | 225 | //printf("%.3f, %.3f, %.3f\r\n",error1, last_error, voltA); |
jkjk010695 | 0:684b50f013f7 | 226 | } |
jkjk010695 | 0:684b50f013f7 | 227 | |
jkjk010695 | 0:684b50f013f7 | 228 | |
jkjk010695 | 0:684b50f013f7 | 229 | |
jkjk010695 | 0:684b50f013f7 | 230 | |
jkjk010695 | 0:684b50f013f7 | 231 | void InitEncoder(void) { |
jkjk010695 | 0:684b50f013f7 | 232 | // Hardware Quadrature Encoder AB for Nucleo F446RE |
jkjk010695 | 0:684b50f013f7 | 233 | // Output on debug port to host PC @ 9600 baud |
jkjk010695 | 0:684b50f013f7 | 234 | |
jkjk010695 | 0:684b50f013f7 | 235 | /* Connections |
jkjk010695 | 0:684b50f013f7 | 236 | PA_0 = Encoder1 A |
jkjk010695 | 0:684b50f013f7 | 237 | PA_1 = Encoder1 B |
jkjk010695 | 0:684b50f013f7 | 238 | PB_5 = Encoder2 A |
jkjk010695 | 0:684b50f013f7 | 239 | PB_4 = Encoder2 B |
jkjk010695 | 0:684b50f013f7 | 240 | */ |
jkjk010695 | 0:684b50f013f7 | 241 | |
jkjk010695 | 0:684b50f013f7 | 242 | // configure GPIO PA0, PA1, PB5 & PB4 as inputs for Encoder |
jkjk010695 | 0:684b50f013f7 | 243 | RCC->AHB1ENR |= 0x00000003; // Enable clock for GPIOA & GPIOB |
jkjk010695 | 0:684b50f013f7 | 244 | |
jkjk010695 | 0:684b50f013f7 | 245 | GPIOA->MODER |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ; // PA0 & PA1 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ |
jkjk010695 | 0:684b50f013f7 | 246 | GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_0 | GPIO_PUPDR_PUPDR1_0 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ |
jkjk010695 | 0:684b50f013f7 | 247 | GPIOA->AFR[0] |= 0x00000011 ; // AF1 for PA0 & PA1 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
jkjk010695 | 0:684b50f013f7 | 248 | GPIOA->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
jkjk010695 | 0:684b50f013f7 | 249 | |
jkjk010695 | 0:684b50f013f7 | 250 | |
jkjk010695 | 0:684b50f013f7 | 251 | GPIOB->MODER |= GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 ; // PB5 & PB4 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ |
jkjk010695 | 0:684b50f013f7 | 252 | GPIOB->PUPDR |= GPIO_PUPDR_PUPDR4_0 | GPIO_PUPDR_PUPDR5_0 ; // Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ |
jkjk010695 | 0:684b50f013f7 | 253 | GPIOB->AFR[0] |= 0x00220000 ; // AF2 for PB5 & PB4 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
jkjk010695 | 0:684b50f013f7 | 254 | GPIOB->AFR[1] |= 0x00000000 ; // /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ |
jkjk010695 | 0:684b50f013f7 | 255 | |
jkjk010695 | 0:684b50f013f7 | 256 | // configure TIM2 & TIM3 as Encoder input |
jkjk010695 | 0:684b50f013f7 | 257 | RCC->APB1ENR |= 0x00000003; // Enable clock for TIM2 & TIM3 |
jkjk010695 | 0:684b50f013f7 | 258 | |
jkjk010695 | 0:684b50f013f7 | 259 | TIM2->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1 |
jkjk010695 | 0:684b50f013f7 | 260 | TIM2->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register |
jkjk010695 | 0:684b50f013f7 | 261 | TIM2->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 |
jkjk010695 | 0:684b50f013f7 | 262 | TIM2->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 |
jkjk010695 | 0:684b50f013f7 | 263 | TIM2->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register |
jkjk010695 | 0:684b50f013f7 | 264 | TIM2->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler |
jkjk010695 | 0:684b50f013f7 | 265 | TIM2->ARR = 0xffffffff; // reload at 0xfffffff < TIM auto-reload register |
jkjk010695 | 0:684b50f013f7 | 266 | |
jkjk010695 | 0:684b50f013f7 | 267 | TIM2->CNT = 0x0000; //reset the counter before we use it |
jkjk010695 | 0:684b50f013f7 | 268 | |
jkjk010695 | 0:684b50f013f7 | 269 | TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1 |
jkjk010695 | 0:684b50f013f7 | 270 | TIM3->SMCR = 0x0003; // SMS='011' (Encoder mode 3) < TIM slave mode control register |
jkjk010695 | 0:684b50f013f7 | 271 | TIM3->CCMR1 = 0xF1F1; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1 |
jkjk010695 | 0:684b50f013f7 | 272 | TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 |
jkjk010695 | 0:684b50f013f7 | 273 | TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register |
jkjk010695 | 0:684b50f013f7 | 274 | TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler |
jkjk010695 | 0:684b50f013f7 | 275 | TIM3->ARR = 0xffffffff; // reload at 0xfffffff < TIM auto-reload register |
jkjk010695 | 0:684b50f013f7 | 276 | |
jkjk010695 | 0:684b50f013f7 | 277 | TIM3->CNT = 0x0000; //reset the counter before we use it |
jkjk010695 | 0:684b50f013f7 | 278 | } |
jkjk010695 | 0:684b50f013f7 | 279 | |
jkjk010695 | 0:684b50f013f7 | 280 | void init_UART() |
jkjk010695 | 0:684b50f013f7 | 281 | { |
jkjk010695 | 0:684b50f013f7 | 282 | pc.baud(9600); // baud rate設為9600 |
jkjk010695 | 0:684b50f013f7 | 283 | pc.attach(&RX_ITR, Serial::RxIrq); // Attach a function(RX_ITR) to call whenever a serial interrupt is generated. |
jkjk010695 | 0:684b50f013f7 | 284 | } |
jkjk010695 | 0:684b50f013f7 | 285 | |
jkjk010695 | 0:684b50f013f7 | 286 | |
jkjk010695 | 0:684b50f013f7 | 287 | void RX_ITR() |
jkjk010695 | 0:684b50f013f7 | 288 | { |
jkjk010695 | 0:684b50f013f7 | 289 | while(pc.readable()) { |
jkjk010695 | 0:684b50f013f7 | 290 | char uart_read; |
jkjk010695 | 0:684b50f013f7 | 291 | uart_read = pc.getc(); |
jkjk010695 | 0:684b50f013f7 | 292 | if(uart_read == 115) { |
jkjk010695 | 0:684b50f013f7 | 293 | RX_flag2 = 1; |
jkjk010695 | 0:684b50f013f7 | 294 | readcount = 0; |
jkjk010695 | 0:684b50f013f7 | 295 | getData[5] = 0; |
jkjk010695 | 0:684b50f013f7 | 296 | } |
jkjk010695 | 0:684b50f013f7 | 297 | if(RX_flag2 == 1) { |
jkjk010695 | 0:684b50f013f7 | 298 | getData[readcount] = uart_read; |
jkjk010695 | 0:684b50f013f7 | 299 | readcount++; |
jkjk010695 | 0:684b50f013f7 | 300 | if(readcount >= 6 & getData[5] == 101) { |
jkjk010695 | 0:684b50f013f7 | 301 | readcount = 0; |
jkjk010695 | 0:684b50f013f7 | 302 | RX_flag2 = 0; |
jkjk010695 | 0:684b50f013f7 | 303 | ///code for decoding/// |
jkjk010695 | 0:684b50f013f7 | 304 | data_received[0] = (getData[2] << 8) | getData[1]; |
jkjk010695 | 0:684b50f013f7 | 305 | data_received[1] = (getData[4] << 8) | getData[3]; |
jkjk010695 | 0:684b50f013f7 | 306 | VELOCITY_SPEED_A = data_received[0]/100; |
jkjk010695 | 0:684b50f013f7 | 307 | VELOCITY_SPEED_B = data_received[1]/100; |
jkjk010695 | 0:684b50f013f7 | 308 | /////////////////////// |
jkjk010695 | 0:684b50f013f7 | 309 | } |
jkjk010695 | 0:684b50f013f7 | 310 | } |
jkjk010695 | 0:684b50f013f7 | 311 | } |
jkjk010695 | 0:684b50f013f7 | 312 | } |