Commit e5751aee authored by mastefan's avatar mastefan
Browse files

VM: some minor changes, trajectory not yet working

parent 781f9d94
...@@ -391,7 +391,7 @@ ...@@ -391,7 +391,7 @@
</font> </font>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>0</number> <number>1</number>
</property> </property>
<property name="usesScrollButtons"> <property name="usesScrollButtons">
<bool>true</bool> <bool>true</bool>
......
...@@ -15,7 +15,7 @@ vicon_frequency : 200 ...@@ -15,7 +15,7 @@ vicon_frequency : 200
# controller_mode : 0: Basic Controller # controller_mode : 0: Basic Controller
# controller_mode : 1: Angle Controller # controller_mode : 1: Angle Controller
# controller_mode : 2: Trajectory Controller # controller_mode : 2: Trajectory Controller
controller_mode : 1 controller_mode : 2
...@@ -109,5 +109,5 @@ gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00] ...@@ -109,5 +109,5 @@ gainMatrixPitchRatefromAngle : [ 0.00, 4.00, 0.00]
gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30] gainMatrixYawRatefromAngle : [ 0.00, 0.00, 2.30]
# LQR: get baseline Angle from Velocity for feedforward (roll, pitch) # LQR: get baseline Angle from Velocity for feedforward (roll, pitch) [-20°,20°]=[-0.34906585, 0.34906585]
gainFeedforwardAnglefromVelocity: [-(20.0 * DEG2RAD), (20.0 * DEG2RAD)] gainFeedforwardAnglefromVelocity: [-0.34906585, 0.34906585]
\ No newline at end of file \ No newline at end of file
...@@ -237,7 +237,7 @@ void buttonPressed_follow_trajectory(){ ...@@ -237,7 +237,7 @@ void buttonPressed_follow_trajectory(){
// Initialize trajectory timers // Initialize trajectory timers
flightSequence = SEQUENCE_NONE; flightSequence = SEQUENCE_NONE;
flying_state = DRONEX_FOLLOW_TRAJECTORY; flying_state = DRONEX_STATE_FOLLOWING_TRAJECTORY;
total_time_since_start = 0; total_time_since_start = 0;
trajectory_x_radius = 0; trajectory_x_radius = 0;
trajectory_y_radius = 0; trajectory_y_radius = 0;
...@@ -536,8 +536,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -536,8 +536,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
} }
break; break;
case DRONEX_FOLLOW_TRAJECTORY: case DRONEX_STATE_FOLLOWING_TRAJECTORY:
if(abs(request.ownCrazyflie.x-request.otherCrazyflies[0].x) < tol_land[0] &&
abs(request.ownCrazyflie.y-request.otherCrazyflies[0].y) < tol_land[1] &&
abs(request.ownCrazyflie.z-request.otherCrazyflies[0].z + 0.2) < tol_land[2] ){
flying_state = DRONEX_STATE_LAND_ON_MOTHERSHIP;
}
break; break;
...@@ -891,7 +896,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -891,7 +896,7 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// x dot, y dot, z dot // x dot, y dot, z dot
float m_velocity_for_controller_2[3]; float m_velocity_for_controller_2[3];
if(flying_state == DRONEX_FOLLOW_TRAJECTORY){ if(flying_state == DRONEX_STATE_FOLLOWING_TRAJECTORY){
calculateTrajectory(request); calculateTrajectory(request);
...@@ -1106,8 +1111,6 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp ...@@ -1106,8 +1111,6 @@ void calculateControlOutputDroneX(Controller::Request &request, Controller::Resp
// Gives back computes trajectory_setpoint and trajectory_velocity // Gives back computes trajectory_setpoint and trajectory_velocity
void calculateTrajectory(Controller::Request &request){ void calculateTrajectory(Controller::Request &request){
trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4;
total_time_since_start = ros::Time::now().toSec() - trajectory_t0; total_time_since_start = ros::Time::now().toSec() - trajectory_t0;
trajectory_setpoint[0] = trajectory_to_ms(request, total_time_since_start)[0]; trajectory_setpoint[0] = trajectory_to_ms(request, total_time_since_start)[0];
...@@ -1116,22 +1119,22 @@ void calculateTrajectory(Controller::Request &request){ ...@@ -1116,22 +1119,22 @@ void calculateTrajectory(Controller::Request &request){
trajectory_setpoint[3] = 0; trajectory_setpoint[3] = 0;
// just some deltaT to look ahead // just some deltaT to look ahead
float deltaT = 0.01; float deltaT = 1.0/m_vicon_frequency;
trajectory_velocity[0] = (trajectory_to_ms(request,total_time_since_start+deltaT)[0]-request.ownCrazyflie.x)/deltaT; trajectory_velocity[0] = (trajectory_to_ms(request, total_time_since_start + deltaT)[0]-request.ownCrazyflie.x)/deltaT;
trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start+deltaT)[1]-request.ownCrazyflie.y)/deltaT; trajectory_velocity[1] = (trajectory_to_ms(request, total_time_since_start + deltaT)[1]-request.ownCrazyflie.y)/deltaT;
trajectory_velocity[2] = 0; trajectory_velocity[2] = 0;
/* // clip if velocity is too big
trajectory_setpoint[0] = originX + cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius); float tot_velocity_xy = sqrt(abs(pow(trajectory_velocity[0],2))+abs(pow(trajectory_velocity[1],2)));
trajectory_setpoint[1] = originY + sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius); if(tot_velocity_xy > 1.0){
trajectory_setpoint[2] = 0.5; trajectory_velocity[0] = 1.0 * trajectory_velocity[0]/tot_velocity_xy;
trajectory_setpoint[3] = 0; trajectory_velocity[1] = 1.0 * trajectory_velocity[1]/tot_velocity_xy;
*/ }
/* if(trajectory_velocity[3]<-0.8){
trajectory_velocity[0] = -2*PI/trajectory_period * sin(2*PI/trajectory_period * total_time_since_start)*(trajectory_x_radius); trajectory_velocity[3] = -0.8;
trajectory_velocity[1] = 2*PI/trajectory_period * cos(2*PI/trajectory_period * total_time_since_start)*(trajectory_y_radius); }else if(trajectory_velocity[3]>0.8){
trajectory_velocity[2] = 0; trajectory_velocity[3] = 0.8;
*/ }
} }
...@@ -1211,7 +1214,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1211,7 +1214,7 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
// Calculate the x,y,z position depending on where the CF should be at a certain time t // Calculate the x,y,z position depending on where the CF should be at a certain time t
if(trajectory_t0 < t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1 if(trajectory_t0 <= t && t <= trajectory_t1){ // Calculate position on line between xcf0 and xm1
for(int i = 0; i < 3; i++){ for(int i = 0; i < 3; i++){
trajectory_return[i] = xcf0[i] + t*trajectory_velocity_of_CF*(xm1[i] - xcf0[i]); trajectory_return[i] = xcf0[i] + t*trajectory_velocity_of_CF*(xm1[i] - xcf0[i]);
} }
...@@ -1224,22 +1227,21 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){ ...@@ -1224,22 +1227,21 @@ std::vector<float> trajectory_to_ms(Controller::Request &request, double t){
trajectory_return[i] = xm2[i] + (t - trajectory_t2)*trajectory_velocity_of_CF*(xms[i] - xm2[i]); trajectory_return[i] = xm2[i] + (t - trajectory_t2)*trajectory_velocity_of_CF*(xms[i] - xm2[i]);
} }
}else{ }else{
trajectory_return[0] = -1; trajectory_return[0] = 0;
trajectory_return[1] = -1; trajectory_return[1] = 0;
trajectory_return[2] = -1; trajectory_return[2] = 0.3;
} }
/*
/*trajectory_x_radius = 1.0; trajectory_x_radius = 1.0;
trajectory_y_radius = 0.4; trajectory_y_radius = 0.4;
//total_time_since_start = ros::Time::now().toSec() - trajectory_start_time;
trajectory_return[0] = originX + cos(2*PI/trajectory_period * t)*(trajectory_x_radius); trajectory_return[0] = originX + cos(2*PI/trajectory_period * t)*(trajectory_x_radius);
trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius); trajectory_return[1] = originY + sin(2*PI/trajectory_period * t)*(trajectory_y_radius);
trajectory_return[2] = 0.5;*/ trajectory_return[2] = 0.5;
*/
return trajectory_return; return trajectory_return;
} }
...@@ -1612,9 +1614,9 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle ...@@ -1612,9 +1614,9 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
debugMsg.value_2 = mothership_vel[1]; debugMsg.value_2 = mothership_vel[1];
debugMsg.value_3 = mothership_vel[2]; debugMsg.value_3 = mothership_vel[2];
debugMsg.value_4 = thrust_factor; debugMsg.value_4 = thrust_factor;
debugMsg.value_5 = dronexSetpoint.x; debugMsg.value_5 = trajectory_setpoint[0];
debugMsg.value_6 = dronexSetpoint.y; debugMsg.value_6 = trajectory_setpoint[1];
debugMsg.value_7 = dronexSetpoint.z; debugMsg.value_7 = trajectory_setpoint[2];
// ...................... // ......................
// debugMsg.value_10 = your_variable_name; // debugMsg.value_10 = your_variable_name;
......
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