void CoaxVisionControl::controlPublisher(size_t rate) { ros::Rate loop_rate(rate); while(ros::ok()) { if (rotorReady()) { if(stage==1) { set_hover(); stabilizationControl(); } if(stage==2) { set_localize(); stabilizationControl(); } } setRawControl(motor1_des,motor2_des,servo1_des,servo2_des); ros::spinOnce(); loop_rate.sleep(); } }
void CoaxSimpleControl::controlPublisher(size_t rate) { ros::Rate loop_rate(rate); double sum_Yaw_desired = 0; double sum_Gx_desired = 0; // global "Vicon" x double sum_Gy_desired = 0; // global "Vicon" y // The continuous while loop. i.e., keeps calling rotorReady while(ros::ok()) { if ((init_count<100)) { sum_Yaw_desired = 0; sum_Gx_desired = sum_Gx_desired + coax_global_x; sum_Gy_desired = sum_Gy_desired + coax_global_y; init_count ++; } else if (!INIT_DESIRE) { yaw_des = sum_Yaw_desired / 100; Gx_des = sum_Gx_desired / 100; Gy_des = sum_Gy_desired / 100; ROS_INFO("Initiated Desired Yaw %f",yaw_des); ROS_INFO("Current Battery Voltage %f",battery_voltage); sum_Yaw_desired = 0; sum_Gx_desired = 0; sum_Gy_desired = 0; INIT_DESIRE = true; ROS_INFO("INIT_DESIRE = true"); } if (INIT_DESIRE && rotorReady()) { stabilizationControl(); } else { if (mil_count == 500 || mil_count == 1000 || mil_count == 1500 || mil_count == 2000 ) { // ROS_INFO("NO Stab Control"); } } mil_count++; if (mil_count >= 2000) { mil_count = 0; } if (mil_count == 1000) { ROS_INFO("mil_count: %i ", mil_count); ROS_INFO("coax_global_x %f. from tf", coax_global_x); ROS_INFO("Publishing Raw Control"); ROS_INFO("motor 2 commanded: %f ", motor2_des); servo1_des = servo1_des*-1; }; setRawControl(motor1_des,motor2_des,servo1_des,servo2_des); // setRawControl(0,0,servo1_des,0); ros::spinOnce(); loop_rate.sleep(); } }