NTHU機器人學 Team3 / Mbed 2 deprecated Bluetooth

trytrykang

Dependencies:   mbed ros_lib_indigo

Fork of ROS_remote_car by NTHU機器人學 Team3

main.cpp@4:f828ddd4fed3, 2018-04-23 (annotated)

Committer:
farmookong
Date:
Mon Apr 23 17:49:36 2018 +0000
Revision:
4:f828ddd4fed3
Parent:
3:3899fc6e93c7
Child:
5:d81ff2f4145d
trytrykang2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
farmookong 0:5e356103dcc7 1 /* LAB DCMotor */
farmookong 0:5e356103dcc7 2 #include "mbed.h"
KCHuang 1:d24c3384bc59 3 #include <ros.h>
KCHuang 1:d24c3384bc59 4 #include <geometry_msgs/Vector3.h>
KCHuang 1:d24c3384bc59 5 #include <geometry_msgs/Twist.h>
KCHuang 1:d24c3384bc59 6
farmookong 0:5e356103dcc7 7 //****************************************************************************** Define
farmookong 0:5e356103dcc7 8 //The number will be compiled as type "double" in default
farmookong 0:5e356103dcc7 9 //Add a "f" after the number can make it compiled as type "float"
farmookong 0:5e356103dcc7 10 #define Ts 0.01f //period of timer1 (s)
farmookong 0:5e356103dcc7 11 #define Servo_Period 20
KCHuang 1:d24c3384bc59 12 #define pi 3.14159265f
farmookong 0:5e356103dcc7 13 //****************************************************************************** End of Define
farmookong 0:5e356103dcc7 14
farmookong 0:5e356103dcc7 15 //****************************************************************************** I/O
farmookong 0:5e356103dcc7 16 //PWM
farmookong 0:5e356103dcc7 17
farmookong 0:5e356103dcc7 18 //Dc motor
KCHuang 1:d24c3384bc59 19 PwmOut pwm1(D7);
farmookong 0:5e356103dcc7 20 PwmOut pwm1n(D11);
farmookong 0:5e356103dcc7 21 PwmOut pwm2(D8);
farmookong 0:5e356103dcc7 22 PwmOut pwm2n(A3);
farmookong 0:5e356103dcc7 23 PwmOut servo(A0);
farmookong 0:5e356103dcc7 24 //Motor1 sensor
KCHuang 1:d24c3384bc59 25 InterruptIn HallA_R(A1);
KCHuang 1:d24c3384bc59 26 InterruptIn HallB_R(A2);
farmookong 0:5e356103dcc7 27 //Motor2 sensor
KCHuang 1:d24c3384bc59 28 InterruptIn HallA_L(D13);
KCHuang 1:d24c3384bc59 29 InterruptIn HallB_L(D12);
farmookong 0:5e356103dcc7 30
farmookong 0:5e356103dcc7 31 //LED
farmookong 0:5e356103dcc7 32 DigitalOut led1(A4);
farmookong 0:5e356103dcc7 33 DigitalOut led2(A5);
farmookong 0:5e356103dcc7 34
farmookong 0:5e356103dcc7 35 //Timer Setting
farmookong 0:5e356103dcc7 36 Ticker timer;
KCHuang 1:d24c3384bc59 37
farmookong 0:5e356103dcc7 38 //****************************************************************************** End of I/O
farmookong 0:5e356103dcc7 39
farmookong 0:5e356103dcc7 40 //****************************************************************************** Functions
farmookong 0:5e356103dcc7 41 void init_timer(void);
farmookong 0:5e356103dcc7 42 void init_CN(void);
farmookong 0:5e356103dcc7 43 void init_PWM(void);
farmookong 0:5e356103dcc7 44 void timer_interrupt(void);
farmookong 0:5e356103dcc7 45 void CN_interrupt(void);
farmookong 0:5e356103dcc7 46 //****************************************************************************** End of Functions
farmookong 0:5e356103dcc7 47
farmookong 0:5e356103dcc7 48 //****************************************************************************** Variables
farmookong 0:5e356103dcc7 49 // Servo
farmookong 0:5e356103dcc7 50 float servo_duty = 0.03; // 0.025~0.113(-90~+90) 0.069->0 degree
KCHuang 1:d24c3384bc59 51 // motor right
KCHuang 1:d24c3384bc59 52 int8_t HallA_state_R = 0;
KCHuang 1:d24c3384bc59 53 int8_t HallB_state_R = 0;
KCHuang 1:d24c3384bc59 54 int8_t motor_state_R = 0;
KCHuang 1:d24c3384bc59 55 int8_t motor_state_old_R = 0;
KCHuang 1:d24c3384bc59 56 int count_R = 0;
KCHuang 1:d24c3384bc59 57 float rotation_R = 0.0f;
KCHuang 1:d24c3384bc59 58 float rotation_ref_R = 0.0f;
KCHuang 1:d24c3384bc59 59 float rotation_err_R = 0.0f;
KCHuang 1:d24c3384bc59 60 float rotation_ierr_R = 0.0f;
KCHuang 1:d24c3384bc59 61 float ctrl_output_R = 0.0f;
farmookong 0:5e356103dcc7 62 float pwm1_duty = 0.0f;
KCHuang 1:d24c3384bc59 63 //motor left
KCHuang 1:d24c3384bc59 64 int8_t HallA_state_L = 0;
KCHuang 1:d24c3384bc59 65 int8_t HallB_state_L = 0;
KCHuang 1:d24c3384bc59 66 int8_t motor_state_L = 0;
KCHuang 1:d24c3384bc59 67 int8_t motor_state_old_L= 0;
KCHuang 1:d24c3384bc59 68 int count_L = 0;
KCHuang 1:d24c3384bc59 69 float rotation_L = 0.0f;
KCHuang 1:d24c3384bc59 70 float rotation_ref_L = 0.0f;
KCHuang 1:d24c3384bc59 71 float rotation_err_L = 0.0f;
KCHuang 1:d24c3384bc59 72 float rotation_ierr_L = 0.0f;
KCHuang 1:d24c3384bc59 73 float ctrl_output_L = 0.0f;
farmookong 0:5e356103dcc7 74 float pwm2_duty = 0.0f;
farmookong 0:5e356103dcc7 75 //servo
farmookong 0:5e356103dcc7 76 int i = 0;
farmookong 0:5e356103dcc7 77 //****************************************************************************** End of Variables
KCHuang 1:d24c3384bc59 78
farmookong 2:f6955790b0e0 79 //****************************************************************************** Bluetooth
farmookong 2:f6955790b0e0 80 class HaseHardware : public MbedHardware
farmookong 2:f6955790b0e0 81 {
farmookong 2:f6955790b0e0 82 public:
farmookong 2:f6955790b0e0 83 HaseHardware(): MbedHardware(D10,D2,115200) {};
farmookong 2:f6955790b0e0 84 };
farmookong 4:f828ddd4fed3 85 ros::NodeHandle_<HaseHardware> nh;
farmookong 2:f6955790b0e0 86 //****************************************************************************** End of Bluetooth
farmookong 2:f6955790b0e0 87
KCHuang 1:d24c3384bc59 88 //****************************************************************************** ros related function
farmookong 4:f828ddd4fed3 89 //ros:: NodeHandle nh;
KCHuang 1:d24c3384bc59 90
KCHuang 1:d24c3384bc59 91 geometry_msgs::Twist vel_msg;
KCHuang 1:d24c3384bc59 92 ros::Publisher vel_feedback("feedback_wheel_angularVel", &vel_msg);
KCHuang 1:d24c3384bc59 93 float v_right = 0.0f;
KCHuang 1:d24c3384bc59 94 float v_left = 0.0f;
KCHuang 1:d24c3384bc59 95 void messageCb(const geometry_msgs::Vector3& msg)
KCHuang 1:d24c3384bc59 96 {
KCHuang 1:d24c3384bc59 97 v_right = msg.y; //right wheel speed m/s
KCHuang 1:d24c3384bc59 98 v_left = msg.x; //left wheel speed m/s
KCHuang 1:d24c3384bc59 99
KCHuang 1:d24c3384bc59 100 rotation_ref_R = -v_right/0.03f*60.0f/(2*pi); // m/s to RPM
KCHuang 1:d24c3384bc59 101 rotation_ref_L = v_left/0.03f*60.0f/(2*pi); // m/s to RPM
KCHuang 1:d24c3384bc59 102 //Limit the speed
KCHuang 1:d24c3384bc59 103 /*if(rotation_ref_R > 120.69f)
KCHuang 1:d24c3384bc59 104 {
KCHuang 1:d24c3384bc59 105 rotation_ref_R = 120.69f;
KCHuang 1:d24c3384bc59 106 }
KCHuang 1:d24c3384bc59 107 else if(rotation_ref_R < -120.69f)
KCHuang 1:d24c3384bc59 108 {
KCHuang 1:d24c3384bc59 109 rotation_ref_R = -120.69;
KCHuang 1:d24c3384bc59 110 }
KCHuang 1:d24c3384bc59 111 if(rotation_ref_L > 120.69f)
KCHuang 1:d24c3384bc59 112 {
KCHuang 1:d24c3384bc59 113 rotation_ref_L = 120.69f;
KCHuang 1:d24c3384bc59 114 }
KCHuang 1:d24c3384bc59 115 else if(rotation_ref_L < -120.69f)
KCHuang 1:d24c3384bc59 116 {
KCHuang 1:d24c3384bc59 117 rotation_ref_L = -120.69f;
KCHuang 1:d24c3384bc59 118 }*/
KCHuang 1:d24c3384bc59 119 }
KCHuang 1:d24c3384bc59 120 ros::Subscriber<geometry_msgs::Vector3> cmd_sub("cmd_wheel_angularVel", messageCb);
KCHuang 1:d24c3384bc59 121
KCHuang 1:d24c3384bc59 122
KCHuang 1:d24c3384bc59 123 //****************************************************************************** End of ros related function
farmookong 0:5e356103dcc7 124 //****************************************************************************** Main
farmookong 0:5e356103dcc7 125 int main()
farmookong 0:5e356103dcc7 126 {
farmookong 0:5e356103dcc7 127 init_timer();
farmookong 0:5e356103dcc7 128 init_PWM();
farmookong 0:5e356103dcc7 129 init_CN();
KCHuang 1:d24c3384bc59 130
KCHuang 1:d24c3384bc59 131 nh.initNode();
KCHuang 1:d24c3384bc59 132
KCHuang 1:d24c3384bc59 133 //cmd_sub = nh.subscribe("cmd_wheel_angularVel",10, messageCb);
KCHuang 1:d24c3384bc59 134 nh.subscribe(cmd_sub);
KCHuang 1:d24c3384bc59 135 nh.advertise(vel_feedback);
KCHuang 1:d24c3384bc59 136
farmookong 0:5e356103dcc7 137 while(1)
farmookong 0:5e356103dcc7 138 {
KCHuang 1:d24c3384bc59 139 vel_msg.linear.x = rotation_ref_R;
KCHuang 1:d24c3384bc59 140 vel_msg.linear.y = rotation_R;
KCHuang 1:d24c3384bc59 141 vel_msg.linear.z = 0.0f;
KCHuang 1:d24c3384bc59 142
KCHuang 1:d24c3384bc59 143 vel_msg.angular.x = rotation_ref_L;
KCHuang 1:d24c3384bc59 144 vel_msg.angular.y = rotation_L;
KCHuang 1:d24c3384bc59 145 vel_msg.angular.z = 0.0f;
KCHuang 1:d24c3384bc59 146
KCHuang 1:d24c3384bc59 147 vel_feedback.publish(&vel_msg);
KCHuang 1:d24c3384bc59 148 nh.spinOnce();
KCHuang 1:d24c3384bc59 149 wait_ms(100);
farmookong 0:5e356103dcc7 150 }
farmookong 0:5e356103dcc7 151 }
farmookong 0:5e356103dcc7 152 //****************************************************************************** End of Main
farmookong 0:5e356103dcc7 153
farmookong 0:5e356103dcc7 154 //****************************************************************************** timer_interrupt
farmookong 0:5e356103dcc7 155 void timer_interrupt()
farmookong 0:5e356103dcc7 156 {
KCHuang 1:d24c3384bc59 157 // Motor R
KCHuang 1:d24c3384bc59 158 rotation_R = (float)count_R * 100.0f / 12.0f * 60.0f / 29.0f; //17.24 hall count convert to RPM
KCHuang 1:d24c3384bc59 159 count_R = 0;
farmookong 0:5e356103dcc7 160 // Code for PI controller //
KCHuang 1:d24c3384bc59 161 rotation_err_R = rotation_ref_R - rotation_R;
KCHuang 1:d24c3384bc59 162 rotation_ierr_R += (rotation_err_R*Ts);
KCHuang 1:d24c3384bc59 163 ctrl_output_R = 0.01f*rotation_err_R+ 0.2f*rotation_ierr_R;
farmookong 0:5e356103dcc7 164 ///////////////////////////
farmookong 0:5e356103dcc7 165
KCHuang 1:d24c3384bc59 166 if(ctrl_output_R >= 0.5f)ctrl_output_R = 0.5f;
KCHuang 1:d24c3384bc59 167 else if(ctrl_output_R <= -0.5f)ctrl_output_R = -0.5f;
KCHuang 1:d24c3384bc59 168 pwm1_duty = ctrl_output_R + 0.5f;
farmookong 0:5e356103dcc7 169 pwm1.write(pwm1_duty);
farmookong 0:5e356103dcc7 170 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 171
KCHuang 1:d24c3384bc59 172 // Motor L
KCHuang 1:d24c3384bc59 173 rotation_L = (float)count_L * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
KCHuang 1:d24c3384bc59 174 count_L = 0;
farmookong 0:5e356103dcc7 175 // Code for PI controller //
KCHuang 1:d24c3384bc59 176 rotation_err_L = rotation_ref_L - rotation_L;
KCHuang 1:d24c3384bc59 177 rotation_ierr_L += (rotation_err_L*Ts);
KCHuang 1:d24c3384bc59 178 ctrl_output_L = 0.01f*rotation_err_L + 0.2f*rotation_ierr_L;
farmookong 0:5e356103dcc7 179 ///////////////////////////
KCHuang 1:d24c3384bc59 180 if(ctrl_output_L >= 0.5f)ctrl_output_L = 0.5f;
KCHuang 1:d24c3384bc59 181 else if(ctrl_output_L <= -0.5f)ctrl_output_L = -0.5f;
KCHuang 1:d24c3384bc59 182 pwm2_duty = ctrl_output_L + 0.5f;
farmookong 0:5e356103dcc7 183 pwm2.write(pwm2_duty);
farmookong 0:5e356103dcc7 184 TIM1->CCER |= 0x40;
KCHuang 1:d24c3384bc59 185 /*
KCHuang 1:d24c3384bc59 186 if(rotation_ierr_R > 100 || rotation_ierr_R < -100)
KCHuang 1:d24c3384bc59 187 {
KCHuang 1:d24c3384bc59 188 rotation_ierr_R = 0;
KCHuang 1:d24c3384bc59 189 }
KCHuang 1:d24c3384bc59 190 if(rotation_ierr_L > 100 || rotation_ierr_L < -100)
KCHuang 1:d24c3384bc59 191 {
KCHuang 1:d24c3384bc59 192 rotation_ierr_L = 0;
KCHuang 1:d24c3384bc59 193 } */
farmookong 0:5e356103dcc7 194 //Servo
farmookong 0:5e356103dcc7 195 if(i==100)
farmookong 0:5e356103dcc7 196 {
farmookong 0:5e356103dcc7 197 if(servo_duty < 0.07f)
farmookong 0:5e356103dcc7 198 {
farmookong 0:5e356103dcc7 199 servo_duty = servo_duty+0.04f/6;
farmookong 0:5e356103dcc7 200 }
farmookong 0:5e356103dcc7 201 else
farmookong 0:5e356103dcc7 202 {
farmookong 0:5e356103dcc7 203 servo_duty = 0.07f;
farmookong 0:5e356103dcc7 204 }
farmookong 0:5e356103dcc7 205 servo.write(servo_duty);
farmookong 0:5e356103dcc7 206 i=0;
farmookong 0:5e356103dcc7 207 }
farmookong 0:5e356103dcc7 208 else
farmookong 0:5e356103dcc7 209 {
farmookong 0:5e356103dcc7 210 i++;
farmookong 0:5e356103dcc7 211 }
KCHuang 1:d24c3384bc59 212
farmookong 0:5e356103dcc7 213 }
farmookong 0:5e356103dcc7 214 //****************************************************************************** End of timer_interrupt
farmookong 0:5e356103dcc7 215
farmookong 0:5e356103dcc7 216 //****************************************************************************** CN_interrupt
farmookong 0:5e356103dcc7 217 void CN_interrupt()
farmookong 0:5e356103dcc7 218 {
KCHuang 1:d24c3384bc59 219 // Motor Right
farmookong 0:5e356103dcc7 220 // Read the current status of hall sensor
KCHuang 1:d24c3384bc59 221 HallA_state_R = HallA_R.read();
KCHuang 1:d24c3384bc59 222 HallB_state_R = HallB_R.read();
farmookong 0:5e356103dcc7 223
farmookong 0:5e356103dcc7 224 ///code for state determination///
KCHuang 1:d24c3384bc59 225 if(HallA_state_R == 0 && HallB_state_R == 0)
KCHuang 1:d24c3384bc59 226 motor_state_R = 1;
KCHuang 1:d24c3384bc59 227 else if(HallA_state_R == 0 && HallB_state_R == 1)
KCHuang 1:d24c3384bc59 228 motor_state_R = 2;
KCHuang 1:d24c3384bc59 229 else if(HallA_state_R == 1 && HallB_state_R == 1)
KCHuang 1:d24c3384bc59 230 motor_state_R = 3;
KCHuang 1:d24c3384bc59 231 else if(HallA_state_R == 1 && HallB_state_R == 0)
KCHuang 1:d24c3384bc59 232 motor_state_R = 4;
farmookong 0:5e356103dcc7 233
KCHuang 1:d24c3384bc59 234 if(motor_state_old_R != 0)
farmookong 0:5e356103dcc7 235 {
KCHuang 1:d24c3384bc59 236 if(motor_state_old_R < motor_state_R)
KCHuang 1:d24c3384bc59 237 {
KCHuang 1:d24c3384bc59 238 count_R += 1;
KCHuang 1:d24c3384bc59 239 if(motor_state_old_R == 1 && motor_state_R == 4)
KCHuang 1:d24c3384bc59 240 count_R -= 2;
KCHuang 1:d24c3384bc59 241 }
KCHuang 1:d24c3384bc59 242 else if(motor_state_old_R > motor_state_R)
KCHuang 1:d24c3384bc59 243 {
KCHuang 1:d24c3384bc59 244 count_R -= 1;
KCHuang 1:d24c3384bc59 245 if(motor_state_old_R == 4 && motor_state_R == 1)
KCHuang 1:d24c3384bc59 246 count_R += 2;
KCHuang 1:d24c3384bc59 247 }
farmookong 0:5e356103dcc7 248 }
KCHuang 1:d24c3384bc59 249 motor_state_old_R = motor_state_R;
farmookong 0:5e356103dcc7 250 //////////////////////////////////
KCHuang 1:d24c3384bc59 251
KCHuang 1:d24c3384bc59 252 // Motor Left
farmookong 0:5e356103dcc7 253 // Read the current status of hall sensor
KCHuang 1:d24c3384bc59 254 HallA_state_L = HallA_L.read();
KCHuang 1:d24c3384bc59 255 HallB_state_L = HallB_L.read();
farmookong 0:5e356103dcc7 256
farmookong 0:5e356103dcc7 257 ///code for state determination///
KCHuang 1:d24c3384bc59 258 if(HallA_state_L == 0 && HallB_state_L == 0)
KCHuang 1:d24c3384bc59 259 motor_state_L = 1;
KCHuang 1:d24c3384bc59 260 else if(HallA_state_L == 0 && HallB_state_L == 1)
KCHuang 1:d24c3384bc59 261 motor_state_L = 2;
KCHuang 1:d24c3384bc59 262 else if(HallA_state_L == 1 && HallB_state_L == 1)
KCHuang 1:d24c3384bc59 263 motor_state_L = 3;
KCHuang 1:d24c3384bc59 264 else if(HallA_state_L == 1 && HallB_state_L == 0)
KCHuang 1:d24c3384bc59 265 motor_state_L = 4;
farmookong 0:5e356103dcc7 266
KCHuang 1:d24c3384bc59 267 if(motor_state_old_L != 0)
farmookong 0:5e356103dcc7 268 {
KCHuang 1:d24c3384bc59 269 if(motor_state_old_L < motor_state_L)
KCHuang 1:d24c3384bc59 270 {
KCHuang 1:d24c3384bc59 271 count_L += 1;
KCHuang 1:d24c3384bc59 272 if(motor_state_old_L == 1 && motor_state_L == 4)
KCHuang 1:d24c3384bc59 273 count_L -= 2;
KCHuang 1:d24c3384bc59 274 }
KCHuang 1:d24c3384bc59 275 else if(motor_state_old_L > motor_state_L)
KCHuang 1:d24c3384bc59 276 {
KCHuang 1:d24c3384bc59 277 count_L -= 1;
KCHuang 1:d24c3384bc59 278 if(motor_state_old_L == 4 && motor_state_L == 1)
KCHuang 1:d24c3384bc59 279 count_L += 2;
KCHuang 1:d24c3384bc59 280 }
farmookong 0:5e356103dcc7 281 }
KCHuang 1:d24c3384bc59 282 motor_state_old_L = motor_state_L;
farmookong 0:5e356103dcc7 283 //////////////////////////////////
farmookong 0:5e356103dcc7 284 }
farmookong 0:5e356103dcc7 285 //****************************************************************************** End of CN_interrupt
farmookong 0:5e356103dcc7 286
farmookong 0:5e356103dcc7 287 //****************************************************************************** init_timer
farmookong 0:5e356103dcc7 288 void init_timer()
farmookong 0:5e356103dcc7 289 {
farmookong 0:5e356103dcc7 290 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
farmookong 0:5e356103dcc7 291 }
farmookong 0:5e356103dcc7 292 //****************************************************************************** End of init_timer
farmookong 0:5e356103dcc7 293
farmookong 0:5e356103dcc7 294 //****************************************************************************** init_PWM
farmookong 0:5e356103dcc7 295 void init_PWM()
farmookong 0:5e356103dcc7 296 {
farmookong 0:5e356103dcc7 297 pwm1.period_us(50);
farmookong 0:5e356103dcc7 298 pwm1.write(0.5);
farmookong 0:5e356103dcc7 299 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 300
farmookong 0:5e356103dcc7 301 pwm2.period_us(50);
farmookong 0:5e356103dcc7 302 pwm2.write(0.5);
farmookong 0:5e356103dcc7 303 TIM1->CCER |= 0x40;
farmookong 0:5e356103dcc7 304
farmookong 0:5e356103dcc7 305 servo.period_ms(Servo_Period);
farmookong 0:5e356103dcc7 306 servo.write(servo_duty);
farmookong 0:5e356103dcc7 307 }
farmookong 0:5e356103dcc7 308 //****************************************************************************** End of init_PWM
farmookong 0:5e356103dcc7 309
farmookong 0:5e356103dcc7 310 //****************************************************************************** init_CN
farmookong 0:5e356103dcc7 311 void init_CN()
farmookong 0:5e356103dcc7 312 {
KCHuang 1:d24c3384bc59 313 // Motor Right
KCHuang 1:d24c3384bc59 314 HallA_R.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 315 HallA_R.fall(&CN_interrupt);
KCHuang 1:d24c3384bc59 316 HallB_R.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 317 HallB_R.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 318
KCHuang 1:d24c3384bc59 319 HallA_state_R = HallA_R.read();
KCHuang 1:d24c3384bc59 320 HallB_state_R = HallB_R.read();
farmookong 0:5e356103dcc7 321
KCHuang 1:d24c3384bc59 322 // Motor Left
KCHuang 1:d24c3384bc59 323 HallA_L.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 324 HallA_L.fall(&CN_interrupt);
KCHuang 1:d24c3384bc59 325 HallB_L.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 326 HallB_L.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 327
KCHuang 1:d24c3384bc59 328 HallA_state_L = HallA_L.read();
KCHuang 1:d24c3384bc59 329 HallB_state_L = HallB_L.read();
farmookong 0:5e356103dcc7 330 }
KCHuang 1:d24c3384bc59 331 //****************************************************************************** End of init_CN