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