cesar.alejandro
/
oscillation_ctrl
1
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

Fixed issues/bugs from branch merge

This commit is contained in:
cesar 2022-04-18 17:24:08 -03:00
parent 309a33c08d
commit 71f7574383
4 changed files with 55 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
launch/klausen_dampen.launch
View File

@ -42,7 +42,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
type="klausen_control.py"
name="klausenCtrl_node"
output="screen"
/>
<node
pkg="oscillation_ctrl"

2
src/klausen_control.py
View File

@ -316,7 +316,7 @@ class Main:
def publisher(self,pub_time):
self.control()
self.pub_q.publish(self.quaternion)
# rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
#rospy.loginfo('roll: %.2f\npitch: %.2f\n',self.EulerAng[0],self.EulerAng[1])
# --------------------------------------------------------------------------------#
# ALGORITHM

32
src/path_follow.cpp
View File

@ -136,7 +136,15 @@ int main(int argc, char **argv)
rate.sleep();
}
if (current_state.connected){
ROS_INFO("Connected to FCU");
} else {
ROS_INFO("Never Connected");
}
/*********** Initiate variables ************************/
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
ros::Rate rate_pub(75.0);
// Needed for attitude command
mavros_msgs::AttitudeTarget attitude;
@ -177,6 +185,7 @@ int main(int argc, char **argv)
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(buff_pose);
ros::spinOnce();
ROS_INFO("Publishing position setpoints");
rate.sleep();
}
@ -199,20 +208,15 @@ int main(int argc, char **argv)
eulAng.commanded = K_ang;
eulAng.desired = J_ang;
if(current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
} else {
ROS_INFO("Could not enter offboard mode");
}
last_request = ros::Time::now();
//last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(3.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
if( arming_client.call(arm_cmd) && arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
@ -229,17 +233,17 @@ int main(int argc, char **argv)
attitude.thrust = thrust.thrust;
// Determine which messages to send
if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){
if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
attitude.orientation = q_des;
attitude.header.stamp = ros::Time::now();
att_targ.publish(attitude);
att_cmds = true;
ROS_INFO("Attitude Ctrl");
ROS_INFO("ATTITUDE CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
} else {
local_pos_pub.publish(buff_pose);
ROS_INFO("Position Ctrl");
ROS_INFO("POSITION CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude);
}
@ -248,7 +252,7 @@ int main(int argc, char **argv)
euler_pub.publish(eulAng);
ros::spinOnce();
rate.sleep();
rate_pub.sleep();
}
}

24
src/ref_signalGen.py
View File

@ -324,12 +324,12 @@ class Main:
# SOLVE ODE (get ref pos, vel, accel)
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
#self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
for i in range(1,len(self.y0)):
self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i])
self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
#self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i])
for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
@ -338,11 +338,11 @@ class Main:
if self.SF_idx < len(self.SF_delay_x[0]):
self.EPS_I[3*j] = self.x[1,j]
self.EPS_I[3*j+1] = self.y[1,j]
self.EPS_I[3*j+2] = self.z[1,j]
#self.EPS_I[3*j+2] = self.z[1,j]
else:
self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x)
self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y)
self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim
#self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim
self.delay(1,feedBack) # Detemine feedback array
@ -358,18 +358,22 @@ class Main:
self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
self.ref_sig.position.x = self.EPS_F[0]
self.ref_sig.position.y = self.EPS_F[1]
#self.ref_sig.position.z = self.EPS_F[2]
self.ref_sig.position.z = 5.0
self.ref_sig.velocity.x = self.EPS_F[3]
self.ref_sig.velocity.y = self.EPS_F[4]
self.ref_sig.velocity.z = self.EPS_F[5]
self.ref_sig.acceleration.x = self.EPS_F[6]
self.ref_sig.acceleration.y = self.EPS_F[7]
self.ref_sig.acceleration.z = self.EPS_F[8]
# Do not need to evaluate z
self.ref_sig.position.z = 5.0
self.ref_sig.velocity.z = 0
self.ref_sig.acceleration.z = 0
#self.ref_sig.position.z = self.EPS_F[2]
#self.ref_sig.velocity.z = self.EPS_F[5]
#self.ref_sig.acceleration.z = self.EPS_F[8]
self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]]
self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
#self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]]
self.SF_idx += 1
self.FB_idx += 1
@ -410,7 +414,7 @@ class Main:
self.pub_ref.publish(self.ref_sig)
# Give user feedback on published message:
self.screen_output()
#self.screen_output()
if __name__=="__main__":