SHARE
TWEET

Untitled

a guest May 19th, 2019 76 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <bits/stdc++.h>
  2. using namespace std;
  3.  
  4. struct vector3d{
  5.    double x;
  6.    double y;
  7.    double z;
  8. };
  9.  
  10. struct vector4d{
  11.    double x;
  12.    double y;
  13.    double z;
  14.    double w;
  15. };
  16.  
  17. class QuadController{
  18.    
  19.     private:
  20.    
  21.     class controller {
  22.         private:
  23.        
  24.         float Kp;
  25.         float Ki;
  26.         float Kd;
  27.        
  28.         float error=0;
  29.         float previousError=0;
  30.         float integral=0;
  31.         float derivative=0;
  32.  
  33.         int last_time=0;
  34.        
  35.         public:
  36.        
  37.         float current=0;
  38.         float target=0;
  39.         float output=0;
  40.  
  41.         controller(float Kp,float Ki,float Kd) : Kp(Kp), Ki(Ki), Kd(Kd) {}
  42.        
  43.         void calc(){
  44.             int current_time = 0;//[GET_TIME]();
  45.             int dt=0;
  46.             if (last_time!=0)
  47.                 dt = current_time - last_time;
  48.             last_time=current_time;
  49.             error = (target - current)/3.1415/2; //normalize: 2pi error => 1
  50.             integral = integral + error*dt;
  51.             derivative = (error - previousError)/dt;
  52.             output = (Kp*error + Ki*integral + Kd*derivative);
  53.             previousError = error;
  54.         }
  55.     };
  56.    
  57.     static constexpr  float steerConst=0.5;
  58.  
  59.     static constexpr  float rotKp=500;
  60.     static constexpr  float rotKi=0;
  61.     static constexpr float rotKd=100;
  62.        
  63.     static constexpr float posKp=100;
  64.     static constexpr float posKi=0;
  65.     static constexpr float posKd=80;
  66.  
  67.     //                              R  P  Y  X  Y  Z
  68.     static constexpr int coeff[4][6]= {{  1, 1,-1, 1,-1, 1}, //FR
  69.                                    { -1, 1, 1, 1, 1, 1}, //FL
  70.                                    {  1,-1, 1,-1,-1, 1}, //BR
  71.                                    { -1,-1,-1,-1, 1, 1}};//BL
  72.            
  73.     static controller rollCtl;
  74.     static controller pitchCtl;
  75.     static controller yawCtl;
  76.     static controller XCtl;
  77.     static controller YCtl;
  78.     static controller ZCtl;
  79.    
  80.     public:
  81.     QuadController(){
  82.         rollCtl = controller(rotKp,rotKi,rotKd);
  83.         pitchCtl = controller(rotKp,rotKi,rotKd);
  84.         yawCtl = controller(rotKp,rotKi,rotKd);
  85.         XCtl = controller(posKp,posKi,posKd);
  86.         YCtl = controller(posKp,posKi,posKd);
  87.         ZCtl = controller(posKp,posKi,posKd);
  88.     }
  89.    
  90.     /**
  91.      * calculate motor speeds by using an PID controllor on the inputs.
  92.      *
  93.      * This sum is the arithmetic sum, not some other kind of sum that only
  94.      * mathematicians have heard of.
  95.      *
  96.      * @param rot current orientation, Vector containing Euler-angles (x=roll,y=pitch,z=yaw, 0 to 2pi)
  97.      * @param input user steering input (x=roll,y=pitch,z=yaw,w=power, -1 to 1)
  98.      * @param pos current position, Vector
  99.      * @param targetPos position to be moved to, ignored if obmitted or NULL
  100.      *
  101.      * @return array of motor speeds.
  102.      */
  103.     int (&run(vector3d* rot,vector4d* input,vector3d* pos=NULL,vector3d* targetPos=NULL))[4]{  
  104.  
  105.         rollCtl.target=steerConst*input->x;
  106.         pitchCtl.target=steerConst*input->y;
  107.         yawCtl.target=steerConst*input->z;
  108.        
  109.         rollCtl.current=rot->x;
  110.         pitchCtl.current=rot->y;
  111.         yawCtl.current=rot->z;
  112.        
  113.         rollCtl.calc();
  114.         pitchCtl.calc();
  115.         yawCtl.calc();
  116.        
  117.         if (targetPos!=NULL){
  118.             XCtl.target=targetPos->x;
  119.             YCtl.target=targetPos->y;
  120.             ZCtl.target=targetPos->z;
  121.            
  122.             XCtl.current=pos->x;
  123.             YCtl.current=pos->y;
  124.             ZCtl.current=pos->z;
  125.            
  126.             XCtl.calc();
  127.             YCtl.calc();
  128.             ZCtl.calc();
  129.         }
  130.         static int output[]={0,0,0,0};
  131.  
  132.         for (int i=0;i<4;i++){
  133.             output[i]=50        +  coeff[i][0]*rollCtl.output + coeff[i][1]*pitchCtl.output + coeff[i][2]*yawCtl.output;
  134.             output[i]=output[i] +  coeff[i][3]*XCtl.output    + coeff[i][4]*YCtl.output     + coeff[i][5]*ZCtl.output;
  135.             output[i]=output[i]*input->w;
  136.  
  137.             if (output[i]<0)  output[i]=0;
  138.             if (output[i]>100)output[i]=100;
  139.         }
  140.         return output;
  141.     }
  142. };
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top