Commit ccd2a769 authored by phfriedl's avatar phfriedl
Browse files

some controller improvements (not flying yet, but moving better..)

parent 8f1fd494
......@@ -65,8 +65,8 @@ void ppsClientToController(ViconData data, bool autocontrolOn){
Controller srvRate;
Setpoint goalLocation;
goalLocation.x = -5; //testvalue
goalLocation.y = 250; //testvalue
goalLocation.x = 90; //testvalue
goalLocation.y = 180; //testvalue
goalLocation.z = 300; //testvalue
srvRate.request.crazyflieLocation = data;
......@@ -120,12 +120,14 @@ bool safetyCheck(ViconData data){
//is called upon every new arrival of data in main
void viconCallback(const ViconData& data){
ROS_INFO("in viconCallback");
//ROS_INFO("in viconCallback");
//ROS_INFO_STREAM(data);
//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
if(data.crazyflieName == cflie){
ROS_INFO("in viconCallback");
ROS_INFO_STREAM(data);
//forward data to safety check
bool autocontrolOn = safetyCheck(data);
//ROS_INFO_STREAM("autocontrolOn: " << autocontrolOn);
......
......@@ -32,16 +32,18 @@ const float a1=1.032633e-6;
const float a0=5.484560e-4;
const float saturationThrust = a2*12000*12000 + a1*12000 + a0;
const float feedforwardm1 = 37000;
const float feedforwardm2 = 37000;
const float feedforwardm3 = 37000;
const float feedforwardm4 = 37000;
void ffSetup(ControlCommand prevCommands)
{
ffThrust1 = a2*prevCommands.motorCmd1*prevCommands.motorCmd1+
a1*prevCommands.motorCmd1+a0;
ffThrust2 = a2*prevCommands.motorCmd2*prevCommands.motorCmd2+
a1*prevCommands.motorCmd2 + a0;
ffThrust3 = a2*prevCommands.motorCmd3*prevCommands.motorCmd3+
a1*prevCommands.motorCmd3 + a0;
ffThrust4 = a2*prevCommands.motorCmd4*prevCommands.motorCmd4+
a1*prevCommands.motorCmd4 + a0;
ffThrust1 = a2*feedforwardm1*feedforwardm1+a1*feedforwardm1+a0;
ffThrust2 = a2*feedforwardm2*feedforwardm2+a1*feedforwardm2+a0;
ffThrust3 = a2*feedforwardm3*feedforwardm3+a1*feedforwardm3+a0;
ffThrust4 = a2*feedforwardm4*feedforwardm4+a1*feedforwardm4+a0;
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
......@@ -50,7 +52,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//Philipp: I have put this here, because in the first call, we wouldnt have previousLocation initialized
//save previous data for calculating velocities in next step
previousLocation = request.crazyflieLocation;
//previousLocation = request.crazyflieLocation;
ViconData vicon = request.crazyflieLocation;
Setpoint goal = request.setpoint;
......@@ -69,19 +71,27 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
const float k[] = {
0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0,
1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0,
0, 0, 0, 0, 0, 0, 0, 0, 3.843099534,
0, 0, 0, 0, 0, 0, 0, 0, 2.843099534,
0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0
};
float px = request.crazyflieLocation.x - request.setpoint.x;
float py = request.crazyflieLocation.y - request.setpoint.y;
float py = -(request.crazyflieLocation.y - request.setpoint.y);
float pz = request.crazyflieLocation.z - request.setpoint.z;
//linear approximation of derivative of position
float vx = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
float vy = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
float vy = -((request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime);
float vz = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
ROS_INFO_STREAM("px: " << px);
ROS_INFO_STREAM("py: " << py);
ROS_INFO_STREAM("pz: " << pz);
ROS_INFO_STREAM("vx: " << vx);
ROS_INFO_STREAM("vy: " << vy);
ROS_INFO_STREAM("vz: " << vz);
float roll = request.crazyflieLocation.roll;
float pitch = request.crazyflieLocation.pitch;
float yaw = request.crazyflieLocation.yaw - request.setpoint.yaw;
......@@ -105,6 +115,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
previousCommand = response.controlOutput;
response.controlOutput.onboardControllerType = 0; //RATE
previousLocation = request.crazyflieLocation;
return true;
}
......
......@@ -118,9 +118,11 @@ int main(int argc, char* argv[]) {
Output_GetSegmentGlobalTranslation outputTranslation =
client.GetSegmentGlobalTranslation(subjectName, segmentName);
ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
Output_GetSegmentGlobalRotationQuaternion outputRotation =
client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
//calculate position and rotation of Crazyflie
......
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