[ros-users] [Discourse.ros.org] [ROS Projects] Controller

Good evening dear community of ROS, I am a Cuban boy of 12 years who by chance discovered the simulator gazebo ros, but due to the poor communication in my country I have limited access to the Internet. Currently I set my goal to develop the simulation of a quadrator in ros, which is able to locate a mobile platform, and land on it, but I have stumbled on the first problem to achieve it, I do not know how to implement a controller that allows me to rotate the quadrator on the z-axis, that is, its yaw, from its rotation matrix.

Best regards, God bless you.

PS: I attached a part of the code, in which you can see the way I currently perform the rotation without using the rotation matrix.

//-------------- YAW CONTROL ---------------- 
            max_grad = 180.0;
            yaw_error = yaw_reference - yaw;
            u = kp_yaw*yaw_error/max_grad;

            commandPilot.angular.z = u;

            

            //------------ ALTITUDE CONTROL ---.---------

    

            altura_error = alt_ref - altitude;
            commandPilot.linear.z = kp_alt * altura_error;

                

            if(commandPilot.linear.z > 0.5)
                commandPilot.linear.z = 0.5;

        

            if(fabsf(altura_error) < 0.01 && fabsf(yaw_error) < 0.5)
            {
                state = 1;
                commandPilot.linear.x = 0;
                commandPilot.linear.y = 0;
                pose_xi = start_pose.position.x;
                pose_yi = start_pose.position.y;
                //~ ROS_INFO_STREAM("Gazebo pose: X = " << pose_xi << " Y = " << pose_yi);
            }                

            

        }else
        if(state == 1)
        {    
            putText(front_image," Dist: "+ stDist.str() + " Dist error: "+ stErrorDist.str() + " Signal: " + stSignal.str(), Point( 5, 55), FONT_HERSHEY_COMPLEX_SMALL, 0.7, CV_RGB( 255, 255, 255), 1, 1);

        

            dist_x = distancia - fabsf(pose_xi);
            dist_error = dist_x - start_pose.position.x;
            signal = kp * dist_error;

            

            commandPilot.linear.x = signal;

            

            if(fabsf(dist_x) < 0.01)
            {
                commandPilot.linear.x = 0;
                commandPilot.linear.y = 0;                
            }            
        }

void image_viewer::Ardrone_odom(const sensor_msgs::Imu::ConstPtr &odom)
{    
    odom_ARdrone_x = odom->orientation.x;
    odom_ARdrone_y = odom->orientation.y;
    odom_ARdrone_z = odom->orientation.z;
    odom_ARdrone_w = odom->orientation.w;

    

    //my code

    

    /*x0 = odom_ARdrone_x;
    y0 = odom_ARdrone_y;
    z0 = odom_ARdrone_z;
    w0 = odom_ARdrone_w;*/

    

    tf::Quaternion q1(odom_ARdrone_x,odom_ARdrone_y,odom_ARdrone_z,odom_ARdrone_w);
    tf::Matrix3x3 m(q1);

    

    //Found final position

    

    //z1 = r31x0 + r32y0 + r33z0

    
    

    m.getRPY(roll, pitch, yaw);

    roll = (roll * 180) / M_PI;
    pitch = (pitch * 180) / M_PI;
    yaw = (yaw * 180) / M_PI;    
}

---
[Visit Topic](https://discourse.ros.org/t/controller/4060/1) or reply to this email to respond.


If you do not want to receive messages from ros-users please use the unsubscribe link below. If you use the one above, you will stop all of ros-users from receiving updates.
______________________________________________________________________________
ros-users mailing list
ros-users@lists.ros.org
http://lists.ros.org/mailman/listinfo/ros-users
Unsubscribe: <http://lists.ros.org/mailman//options/ros-users>