dikuei yen / Mbed 2 deprecated ROS_robot

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.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }