To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit e5751aee authored by mastefan's avatar mastefan
Browse files

VM: some minor changes, trajectory not yet working

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