ROBOTIC CHAMPION TEAM / Mbed 2 deprecated ROS_remote_car

lab5

Dependencies:   mbed ros_lib_indigo

main.cpp@0:0846fa975a83, 2017-03-28 (annotated)

Committer:
tea5062001
Date:
Tue Mar 28 13:14:57 2017 +0000
Revision:
0:0846fa975a83
lab5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tea5062001 0:0846fa975a83 1 #include "mbed.h"
tea5062001 0:0846fa975a83 2 #include <ros.h>
tea5062001 0:0846fa975a83 3 #include <geometry_msgs/Twist.h>
tea5062001 0:0846fa975a83 4 #include <geometry_msgs/Vector3.h>
tea5062001 0:0846fa975a83 5
tea5062001 0:0846fa975a83 6 //The number will be compiled as type "double" in default
tea5062001 0:0846fa975a83 7 //Add a "f" after the number can make it compiled as type "float"
tea5062001 0:0846fa975a83 8 #define Ts 0.01f //period of timer1 (s)
tea5062001 0:0846fa975a83 9 #define Kp 0.006f
tea5062001 0:0846fa975a83 10 #define Ki 0.02f
tea5062001 0:0846fa975a83 11
tea5062001 0:0846fa975a83 12 Ticker timer1;
tea5062001 0:0846fa975a83 13 // servo motor
tea5062001 0:0846fa975a83 14 PwmOut servo_cmd(A0);
tea5062001 0:0846fa975a83 15 // DC motor
tea5062001 0:0846fa975a83 16 PwmOut pwm1(D7);
tea5062001 0:0846fa975a83 17 PwmOut pwm1n(D11);
tea5062001 0:0846fa975a83 18 PwmOut pwm2(D8);
tea5062001 0:0846fa975a83 19 PwmOut pwm2n(A3);
tea5062001 0:0846fa975a83 20
tea5062001 0:0846fa975a83 21 // Motor1 sensor
tea5062001 0:0846fa975a83 22 InterruptIn HallA(A1);
tea5062001 0:0846fa975a83 23 InterruptIn HallB(A2);
tea5062001 0:0846fa975a83 24 // Motor2 sensor
tea5062001 0:0846fa975a83 25 InterruptIn HallA_2(D13);
tea5062001 0:0846fa975a83 26 InterruptIn HallB_2(D12);
tea5062001 0:0846fa975a83 27
tea5062001 0:0846fa975a83 28 // 函式宣告
tea5062001 0:0846fa975a83 29 void init_IO();
tea5062001 0:0846fa975a83 30 void init_TIMER();
tea5062001 0:0846fa975a83 31 void timer1_ITR();
tea5062001 0:0846fa975a83 32 void init_CN();
tea5062001 0:0846fa975a83 33 void CN_ITR();
tea5062001 0:0846fa975a83 34 void init_PWM();
tea5062001 0:0846fa975a83 35
tea5062001 0:0846fa975a83 36 // servo motor
tea5062001 0:0846fa975a83 37 float servo_duty = 0.025; // 0.069 +(0.088/180)*angle, -90<angle<90
tea5062001 0:0846fa975a83 38 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
tea5062001 0:0846fa975a83 39 int angle = 0;
tea5062001 0:0846fa975a83 40 int counter;
tea5062001 0:0846fa975a83 41
tea5062001 0:0846fa975a83 42
tea5062001 0:0846fa975a83 43 // Hall sensor
tea5062001 0:0846fa975a83 44 int HallA_1_state = 0;
tea5062001 0:0846fa975a83 45 int HallB_1_state = 0;
tea5062001 0:0846fa975a83 46 int state_1 = 0;
tea5062001 0:0846fa975a83 47 int state_1_old = 0;
tea5062001 0:0846fa975a83 48 int HallA_2_state = 0;
tea5062001 0:0846fa975a83 49 int HallB_2_state = 0;
tea5062001 0:0846fa975a83 50 int state_2 = 0;
tea5062001 0:0846fa975a83 51 int state_2_old = 0;
tea5062001 0:0846fa975a83 52
tea5062001 0:0846fa975a83 53 int c = 0;
tea5062001 0:0846fa975a83 54 int d = 0;
tea5062001 0:0846fa975a83 55
tea5062001 0:0846fa975a83 56 // DC motor rotation speed control
tea5062001 0:0846fa975a83 57 int speed_count_1 = 0;
tea5062001 0:0846fa975a83 58 float rotation_speed_1 = 0.0;
tea5062001 0:0846fa975a83 59 float rotation_speed_ref_1 = 0;
tea5062001 0:0846fa975a83 60 float pwm1_duty = 0.5;
tea5062001 0:0846fa975a83 61 float PI_out_1 = 0.0;
tea5062001 0:0846fa975a83 62 float err_1 = 0.0;
tea5062001 0:0846fa975a83 63 float ierr_1 = 0.0;
tea5062001 0:0846fa975a83 64 int speed_count_2 = 0;
tea5062001 0:0846fa975a83 65 float rotation_speed_2 = 0.0;
tea5062001 0:0846fa975a83 66 float rotation_speed_ref_2 = 0;
tea5062001 0:0846fa975a83 67 float pwm2_duty = 0.5;
tea5062001 0:0846fa975a83 68 float PI_out_2 = 0.0;
tea5062001 0:0846fa975a83 69 float err_2 = 0.0;
tea5062001 0:0846fa975a83 70 float ierr_2 = 0.0;
tea5062001 0:0846fa975a83 71
tea5062001 0:0846fa975a83 72 //rosserial
tea5062001 0:0846fa975a83 73 ros::NodeHandle nh;
tea5062001 0:0846fa975a83 74
tea5062001 0:0846fa975a83 75 geometry_msgs::Twist vel_msg;
tea5062001 0:0846fa975a83 76 ros::Publisher p("feedback_wheel_angularVel", &vel_msg);
tea5062001 0:0846fa975a83 77
tea5062001 0:0846fa975a83 78 void messageCallback(const geometry_msgs::Vector3 &msg_receive)
tea5062001 0:0846fa975a83 79 {
tea5062001 0:0846fa975a83 80 rotation_speed_ref_1 = -msg_receive.x;
tea5062001 0:0846fa975a83 81 rotation_speed_ref_2 = msg_receive.y;
tea5062001 0:0846fa975a83 82 }
tea5062001 0:0846fa975a83 83
tea5062001 0:0846fa975a83 84 ros::Subscriber<geometry_msgs::Vector3> s("cmd_wheel_angularVel",messageCallback);
tea5062001 0:0846fa975a83 85
tea5062001 0:0846fa975a83 86 int main()
tea5062001 0:0846fa975a83 87 {
tea5062001 0:0846fa975a83 88 init_TIMER();
tea5062001 0:0846fa975a83 89 init_PWM();
tea5062001 0:0846fa975a83 90 init_CN();
tea5062001 0:0846fa975a83 91
tea5062001 0:0846fa975a83 92 nh.initNode();
tea5062001 0:0846fa975a83 93 nh.subscribe(s);
tea5062001 0:0846fa975a83 94 nh.advertise(p);
tea5062001 0:0846fa975a83 95
tea5062001 0:0846fa975a83 96 while(1)
tea5062001 0:0846fa975a83 97 {
tea5062001 0:0846fa975a83 98 vel_msg.linear.x = rotation_speed_ref_1;
tea5062001 0:0846fa975a83 99 vel_msg.linear.y = rotation_speed_1;
tea5062001 0:0846fa975a83 100 vel_msg.linear.z = 0;
tea5062001 0:0846fa975a83 101
tea5062001 0:0846fa975a83 102 vel_msg.angular.x = rotation_speed_ref_2;
tea5062001 0:0846fa975a83 103 vel_msg.angular.y = rotation_speed_2;
tea5062001 0:0846fa975a83 104 vel_msg.angular.z = 0;
tea5062001 0:0846fa975a83 105
tea5062001 0:0846fa975a83 106 p.publish(&vel_msg);
tea5062001 0:0846fa975a83 107
tea5062001 0:0846fa975a83 108 nh.spinOnce();
tea5062001 0:0846fa975a83 109 wait_ms(20);
tea5062001 0:0846fa975a83 110 }
tea5062001 0:0846fa975a83 111 }
tea5062001 0:0846fa975a83 112
tea5062001 0:0846fa975a83 113 void init_TIMER()
tea5062001 0:0846fa975a83 114 {
tea5062001 0:0846fa975a83 115 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
tea5062001 0:0846fa975a83 116 }
tea5062001 0:0846fa975a83 117
tea5062001 0:0846fa975a83 118 void init_PWM()
tea5062001 0:0846fa975a83 119 {
tea5062001 0:0846fa975a83 120 servo_cmd.period_ms(20);
tea5062001 0:0846fa975a83 121 servo_cmd.write(servo_duty);
tea5062001 0:0846fa975a83 122
tea5062001 0:0846fa975a83 123 pwm1.period_us(50);
tea5062001 0:0846fa975a83 124 pwm1.write(0.5);
tea5062001 0:0846fa975a83 125 TIM1->CCER |= 0x4;
tea5062001 0:0846fa975a83 126
tea5062001 0:0846fa975a83 127 pwm2.period_us(50);
tea5062001 0:0846fa975a83 128 pwm2.write(0.5);
tea5062001 0:0846fa975a83 129 TIM1->CCER |= 0x40;
tea5062001 0:0846fa975a83 130 }
tea5062001 0:0846fa975a83 131 void init_CN()
tea5062001 0:0846fa975a83 132 {
tea5062001 0:0846fa975a83 133 HallA.rise(&CN_ITR);
tea5062001 0:0846fa975a83 134 HallA.fall(&CN_ITR);
tea5062001 0:0846fa975a83 135 HallB.rise(&CN_ITR);
tea5062001 0:0846fa975a83 136 HallB.fall(&CN_ITR);
tea5062001 0:0846fa975a83 137
tea5062001 0:0846fa975a83 138 HallA_2.rise(&CN_ITR);
tea5062001 0:0846fa975a83 139 HallA_2.fall(&CN_ITR);
tea5062001 0:0846fa975a83 140 HallB_2.rise(&CN_ITR);
tea5062001 0:0846fa975a83 141 HallB_2.fall(&CN_ITR);
tea5062001 0:0846fa975a83 142 }
tea5062001 0:0846fa975a83 143
tea5062001 0:0846fa975a83 144 void CN_ITR()
tea5062001 0:0846fa975a83 145 {
tea5062001 0:0846fa975a83 146 // motor1
tea5062001 0:0846fa975a83 147 HallA_1_state = HallA.read();
tea5062001 0:0846fa975a83 148 HallB_1_state = HallB.read();
tea5062001 0:0846fa975a83 149
tea5062001 0:0846fa975a83 150 ///code for state determination///
tea5062001 0:0846fa975a83 151
tea5062001 0:0846fa975a83 152 state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10)
tea5062001 0:0846fa975a83 153
tea5062001 0:0846fa975a83 154 if(state_1_old != state_1)
tea5062001 0:0846fa975a83 155 {
tea5062001 0:0846fa975a83 156 if((state_1_old/10) == (state_1_old%10))
tea5062001 0:0846fa975a83 157 {
tea5062001 0:0846fa975a83 158 if((state_1%10) != (state_1_old%10))
tea5062001 0:0846fa975a83 159 {
tea5062001 0:0846fa975a83 160 speed_count_1++;
tea5062001 0:0846fa975a83 161 }
tea5062001 0:0846fa975a83 162 else if((state_1/10) != (state_1_old/10))
tea5062001 0:0846fa975a83 163 {
tea5062001 0:0846fa975a83 164 speed_count_1--;
tea5062001 0:0846fa975a83 165 }
tea5062001 0:0846fa975a83 166 }
tea5062001 0:0846fa975a83 167 else if((state_1_old/10) != (state_1_old%10))
tea5062001 0:0846fa975a83 168 {
tea5062001 0:0846fa975a83 169 if((state_1%10) != (state_1_old%10))
tea5062001 0:0846fa975a83 170 {
tea5062001 0:0846fa975a83 171 speed_count_1--;
tea5062001 0:0846fa975a83 172 }
tea5062001 0:0846fa975a83 173 else if((state_1/10) != (state_1_old/10))
tea5062001 0:0846fa975a83 174 {
tea5062001 0:0846fa975a83 175 speed_count_1++;
tea5062001 0:0846fa975a83 176 }
tea5062001 0:0846fa975a83 177 }
tea5062001 0:0846fa975a83 178
tea5062001 0:0846fa975a83 179 state_1_old = state_1;
tea5062001 0:0846fa975a83 180 }
tea5062001 0:0846fa975a83 181
tea5062001 0:0846fa975a83 182
tea5062001 0:0846fa975a83 183 //////////////////////////////////
tea5062001 0:0846fa975a83 184
tea5062001 0:0846fa975a83 185 //forward : speed_count_1 + 1
tea5062001 0:0846fa975a83 186 //backward : speed_count_1 - 1
tea5062001 0:0846fa975a83 187
tea5062001 0:0846fa975a83 188
tea5062001 0:0846fa975a83 189 // motor2
tea5062001 0:0846fa975a83 190 HallA_2_state = HallA_2.read();
tea5062001 0:0846fa975a83 191 HallB_2_state = HallB_2.read();
tea5062001 0:0846fa975a83 192
tea5062001 0:0846fa975a83 193 ///code for state determination///
tea5062001 0:0846fa975a83 194
tea5062001 0:0846fa975a83 195 state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10)
tea5062001 0:0846fa975a83 196
tea5062001 0:0846fa975a83 197 if(state_2_old != state_2)
tea5062001 0:0846fa975a83 198 {
tea5062001 0:0846fa975a83 199 if((state_2_old/10) == (state_2_old%10))
tea5062001 0:0846fa975a83 200 {
tea5062001 0:0846fa975a83 201 if((state_2%10) != (state_2_old%10))
tea5062001 0:0846fa975a83 202 {
tea5062001 0:0846fa975a83 203 speed_count_2++;
tea5062001 0:0846fa975a83 204 }
tea5062001 0:0846fa975a83 205 else if((state_2/10) != (state_2_old/10))
tea5062001 0:0846fa975a83 206 {
tea5062001 0:0846fa975a83 207 speed_count_2--;
tea5062001 0:0846fa975a83 208 }
tea5062001 0:0846fa975a83 209 }
tea5062001 0:0846fa975a83 210 else if((state_2_old/10) != (state_2_old%10))
tea5062001 0:0846fa975a83 211 {
tea5062001 0:0846fa975a83 212 if((state_2%10) != (state_2_old%10))
tea5062001 0:0846fa975a83 213 {
tea5062001 0:0846fa975a83 214 speed_count_2--;
tea5062001 0:0846fa975a83 215 }
tea5062001 0:0846fa975a83 216 else if((state_2/10) != (state_2_old/10))
tea5062001 0:0846fa975a83 217 {
tea5062001 0:0846fa975a83 218 speed_count_2++;
tea5062001 0:0846fa975a83 219 }
tea5062001 0:0846fa975a83 220 }
tea5062001 0:0846fa975a83 221
tea5062001 0:0846fa975a83 222 state_2_old = state_2;
tea5062001 0:0846fa975a83 223 }
tea5062001 0:0846fa975a83 224
tea5062001 0:0846fa975a83 225
tea5062001 0:0846fa975a83 226
tea5062001 0:0846fa975a83 227 //////////////////////////////////
tea5062001 0:0846fa975a83 228
tea5062001 0:0846fa975a83 229 //forward : speed_count_2 + 1
tea5062001 0:0846fa975a83 230 //backward : speed_count_2 - 1
tea5062001 0:0846fa975a83 231 }
tea5062001 0:0846fa975a83 232
tea5062001 0:0846fa975a83 233 void timer1_ITR()
tea5062001 0:0846fa975a83 234 {
tea5062001 0:0846fa975a83 235 // servo motor
tea5062001 0:0846fa975a83 236 ///code for sevo motor///
tea5062001 0:0846fa975a83 237
tea5062001 0:0846fa975a83 238 counter = counter + 1;
tea5062001 0:0846fa975a83 239
tea5062001 0:0846fa975a83 240 if(counter == 100)
tea5062001 0:0846fa975a83 241 {
tea5062001 0:0846fa975a83 242 servo_duty = 0.069;
tea5062001 0:0846fa975a83 243 }
tea5062001 0:0846fa975a83 244 if(counter == 200)
tea5062001 0:0846fa975a83 245 {
tea5062001 0:0846fa975a83 246 servo_duty = 0.0763;
tea5062001 0:0846fa975a83 247 }
tea5062001 0:0846fa975a83 248 if(counter == 300)
tea5062001 0:0846fa975a83 249 {
tea5062001 0:0846fa975a83 250 servo_duty = 0.0837;
tea5062001 0:0846fa975a83 251 }
tea5062001 0:0846fa975a83 252 if(counter == 400)
tea5062001 0:0846fa975a83 253 {
tea5062001 0:0846fa975a83 254 servo_duty = 0.091;
tea5062001 0:0846fa975a83 255 }
tea5062001 0:0846fa975a83 256 if(counter == 500)
tea5062001 0:0846fa975a83 257 {
tea5062001 0:0846fa975a83 258 servo_duty = 0.0983;
tea5062001 0:0846fa975a83 259 }
tea5062001 0:0846fa975a83 260 if(counter == 600)
tea5062001 0:0846fa975a83 261 {
tea5062001 0:0846fa975a83 262 servo_duty = 0.106;
tea5062001 0:0846fa975a83 263 }
tea5062001 0:0846fa975a83 264
tea5062001 0:0846fa975a83 265 if(counter == 700)
tea5062001 0:0846fa975a83 266 {
tea5062001 0:0846fa975a83 267 servo_duty = 0.113;
tea5062001 0:0846fa975a83 268 }
tea5062001 0:0846fa975a83 269 if(counter > 700)
tea5062001 0:0846fa975a83 270 {
tea5062001 0:0846fa975a83 271 counter=0;
tea5062001 0:0846fa975a83 272 }
tea5062001 0:0846fa975a83 273
tea5062001 0:0846fa975a83 274 servo_cmd.write(servo_duty);
tea5062001 0:0846fa975a83 275
tea5062001 0:0846fa975a83 276
tea5062001 0:0846fa975a83 277 /////////////////////////
tea5062001 0:0846fa975a83 278
tea5062001 0:0846fa975a83 279 if(servo_duty >= 0.113f)servo_duty = 0.113;
tea5062001 0:0846fa975a83 280 else if(servo_duty <= 0.025f)servo_duty = 0.025;
tea5062001 0:0846fa975a83 281 servo_cmd.write(servo_duty);
tea5062001 0:0846fa975a83 282
tea5062001 0:0846fa975a83 283 // motor1
tea5062001 0:0846fa975a83 284 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f ; //unit: rpm
tea5062001 0:0846fa975a83 285 speed_count_1 = 0;
tea5062001 0:0846fa975a83 286
tea5062001 0:0846fa975a83 287 ///PI controller for motor1///
tea5062001 0:0846fa975a83 288
tea5062001 0:0846fa975a83 289 err_1 = rotation_speed_ref_1 - rotation_speed_1;
tea5062001 0:0846fa975a83 290 ierr_1 = ierr_1 + err_1*Ts;
tea5062001 0:0846fa975a83 291 PI_out_1 = Kp*err_1 + Ki*ierr_1;
tea5062001 0:0846fa975a83 292
tea5062001 0:0846fa975a83 293 //////////////////////////////
tea5062001 0:0846fa975a83 294
tea5062001 0:0846fa975a83 295 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
tea5062001 0:0846fa975a83 296 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
tea5062001 0:0846fa975a83 297 pwm1_duty = PI_out_1 + 0.5f;
tea5062001 0:0846fa975a83 298 pwm1.write(PI_out_1 + 0.5f);
tea5062001 0:0846fa975a83 299 TIM1->CCER |= 0x4;
tea5062001 0:0846fa975a83 300
tea5062001 0:0846fa975a83 301 //motor2
tea5062001 0:0846fa975a83 302 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
tea5062001 0:0846fa975a83 303 speed_count_2 = 0;
tea5062001 0:0846fa975a83 304
tea5062001 0:0846fa975a83 305 ///PI controller for motor2///
tea5062001 0:0846fa975a83 306
tea5062001 0:0846fa975a83 307 err_2 = rotation_speed_ref_2 - rotation_speed_2;
tea5062001 0:0846fa975a83 308 ierr_2 = ierr_2 + err_2*Ts;
tea5062001 0:0846fa975a83 309 PI_out_2 = Kp*err_2 + Ki*ierr_2;
tea5062001 0:0846fa975a83 310
tea5062001 0:0846fa975a83 311 //////////////////////////////
tea5062001 0:0846fa975a83 312
tea5062001 0:0846fa975a83 313 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
tea5062001 0:0846fa975a83 314 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
tea5062001 0:0846fa975a83 315 pwm2_duty = PI_out_2 + 0.5f;
tea5062001 0:0846fa975a83 316 pwm2.write(PI_out_2 + 0.5f);
tea5062001 0:0846fa975a83 317 TIM1->CCER |= 0x40;
tea5062001 0:0846fa975a83 318 }