![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
carbase
Dependencies: mbed mbed-rtos ros_lib_melodic
Diff: main.cpp
- Revision:
- 5:e9a3ee77cb97
- Parent:
- 4:aa8ef06b9469
- Child:
- 6:66de1f4abff4
diff -r aa8ef06b9469 -r e9a3ee77cb97 main.cpp --- a/main.cpp Sat Jan 30 12:05:58 2021 +0000 +++ b/main.cpp Thu Feb 04 08:40:48 2021 +0000 @@ -43,18 +43,21 @@ } void motorMove(int motor_index, double pwm){ - if(pwm == 0.5) - motor_stop[motor_index] = 1; - else - motor_stop[motor_index] = 0; + motor_stop[motor_index] = 0; motor_pwm[motor_index] = pwm; } - +void motorStop(int motor_index){ + motor_stop[motor_index] = 1; + motor_pwm[motor_index] = 0.5; +} void baseCallback(const std_msgs::Float32MultiArray& base_msg){ OmniWheel omni_wheel_data = motor_drive.getOmniWheelData(base_msg.data[1], base_msg.data[0], base_msg.data[2]); vector<double> omni_wheel_pwm = omni_wheel_data.getPwm(); for(int i = 0; i < motor_num; i++){ - motorMove(i, omni_wheel_pwm[i]); + if(omni_wheel_pwm[i]==0.5) + motorStop(i); + else + motorMove(i, omni_wheel_pwm[i]); } } @@ -70,7 +73,6 @@ nh.subscribe(subax); nh.subscribe(subbt); motor_init(); - while (1) { nh.spinOnce();