main.cpp

00001 #include "mbed.h"
00002 #include "PT1_Controller.h"
00003 #include "Target_Planer.h"
00004 #include "Motion.h"
00005  
00006 using namespace  std;
00007 
00008 #define   pi 3.141592653589793
00009 
00010 Serial pc(SERIAL_TX, SERIAL_RX);
00011 
00012 double kpv = 0.6412;        // proportional controller for distance error
00013 double kpw = 0.6801;        // proportional controller for angle error
00014 double Ts = 1/30.0;         // samplingrate 30 Hz
00015 double Tf = 1/(2.0*pi*4.0); // fg = 4 Hz
00016 PT1_Controller distance_cntr(kpv, Ts, Tf);
00017 PT1_Controller angle_cntr(kpw, Ts, Tf);
00018 
00019 // (vRx (m/s), wR (rad/s)) -> (vRxRB (unit unknown), wRRB (unit unknown))
00020 // Cu2uRB = 2.8070         0
00021 //               0    0.9263
00022 double kv2RB = 2.8070; // maps desired forward velocity from m/s 2 RB unit
00023 double kw2RB = 0.9263; // maps desired turn speed from rad/s 2 RB unit
00024 
00025 Timer timer; // timer for time measurement
00026 float dt = 0.0f;
00027 
00028 uint32_t i;
00029 
00030 // user defined functions
00031 double quat2psi(double qw, double qx, double qy, double qz);
00032 void   quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi);
00033 double getShortRotation(double ang);
00034 
00035 const int N = 40;
00036 double Pathx[N] = {7.3000, 7.2500, 7.2000, 7.1500, 7.1000, 7.0500, 7.0000, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500, 6.2000, 6.1500, 6.1000, 6.0500, 6.0000, 5.9500, 5.9000, 5.8500, 5.8000, 5.7500, 5.7000, 5.6500, 5.6000, 5.5500, 5.5000, 5.4500, 5.4000, 5.3500};
00037 double Pathy[N] = {6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500};
00038 double R = 0.116959064327485;
00039 Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
00040 double RobotPosx = 4.35;
00041 double RobotPosy = 6.19;
00042 double TargetPosx = -999.0;
00043 double TargetPosy = -999.0;
00044 double TargetAng  = -999.0;
00045 
00046 double pos0   = 179.0*pi/180.0;
00047 float  vel0   = 0.0f*(float)pi/180.0f;
00048 float  velMax = 90.0f*(float)pi/180.0f;
00049 float  accMax = 60.0f*(float)pi/180.0f;
00050 Motion motion_planer(pos0, vel0, velMax, accMax);
00051 double targetPos = 0*pi/180.0;
00052 float  targetVel = 0*(float)pi/180.0f;
00053 
00054 int main()
00055 {
00056     pc.baud(2000000);
00057 
00058     timer.start();
00059 
00060     i = 0;
00061     
00062     // reset internal filter storage to zero
00063     distance_cntr.reset();
00064     angle_cntr.reset();
00065         
00066     while(1) {
00067         
00068         dt = timer.read();
00069         timer.reset();
00070         
00071         target_planer.initialize(RobotPosx, RobotPosy);
00072                 
00073         /*
00074         double phiR = quat2psi(double qw, double qx, double qy, double qz); // transform actual robot orientation (quaternioni) into turning angle
00075         // it is assumed that target_ang and phiR both lie between -pi and pi
00076         double e_ang = getShortRotation(target_angle - phiR);
00077         // update angle controller
00078         */
00079         double qw = 0.984005578673161;
00080         double qx = 0.009063618716137;
00081         double qy = 0.177899336159591;
00082         double qz = 0.001629344754243;
00083         double phi, theta, psi;
00084         quat2rpy(qw, qx, qy, qz, &phi, &theta, &psi);
00085         if(i++ == 0) {
00086             pc.printf("%.15f; %.15f; %.15f; \r\n", phi, theta, psi);
00087         }
00088         
00089         
00090         /*
00091         if(i < 200) {
00092             // pc.printf("%i; %.12f; %.12f; %0.12f; %.6f;\r\n", i, quat2psi(qw, qx, qy, qz), qx*qy + qw*qz, 0.5 - (qy*qy + qz*qz), dt);
00093             // pc.printf("%i; %i; %.6f;\r\n", i, target_planer.returnPathLength(), dt);
00094             // pc.printf("%i; %.12f; %.12f; %i; %i; %.6f;\r\n", i, target_planer.returnPathxAtIndex(i), target_planer.returnPathyAtIndex(i), target_planer.returnPathLength(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
00095             // pc.printf("%i; %.12f; %.12f;;\r\n", i, TargetPosx, TargetPosy);
00096             // target_planer.update(RobotPosx, RobotPosx);
00097             // target_planer.readTargetPos(&TargetPosx, &TargetPosy);
00098             // pc.printf("%i; %.12f; %.12f; %.12f; %i; %i; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, target_planer.return_i_min_(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
00099             // target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
00100             
00101             pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f;\r\n", i, motion_planer.getTimeToPosition(targetPos), motion_planer.getDistanceToStop(), motion_planer.getPosition(), motion_planer.getVelocity(), motion_planer.getAcceleration(), dt);
00102             motion_planer.incrementToPosition(targetPos, Ts);
00103         }
00104         
00105         i++;        
00106         wait_us(33333);
00107         */
00108     }
00109 }
00110 
00111 double quat2psi(double qw, double qx, double qy, double qz)
00112 {
00113     double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);    
00114     return atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
00115 }
00116 
00117 void quat2rpy(double qw, double qx, double qy, double qz, double *phi, double *theta, double *psi)
00118 {
00119     double s = 1/(qw*qw + qx*qx + qy*qy + qz*qz);
00120     
00121     *phi = atan2(s*(qy*qz + qw*qx), 0.5 - s*(qx*qx + qy*qy));
00122     
00123     *theta = -2.0*s*(qx*qz - qw*qy);
00124     if(*theta >  1.0) *theta =  1.0;
00125     if(*theta < -1.0) *theta = -1.0;
00126     *theta = asin(*theta);
00127     
00128     *psi = atan2(s*(qx*qy + qw*qz), 0.5 - s*(qy*qy + qz*qz));
00129     
00130     return;
00131 }
00132 
00133 double getShortRotation(double ang)
00134 {   
00135     // we maybe need to introduce a threshold in case the angle is noisy and around +/-pi (180° rotation)
00136     return atan2(sin(ang), cos(ang));
00137 }
00138