Commit c56441de authored by bucyril's avatar bucyril
Browse files

corrected circle controller, made crazyflie crash safely

parent 6b6ece5f
......@@ -216,6 +216,7 @@ add_executable(PPSClient src/PPSClient.cpp)
add_executable(SafeControllerService src/SafeControllerService.cpp)
add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(CircleControllerService src/CircleControllerService.cpp)
......@@ -248,6 +249,7 @@ add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TA
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies
......@@ -281,6 +283,8 @@ target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
# GUI -- link libraries
target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
......
......@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [-0.5, 0.0, 0.4, 1.2]
defaultSetpoint: [0.0, 0.0, 0.0, 0.0]
......@@ -51,9 +51,9 @@ float saturationThrust;
CrazyflieData previousLocation;
//circle stuff
float time;
const float OMEGA = 0.1*2*PI;
const float RADIUS = 0.2;
float currentTime;
const float OMEGA = 0.5*2*PI;
const float RADIUS = 0.35;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
......@@ -141,17 +141,17 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
}
void calculateCircle(Setpoint &circlePoint){
circlePoint.x = RADIUS*cos(OMEGA*time);
circlePoint.y = RADIUS*sin(OMEGA*time);
circlePoint.x = RADIUS*cos(OMEGA*currentTime);
circlePoint.y = RADIUS*sin(OMEGA*currentTime);
circlePoint.z = 0.5;
circlePoint.yaw = OMEGA*time;
circlePoint.yaw = OMEGA*currentTime;
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie;
time += request.ownCrazyflie.acquiringTime;
currentTime += request.ownCrazyflie.acquiringTime;
Setpoint circlePoint;
calculateCircle(circlePoint);
......@@ -212,7 +212,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
int main(int argc, char* argv[]) {
ros::init(argc, argv, "CircleControllerService");
time = 0;
currentTime = 0;
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
......
......@@ -125,7 +125,7 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR_STREAM("custom controller name: valid: " << customController.getService());
usingSafeController = true;
} else {
usingSafeController = safetyCheck(global, controllerCall.response.controlOutput);
usingSafeController = !safetyCheck(global, controllerCall.response.controlOutput);
if(usingSafeController) {
ROS_INFO_STREAM("safety check failed, switching to safe controller");
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment