志鈴 謝 / Mbed 2 deprecated Motor_Control

LDSC motor control

Dependencies:   mbed

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?

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