Commit 9e849ea7 authored by mastefan's avatar mastefan
Browse files

DroneXGUI react on Buttonpress by changing states

parent 503f4c5e
......@@ -102,6 +102,11 @@
#define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LANDING 2
#define DRONEX_STATE_APPROACH 3
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
......@@ -143,6 +148,11 @@
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
#define STATE_FLYING 3
#define STATE_LAND 4
// Namespacing the package
using namespace d_fall_pps;
......@@ -166,7 +176,12 @@ float m_time_seconds;
// > Mass of the Crazyflie quad-rotor, in [grams]
float m_mass_CF_grams;
Setpoint dronexSetpoint;
int flying_state = DRONEX_STATE_APPROACH;
//describes the area of the crazyflie and other parameters
//CrazyflieContext context;
// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
float m_mass_total_grams;
......@@ -299,7 +314,8 @@ ros::Publisher my_current_xyz_yaw_publisher;
// ROS Publisher for the current setpoint
ros::Publisher dronexSetpointToGUIPublisher;
// publisher for flying state
ros::Publisher flyingStatePublisher;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
......@@ -346,6 +362,8 @@ void perControlCycleOperations();
// CALLBACK FROM ROS MESSAGES RECEIVED
void buttonPressedCallback(const std_msgs::Int32& msg);
void dronexCoordinateCallback(const ViconData& viconData);
//void coordinatesToLocal(CrazyflieData& cf);
// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
void buttonPressed_gotoStart();
......
......@@ -3,8 +3,8 @@ mass_CF : 32
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.20 # [meters]
max_setpoint_change_per_second_vertical : 0.10 # [meters]
max_setpoint_change_per_second_horizontal : 0.10 # [meters]
max_setpoint_change_per_second_vertical : 0.20 # [meters]
max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
# Frequency of the controller, in hertz
......
......@@ -192,11 +192,12 @@ void perControlCycleOperations()
void buttonPressed_take_off(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Taking off...");
flying_state = DRONEX_STATE_TAKING_OFF;
}
void buttonPressed_land(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Landing...");
flying_state = DRONEX_STATE_LANDING;
}
......@@ -418,7 +419,68 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
m_time_ticks++;
m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
//ROS_INFO_STREAM("Dronex x: " << request.otherCrazyflies[0].x);
//dronexSetpoint = request.otherCrazyflies[0];
switch(flying_state){
case DRONEX_STATE_APPROACH:
{
ROS_INFO("DRONEX_STATE_APPROACH");
dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.2;
setpointCallback(dronexSetpoint);
}
break;
case DRONEX_STATE_GROUND:
{
ROS_INFO("DRONEX_STATE_GROUND");
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
return true;
}
break;
case DRONEX_STATE_LANDING:
{
ROS_INFO_STREAM("DRONEX_STATE_LANDING" << request.ownCrazyflie.z);
if(request.ownCrazyflie.z > 0.1){
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z - 0.01;
}else{
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = STATE_MOTORS_OFF;
flyingStatePublisher.publish(flying_state_msg);
}
return true;
}
break;
case DRONEX_STATE_TAKING_OFF:
{
ROS_INFO_STREAM("DRONEX_STATE_TAKING_OFF");
if(request.ownCrazyflie.z < 0.8){
dronexSetpoint.x = request.ownCrazyflie.x;
dronexSetpoint.y = request.ownCrazyflie.y;
dronexSetpoint.z = request.ownCrazyflie.z + 0.01;
}else{
//flying_state = DRONEX_STATE_APPROACH;
}
setpointCallback(dronexSetpoint);
break;
}
}
/*dronexSetpoint.x = request.otherCrazyflies[0].x;
dronexSetpoint.y = request.otherCrazyflies[0].y;
dronexSetpoint.z = request.otherCrazyflies[0].z + 0.3*/
//setpointCallback(dronexSetpoint);
// CALL THE FUNCTION FOR PER CYLCE OPERATIONS
perControlCycleOperations();
......@@ -998,12 +1060,30 @@ void dronexCoordinateCallback(const ViconData& viconData){
}
m_setpoint[0] = droneX.x;
/*m_setpoint[0] = droneX.x;
m_setpoint[1] = droneX.y;
m_setpoint[2] = droneX.z+0.2;
m_setpoint[3] = droneX.yaw;
m_setpoint[3] = droneX.yaw;*/
}
/*void coordinatesToLocal(CrazyflieData& cf) {
AreaBounds area = context.localArea;
float originX = (area.xmin + area.xmax) / 2.0;
float originY = (area.ymin + area.ymax) / 2.0;
// change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
float originZ = 0.0;
// float originZ = (area.zmin + area.zmax) / 2.0;
cf.x -= originX;
cf.y -= originY;
cf.z -= originZ;
}*/
// ************************************************************************************************
// PPPP U U BBBB L III SSSS H H X X Y Y ZZZZZ Y Y A W W
// P P U U B B L I S H H X X Y Y Z Y Y A A W W
......@@ -1418,7 +1498,28 @@ int main(int argc, char* argv[]) {
// Subscribe to get Coordinates of Dronex
//ros::Subscriber dronexCoordinateSubscriber = nodeHandle.subscribe("dronexCoordinate", 1, dronexCoordinateCallback);
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, dronexCoordinateCallback);
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
//ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, dronexCoordinateCallback);
// Print out some information to the user.
ROS_INFO("[DroneX CONTROLLER] Service ready :-)");
......
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