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();
}
}
