NTHU機器人學 Team3 / Mbed 2 deprecated ROS_remote_car_2

0

Dependencies:   mbed ros_lib_indigo

Fork of ROS_remote_car by NTHU機器人學 Team3

main.cpp@2:a6604a2563df, 2018-06-15 (annotated)

Committer:
KCHuang
Date:
Fri Jun 15 02:19:26 2018 +0000
Revision:
2:a6604a2563df
Parent:
1:d24c3384bc59 
77777

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