scorpio1
/
oscillation_ctrl
forked from cesar.alejandro/oscillation_ctrl
1
0
Fork 0
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" type="klausen_control.py"
name="klausenCtrl_node" name="klausenCtrl_node"
output="screen" output="screen"
/> />
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"

2
src/klausen_control.py
View File

@ -316,7 +316,7 @@ class Main:
def publisher(self,pub_time): def publisher(self,pub_time):
self.control() self.control()
self.pub_q.publish(self.quaternion) 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 # ALGORITHM

32
src/path_follow.cpp
View File

@ -136,7 +136,15 @@ int main(int argc, char **argv)
rate.sleep(); rate.sleep();
} }
if (current_state.connected){
ROS_INFO("Connected to FCU");
} else {
ROS_INFO("Never Connected");
}
/*********** Initiate variables ************************/ /*********** 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 // Needed for attitude command
mavros_msgs::AttitudeTarget attitude; mavros_msgs::AttitudeTarget attitude;
@ -177,6 +185,7 @@ int main(int argc, char **argv)
for(int i = 100; ros::ok() && i > 0; --i){ for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
ros::spinOnce(); ros::spinOnce();
ROS_INFO("Publishing position setpoints");
rate.sleep(); rate.sleep();
} }
@ -199,20 +208,15 @@ int main(int argc, char **argv)
eulAng.commanded = K_ang; eulAng.commanded = K_ang;
eulAng.desired = J_ang; eulAng.desired = J_ang;
if(current_state.mode != "OFFBOARD" && if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
(ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
} else { } else {
ROS_INFO("Could not enter offboard mode"); ROS_INFO("Could not enter offboard mode");
} }
last_request = ros::Time::now(); //last_request = ros::Time::now();
} else { } else {
if( !current_state.armed && if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
(ros::Time::now() - last_request > ros::Duration(3.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed"); ROS_INFO("Vehicle armed");
} }
last_request = ros::Time::now(); last_request = ros::Time::now();
@ -229,17 +233,17 @@ int main(int argc, char **argv)
attitude.thrust = thrust.thrust; attitude.thrust = thrust.thrust;
// Determine which messages to send // 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.orientation = q_des;
attitude.header.stamp = ros::Time::now(); attitude.header.stamp = ros::Time::now();
att_targ.publish(attitude); att_targ.publish(attitude);
att_cmds = true; att_cmds = true;
ROS_INFO("Attitude Ctrl"); ROS_INFO("ATTITUDE CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("Cur Altitude: %.2f", current_altitude);
} else { } else {
local_pos_pub.publish(buff_pose); local_pos_pub.publish(buff_pose);
ROS_INFO("Position Ctrl"); ROS_INFO("POSITION CTRL");
ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Des Altitude: %.2f", alt_des);
ROS_INFO("Cur Altitude: %.2f", current_altitude); ROS_INFO("Cur Altitude: %.2f", current_altitude);
} }
@ -248,7 +252,7 @@ int main(int argc, char **argv)
euler_pub.publish(eulAng); euler_pub.publish(eulAng);
ros::spinOnce(); 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) # SOLVE ODE (get ref pos, vel, accel)
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) 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.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)): 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.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.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 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]): if self.SF_idx < len(self.SF_delay_x[0]):
self.EPS_I[3*j] = self.x[1,j] self.EPS_I[3*j] = self.x[1,j]
self.EPS_I[3*j+1] = self.y[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: 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] = 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+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 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.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.x = self.EPS_F[0]
self.ref_sig.position.y = self.EPS_F[1] 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.x = self.EPS_F[3]
self.ref_sig.velocity.y = self.EPS_F[4] 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.x = self.EPS_F[6]
self.ref_sig.acceleration.y = self.EPS_F[7] 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.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.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.SF_idx += 1
self.FB_idx += 1 self.FB_idx += 1
@ -410,7 +414,7 @@ class Main:
self.pub_ref.publish(self.ref_sig) self.pub_ref.publish(self.ref_sig)
# Give user feedback on published message: # Give user feedback on published message:
self.screen_output() #self.screen_output()
if __name__=="__main__": if __name__=="__main__":