NTHU機器人學 Team3 / Mbed 2 deprecated ROS_remote_car

哦0'_'0

Dependencies:   mbed ros_lib_indigo

Fork of LAB4 by NTHU機器人學 Team3

main.cpp@1:d24c3384bc59, 2018-04-18 (annotated)

Committer:
KCHuang
Date:
Wed Apr 18 09:48:11 2018 +0000
Revision:
1:d24c3384bc59
Parent:
0:5e356103dcc7 
??

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
KCHuang 1:d24c3384bc59 79 //****************************************************************************** ros related function
KCHuang 1:d24c3384bc59 80 ros:: NodeHandle nh;
KCHuang 1:d24c3384bc59 81
KCHuang 1:d24c3384bc59 82 geometry_msgs::Twist vel_msg;
KCHuang 1:d24c3384bc59 83 ros::Publisher vel_feedback("feedback_wheel_angularVel", &vel_msg);
KCHuang 1:d24c3384bc59 84 float v_right = 0.0f;
KCHuang 1:d24c3384bc59 85 float v_left = 0.0f;
KCHuang 1:d24c3384bc59 86 void messageCb(const geometry_msgs::Vector3& msg)
KCHuang 1:d24c3384bc59 87 {
KCHuang 1:d24c3384bc59 88 v_right = msg.y; //right wheel speed m/s
KCHuang 1:d24c3384bc59 89 v_left = msg.x; //left wheel speed m/s
KCHuang 1:d24c3384bc59 90
KCHuang 1:d24c3384bc59 91 rotation_ref_R = -v_right/0.03f*60.0f/(2*pi); // m/s to RPM
KCHuang 1:d24c3384bc59 92 rotation_ref_L = v_left/0.03f*60.0f/(2*pi); // m/s to RPM
KCHuang 1:d24c3384bc59 93 //Limit the speed
KCHuang 1:d24c3384bc59 94 /*if(rotation_ref_R > 120.69f)
KCHuang 1:d24c3384bc59 95 {
KCHuang 1:d24c3384bc59 96 rotation_ref_R = 120.69f;
KCHuang 1:d24c3384bc59 97 }
KCHuang 1:d24c3384bc59 98 else if(rotation_ref_R < -120.69f)
KCHuang 1:d24c3384bc59 99 {
KCHuang 1:d24c3384bc59 100 rotation_ref_R = -120.69;
KCHuang 1:d24c3384bc59 101 }
KCHuang 1:d24c3384bc59 102 if(rotation_ref_L > 120.69f)
KCHuang 1:d24c3384bc59 103 {
KCHuang 1:d24c3384bc59 104 rotation_ref_L = 120.69f;
KCHuang 1:d24c3384bc59 105 }
KCHuang 1:d24c3384bc59 106 else if(rotation_ref_L < -120.69f)
KCHuang 1:d24c3384bc59 107 {
KCHuang 1:d24c3384bc59 108 rotation_ref_L = -120.69f;
KCHuang 1:d24c3384bc59 109 }*/
KCHuang 1:d24c3384bc59 110 }
KCHuang 1:d24c3384bc59 111 ros::Subscriber<geometry_msgs::Vector3> cmd_sub("cmd_wheel_angularVel", messageCb);
KCHuang 1:d24c3384bc59 112
KCHuang 1:d24c3384bc59 113
KCHuang 1:d24c3384bc59 114 //****************************************************************************** End of ros related function
farmookong 0:5e356103dcc7 115 //****************************************************************************** Main
farmookong 0:5e356103dcc7 116 int main()
farmookong 0:5e356103dcc7 117 {
farmookong 0:5e356103dcc7 118 init_timer();
farmookong 0:5e356103dcc7 119 init_PWM();
farmookong 0:5e356103dcc7 120 init_CN();
KCHuang 1:d24c3384bc59 121
KCHuang 1:d24c3384bc59 122 nh.initNode();
KCHuang 1:d24c3384bc59 123
KCHuang 1:d24c3384bc59 124 //cmd_sub = nh.subscribe("cmd_wheel_angularVel",10, messageCb);
KCHuang 1:d24c3384bc59 125 nh.subscribe(cmd_sub);
KCHuang 1:d24c3384bc59 126 nh.advertise(vel_feedback);
KCHuang 1:d24c3384bc59 127
farmookong 0:5e356103dcc7 128 while(1)
farmookong 0:5e356103dcc7 129 {
KCHuang 1:d24c3384bc59 130 vel_msg.linear.x = rotation_ref_R;
KCHuang 1:d24c3384bc59 131 vel_msg.linear.y = rotation_R;
KCHuang 1:d24c3384bc59 132 vel_msg.linear.z = 0.0f;
KCHuang 1:d24c3384bc59 133
KCHuang 1:d24c3384bc59 134 vel_msg.angular.x = rotation_ref_L;
KCHuang 1:d24c3384bc59 135 vel_msg.angular.y = rotation_L;
KCHuang 1:d24c3384bc59 136 vel_msg.angular.z = 0.0f;
KCHuang 1:d24c3384bc59 137
KCHuang 1:d24c3384bc59 138 vel_feedback.publish(&vel_msg);
KCHuang 1:d24c3384bc59 139 nh.spinOnce();
KCHuang 1:d24c3384bc59 140 wait_ms(100);
farmookong 0:5e356103dcc7 141 }
farmookong 0:5e356103dcc7 142 }
farmookong 0:5e356103dcc7 143 //****************************************************************************** End of Main
farmookong 0:5e356103dcc7 144
farmookong 0:5e356103dcc7 145 //****************************************************************************** timer_interrupt
farmookong 0:5e356103dcc7 146 void timer_interrupt()
farmookong 0:5e356103dcc7 147 {
KCHuang 1:d24c3384bc59 148 // Motor R
KCHuang 1:d24c3384bc59 149 rotation_R = (float)count_R * 100.0f / 12.0f * 60.0f / 29.0f; //17.24 hall count convert to RPM
KCHuang 1:d24c3384bc59 150 count_R = 0;
farmookong 0:5e356103dcc7 151 // Code for PI controller //
KCHuang 1:d24c3384bc59 152 rotation_err_R = rotation_ref_R - rotation_R;
KCHuang 1:d24c3384bc59 153 rotation_ierr_R += (rotation_err_R*Ts);
KCHuang 1:d24c3384bc59 154 ctrl_output_R = 0.01f*rotation_err_R+ 0.2f*rotation_ierr_R;
farmookong 0:5e356103dcc7 155 ///////////////////////////
farmookong 0:5e356103dcc7 156
KCHuang 1:d24c3384bc59 157 if(ctrl_output_R >= 0.5f)ctrl_output_R = 0.5f;
KCHuang 1:d24c3384bc59 158 else if(ctrl_output_R <= -0.5f)ctrl_output_R = -0.5f;
KCHuang 1:d24c3384bc59 159 pwm1_duty = ctrl_output_R + 0.5f;
farmookong 0:5e356103dcc7 160 pwm1.write(pwm1_duty);
farmookong 0:5e356103dcc7 161 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 162
KCHuang 1:d24c3384bc59 163 // Motor L
KCHuang 1:d24c3384bc59 164 rotation_L = (float)count_L * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
KCHuang 1:d24c3384bc59 165 count_L = 0;
farmookong 0:5e356103dcc7 166 // Code for PI controller //
KCHuang 1:d24c3384bc59 167 rotation_err_L = rotation_ref_L - rotation_L;
KCHuang 1:d24c3384bc59 168 rotation_ierr_L += (rotation_err_L*Ts);
KCHuang 1:d24c3384bc59 169 ctrl_output_L = 0.01f*rotation_err_L + 0.2f*rotation_ierr_L;
farmookong 0:5e356103dcc7 170 ///////////////////////////
KCHuang 1:d24c3384bc59 171 if(ctrl_output_L >= 0.5f)ctrl_output_L = 0.5f;
KCHuang 1:d24c3384bc59 172 else if(ctrl_output_L <= -0.5f)ctrl_output_L = -0.5f;
KCHuang 1:d24c3384bc59 173 pwm2_duty = ctrl_output_L + 0.5f;
farmookong 0:5e356103dcc7 174 pwm2.write(pwm2_duty);
farmookong 0:5e356103dcc7 175 TIM1->CCER |= 0x40;
KCHuang 1:d24c3384bc59 176 /*
KCHuang 1:d24c3384bc59 177 if(rotation_ierr_R > 100 || rotation_ierr_R < -100)
KCHuang 1:d24c3384bc59 178 {
KCHuang 1:d24c3384bc59 179 rotation_ierr_R = 0;
KCHuang 1:d24c3384bc59 180 }
KCHuang 1:d24c3384bc59 181 if(rotation_ierr_L > 100 || rotation_ierr_L < -100)
KCHuang 1:d24c3384bc59 182 {
KCHuang 1:d24c3384bc59 183 rotation_ierr_L = 0;
KCHuang 1:d24c3384bc59 184 } */
farmookong 0:5e356103dcc7 185 //Servo
farmookong 0:5e356103dcc7 186 if(i==100)
farmookong 0:5e356103dcc7 187 {
farmookong 0:5e356103dcc7 188 if(servo_duty < 0.07f)
farmookong 0:5e356103dcc7 189 {
farmookong 0:5e356103dcc7 190 servo_duty = servo_duty+0.04f/6;
farmookong 0:5e356103dcc7 191 }
farmookong 0:5e356103dcc7 192 else
farmookong 0:5e356103dcc7 193 {
farmookong 0:5e356103dcc7 194 servo_duty = 0.07f;
farmookong 0:5e356103dcc7 195 }
farmookong 0:5e356103dcc7 196 servo.write(servo_duty);
farmookong 0:5e356103dcc7 197 i=0;
farmookong 0:5e356103dcc7 198 }
farmookong 0:5e356103dcc7 199 else
farmookong 0:5e356103dcc7 200 {
farmookong 0:5e356103dcc7 201 i++;
farmookong 0:5e356103dcc7 202 }
KCHuang 1:d24c3384bc59 203
farmookong 0:5e356103dcc7 204 }
farmookong 0:5e356103dcc7 205 //****************************************************************************** End of timer_interrupt
farmookong 0:5e356103dcc7 206
farmookong 0:5e356103dcc7 207 //****************************************************************************** CN_interrupt
farmookong 0:5e356103dcc7 208 void CN_interrupt()
farmookong 0:5e356103dcc7 209 {
KCHuang 1:d24c3384bc59 210 // Motor Right
farmookong 0:5e356103dcc7 211 // Read the current status of hall sensor
KCHuang 1:d24c3384bc59 212 HallA_state_R = HallA_R.read();
KCHuang 1:d24c3384bc59 213 HallB_state_R = HallB_R.read();
farmookong 0:5e356103dcc7 214
farmookong 0:5e356103dcc7 215 ///code for state determination///
KCHuang 1:d24c3384bc59 216 if(HallA_state_R == 0 && HallB_state_R == 0)
KCHuang 1:d24c3384bc59 217 motor_state_R = 1;
KCHuang 1:d24c3384bc59 218 else if(HallA_state_R == 0 && HallB_state_R == 1)
KCHuang 1:d24c3384bc59 219 motor_state_R = 2;
KCHuang 1:d24c3384bc59 220 else if(HallA_state_R == 1 && HallB_state_R == 1)
KCHuang 1:d24c3384bc59 221 motor_state_R = 3;
KCHuang 1:d24c3384bc59 222 else if(HallA_state_R == 1 && HallB_state_R == 0)
KCHuang 1:d24c3384bc59 223 motor_state_R = 4;
farmookong 0:5e356103dcc7 224
KCHuang 1:d24c3384bc59 225 if(motor_state_old_R != 0)
farmookong 0:5e356103dcc7 226 {
KCHuang 1:d24c3384bc59 227 if(motor_state_old_R < motor_state_R)
KCHuang 1:d24c3384bc59 228 {
KCHuang 1:d24c3384bc59 229 count_R += 1;
KCHuang 1:d24c3384bc59 230 if(motor_state_old_R == 1 && motor_state_R == 4)
KCHuang 1:d24c3384bc59 231 count_R -= 2;
KCHuang 1:d24c3384bc59 232 }
KCHuang 1:d24c3384bc59 233 else if(motor_state_old_R > motor_state_R)
KCHuang 1:d24c3384bc59 234 {
KCHuang 1:d24c3384bc59 235 count_R -= 1;
KCHuang 1:d24c3384bc59 236 if(motor_state_old_R == 4 && motor_state_R == 1)
KCHuang 1:d24c3384bc59 237 count_R += 2;
KCHuang 1:d24c3384bc59 238 }
farmookong 0:5e356103dcc7 239 }
KCHuang 1:d24c3384bc59 240 motor_state_old_R = motor_state_R;
farmookong 0:5e356103dcc7 241 //////////////////////////////////
KCHuang 1:d24c3384bc59 242
KCHuang 1:d24c3384bc59 243 // Motor Left
farmookong 0:5e356103dcc7 244 // Read the current status of hall sensor
KCHuang 1:d24c3384bc59 245 HallA_state_L = HallA_L.read();
KCHuang 1:d24c3384bc59 246 HallB_state_L = HallB_L.read();
farmookong 0:5e356103dcc7 247
farmookong 0:5e356103dcc7 248 ///code for state determination///
KCHuang 1:d24c3384bc59 249 if(HallA_state_L == 0 && HallB_state_L == 0)
KCHuang 1:d24c3384bc59 250 motor_state_L = 1;
KCHuang 1:d24c3384bc59 251 else if(HallA_state_L == 0 && HallB_state_L == 1)
KCHuang 1:d24c3384bc59 252 motor_state_L = 2;
KCHuang 1:d24c3384bc59 253 else if(HallA_state_L == 1 && HallB_state_L == 1)
KCHuang 1:d24c3384bc59 254 motor_state_L = 3;
KCHuang 1:d24c3384bc59 255 else if(HallA_state_L == 1 && HallB_state_L == 0)
KCHuang 1:d24c3384bc59 256 motor_state_L = 4;
farmookong 0:5e356103dcc7 257
KCHuang 1:d24c3384bc59 258 if(motor_state_old_L != 0)
farmookong 0:5e356103dcc7 259 {
KCHuang 1:d24c3384bc59 260 if(motor_state_old_L < motor_state_L)
KCHuang 1:d24c3384bc59 261 {
KCHuang 1:d24c3384bc59 262 count_L += 1;
KCHuang 1:d24c3384bc59 263 if(motor_state_old_L == 1 && motor_state_L == 4)
KCHuang 1:d24c3384bc59 264 count_L -= 2;
KCHuang 1:d24c3384bc59 265 }
KCHuang 1:d24c3384bc59 266 else if(motor_state_old_L > motor_state_L)
KCHuang 1:d24c3384bc59 267 {
KCHuang 1:d24c3384bc59 268 count_L -= 1;
KCHuang 1:d24c3384bc59 269 if(motor_state_old_L == 4 && motor_state_L == 1)
KCHuang 1:d24c3384bc59 270 count_L += 2;
KCHuang 1:d24c3384bc59 271 }
farmookong 0:5e356103dcc7 272 }
KCHuang 1:d24c3384bc59 273 motor_state_old_L = motor_state_L;
farmookong 0:5e356103dcc7 274 //////////////////////////////////
farmookong 0:5e356103dcc7 275 }
farmookong 0:5e356103dcc7 276 //****************************************************************************** End of CN_interrupt
farmookong 0:5e356103dcc7 277
farmookong 0:5e356103dcc7 278 //****************************************************************************** init_timer
farmookong 0:5e356103dcc7 279 void init_timer()
farmookong 0:5e356103dcc7 280 {
farmookong 0:5e356103dcc7 281 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
farmookong 0:5e356103dcc7 282 }
farmookong 0:5e356103dcc7 283 //****************************************************************************** End of init_timer
farmookong 0:5e356103dcc7 284
farmookong 0:5e356103dcc7 285 //****************************************************************************** init_PWM
farmookong 0:5e356103dcc7 286 void init_PWM()
farmookong 0:5e356103dcc7 287 {
farmookong 0:5e356103dcc7 288 pwm1.period_us(50);
farmookong 0:5e356103dcc7 289 pwm1.write(0.5);
farmookong 0:5e356103dcc7 290 TIM1->CCER |= 0x4;
farmookong 0:5e356103dcc7 291
farmookong 0:5e356103dcc7 292 pwm2.period_us(50);
farmookong 0:5e356103dcc7 293 pwm2.write(0.5);
farmookong 0:5e356103dcc7 294 TIM1->CCER |= 0x40;
farmookong 0:5e356103dcc7 295
farmookong 0:5e356103dcc7 296 servo.period_ms(Servo_Period);
farmookong 0:5e356103dcc7 297 servo.write(servo_duty);
farmookong 0:5e356103dcc7 298 }
farmookong 0:5e356103dcc7 299 //****************************************************************************** End of init_PWM
farmookong 0:5e356103dcc7 300
farmookong 0:5e356103dcc7 301 //****************************************************************************** init_CN
farmookong 0:5e356103dcc7 302 void init_CN()
farmookong 0:5e356103dcc7 303 {
KCHuang 1:d24c3384bc59 304 // Motor Right
KCHuang 1:d24c3384bc59 305 HallA_R.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 306 HallA_R.fall(&CN_interrupt);
KCHuang 1:d24c3384bc59 307 HallB_R.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 308 HallB_R.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 309
KCHuang 1:d24c3384bc59 310 HallA_state_R = HallA_R.read();
KCHuang 1:d24c3384bc59 311 HallB_state_R = HallB_R.read();
farmookong 0:5e356103dcc7 312
KCHuang 1:d24c3384bc59 313 // Motor Left
KCHuang 1:d24c3384bc59 314 HallA_L.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 315 HallA_L.fall(&CN_interrupt);
KCHuang 1:d24c3384bc59 316 HallB_L.rise(&CN_interrupt);
KCHuang 1:d24c3384bc59 317 HallB_L.fall(&CN_interrupt);
farmookong 0:5e356103dcc7 318
KCHuang 1:d24c3384bc59 319 HallA_state_L = HallA_L.read();
KCHuang 1:d24c3384bc59 320 HallB_state_L = HallB_L.read();
farmookong 0:5e356103dcc7 321 }
KCHuang 1:d24c3384bc59 322 //****************************************************************************** End of init_CN