kurt.cc
Go to the documentation of this file.
00001 #include <ros/console.h>
00002 
00003 #include <algorithm>
00004 #include <cmath>
00005 #include <cstdio>
00006 
00007 #include <net/if.h>
00008 #include <sys/ioctl.h>
00009 #include <linux/can.h>
00010 
00011 #include "comm.h"
00012 #include "kurt.h"
00013 
00014 Kurt::~Kurt()
00015 {
00016   if(use_rotunit_)
00017     can_rotunit_send(0.0);
00018   k_hard_stop();
00019   if(!use_microcontroller_)
00020   {
00021     free(pwm_v_l_);
00022     free(pwm_v_r_);
00023   }
00024 }
00025 
00026 bool Kurt::setPWMData(const std::string &speedPwmLeerlaufTable, double feedforward_turn, double ki, double kp)
00027 {
00028   ki_l = ki_r = ki;
00029   kp_l = kp_r = kp;
00030 
00031   int nr;
00032   double *v_pwm_l, *v_pwm_r;
00033   if (!read_speed_to_pwm_leerlauf_tabelle(speedPwmLeerlaufTable, &nr, &v_pwm_l, &v_pwm_r))
00034   {
00035     return false;
00036   }
00037   make_pwm_v_tab(nr, v_pwm_l, v_pwm_r, nr_v_, &pwm_v_l_, &pwm_v_r_, &vmax_);
00038   free(v_pwm_l);
00039   free(v_pwm_r);
00040   use_microcontroller_ = false;
00041 
00042   return true;
00043 }
00044 
00045 int Kurt::can_motor(int left_pwm,  char left_dir,  char left_brake,
00046     int right_pwm, char right_dir, char right_brake)
00047 {
00048   char left_dir_brake =  (left_dir << 1) + left_brake;
00049   char right_dir_brake = (right_dir << 1) + right_brake;
00050 
00051   can_frame frame;
00052   frame.can_id = CAN_CONTROL;
00053   frame.can_dlc = 8;
00054   frame.data[0] = RAW >> 8;
00055   frame.data[1] = RAW;
00056   frame.data[2] = (left_dir_brake);
00057   frame.data[3] = (left_pwm >> 8);
00058   frame.data[4] = (left_pwm);
00059   frame.data[5] = (right_dir_brake);
00060   frame.data[6] = (right_pwm >> 8);
00061   frame.data[7] = (right_pwm);
00062 
00063   if (!can_.send_frame(&frame))
00064   {
00065     ROS_ERROR("can_motor: Error sending PWM data");
00066     return 1;
00067   }
00068   return 0;
00069 }
00070 
00071 void Kurt::k_hard_stop(void)
00072 {
00073   unsigned short pwm_left, pwm_right;
00074   unsigned char dir_left, dir_right, brake_left, brake_right;
00075   pwm_left = 1023;
00076   dir_left = 0;
00077   brake_left = 1;
00078   pwm_right = 1023;
00079   dir_right = 0;
00080   brake_right = 1;
00081 
00082   do
00083   { }
00084   while (!can_motor(pwm_left, dir_left, brake_left, pwm_right, dir_right, brake_right));
00085 }
00086 
00087 // PWM Lookup
00088 // basis zuordnung
00089 // darueber liegt ein PID geschwindigkeits regler
00090 // Leerlauf adaption fuer den Teppich boden und geradeaus fahrt
00091 void Kurt::set_wheel_speed1(double v_l, double v_r, int integration_l, int integration_r)
00092 {
00093   unsigned short pwm_left, pwm_right;
00094   unsigned char dir_left, dir_right, brake_left, brake_right;
00095 
00096   int index_l, index_r; // point in speed_pwm tabelle
00097 
00098   // calc pwm values form speed array
00099   index_l = (int)(fabs(v_l) / vmax_ * nr_v_);
00100   index_r = (int)(fabs(v_r) / vmax_ * nr_v_);
00101 
00102   index_l = std::min(nr_v_ - 1, index_l);
00103   index_r = std::min(nr_v_ - 1, index_r);
00104 
00105   // 1023 = zero, 0 = maxspeed
00106   if (fabs(v_l) > 0.01)
00107     pwm_left = std::max(0, std::min(1023, 1024 - pwm_v_l_[index_l] - leerlauf_adapt_ - integration_l));
00108   else
00109     pwm_left = 1023;
00110   if (fabs(v_r) > 0.01)
00111     pwm_right = std::max(0, std::min(1023, 1024 - pwm_v_r_[index_r] - leerlauf_adapt_ - integration_r));
00112   else
00113     pwm_right = 1023;
00114 
00115   if (v_l >= 0.0)
00116     dir_left = 0; // forward
00117   else
00118     dir_left = 1; // backward
00119   if (v_r >= 0.0)
00120     dir_right = 0; // s.o right wheel
00121   else
00122     dir_right = 1;
00123 
00124   // relase breaks
00125   brake_left = 0;
00126   brake_right = 0;
00127 
00128   can_motor(pwm_left, dir_left, brake_left, pwm_right, dir_right, brake_right);
00129 }
00130 
00131 // pid geschwindigkeits regler fuers linke und rechte rad
00132 // omega wird benoetig um die integration fuer den darunterstehenden regler
00133 // zu berechnen
00134 void Kurt::set_wheel_speed2(double _v_l_soll, double _v_r_soll, double _v_l_ist,
00135     double _v_r_ist, double _omega, double _AntiWindup)
00136 {
00137   // stellgroessen v=speed, l= links, r= rechts
00138   static double zl = 0.0, zr = 0.0;
00139   static double last_zl = 0.0, last_zr = 0.0;
00140   // regelabweichung e = soll - ist;
00141   static double el = 0.0, er = 0.0;
00142   static double last_el = 0.0, last_er = 0.0;
00143   // integral
00144   static double int_el = 0.0, int_er = 0.0;
00145   // differenzieren
00146   static double del = 0.0, der = 0.0;
00147   // zeitinterval
00148   double dt = 0.01;
00149   // filter fuer gueltige Werte
00150   static double last_v_l_ist = 0.0, last_v_r_ist = 0.0;
00151   // static int reached = 0;
00152 
00153   // These arrays need to be initialized because we read from indices 0, 1, 2
00154   // before writing to them. If an array is partially initialized, elements
00155   // that are not initialized receive the value 0 of the appropriate type, so
00156   // initializing the first value is enough.
00157   static double v_l_list[MAX_V_LIST] = {0.0}, v_r_list[MAX_V_LIST] = {0.0};
00158   static int vl_index = 3, vr_index = 3;
00159   int i;
00160   static double f_v_l_ist, f_v_r_ist;
00161   // kd_l and kd_r allways 0 (using only pi controller here)
00162   double kd_l = 0.0, kd_r = 0.0; // nur pi regler d-anteil ausblenden
00163 
00164   int_el *= _AntiWindup;
00165   int_er *= _AntiWindup;
00166 
00167   last_v_l_ist *= _AntiWindup;
00168   last_v_r_ist *= _AntiWindup;
00169 
00170   double turn_feedforward_l = -_omega / M_PI * feedforward_turn_;
00171   double turn_feedforward_r = _omega / M_PI * feedforward_turn_;
00172 
00173   // filtern: grosser aenderungen deuten auf fehlerhafte messungen hin
00174   if (fabs(_v_l_ist - last_v_l_ist) < 0.19)
00175   {
00176     // filter glaettung werte speichern
00177     v_l_list[vl_index] = _v_l_ist;
00178     f_v_l_ist = (v_l_list[vl_index] + v_l_list[vl_index - 1] + v_l_list[vl_index - 2] + v_l_list[vl_index - 3]) / 4.0;
00179     vl_index++; // achtung auf ueberlauf
00180     if (vl_index >= MAX_V_LIST)
00181     {
00182       vl_index = 3; // zum schutz vor ueberlauf kopieren bei hold einen mehr kopieren
00183       for (i = 0; i < 3; i++)
00184         v_l_list[2 - i] = v_l_list[MAX_V_LIST - i - 1];
00185     }
00186 
00187     el = _v_l_soll - f_v_l_ist;
00188     del = (el - last_el) / dt;
00189     int_el += el * dt;
00190 
00191     zl = kp_l * el + kd_l * del + ki_l * int_el + _v_l_soll + turn_feedforward_l;
00192 
00193     last_el = el; // last e
00194   }
00195 
00196   // filtern: grosser aenderungen deuten auf fehlerhafte messungen hin
00197   if (fabs(_v_r_ist - last_v_r_ist) < 0.19)
00198   {
00199     // filter glaettung werte speichern
00200     v_r_list[vr_index] = _v_r_ist;
00201     f_v_r_ist = (v_r_list[vr_index] + v_r_list[vr_index - 1] + v_r_list[vr_index - 2] + v_r_list[vr_index - 3]) / 4.0;
00202     vr_index++; // achtung auf ueberlauf
00203     if (vr_index >= MAX_V_LIST)
00204     {
00205       vr_index = 3; // zum schutz vor ueberlauf kopieren bei hold einen mehr kopieren
00206       for (i = 0; i < 3; i++)
00207       {
00208         v_r_list[2 - i] = v_r_list[MAX_V_LIST - i - 1];
00209       }
00210     }
00211 
00212     er = _v_r_soll - f_v_r_ist;
00213     der = (er - last_er) / dt;
00214     int_er += er * dt;
00215 
00216     zr = kp_r * er + kd_r * der + ki_r * int_er + _v_r_soll + turn_feedforward_r;
00217 
00218     last_er = er; // last e
00219   }
00220 
00221   // range check und antiwindup stellgroessenbeschraenkung
00222   // verhindern das der integrier weiter hochlaeuft
00223   // deshalb die vorher addierten werte wieder abziehen
00224   if (zl > vmax_)
00225   {
00226     zl = vmax_;
00227     int_el -= el * dt;
00228   }
00229   if (zr > vmax_)
00230   {
00231     zr = vmax_;
00232     int_er -= er * dt;
00233   }
00234   if (zl < -vmax_)
00235   {
00236     zl = -vmax_;
00237     int_el -= el * dt;
00238   }
00239   if (zr < -vmax_)
00240   {
00241     zr = -vmax_;
00242     int_er -= er * dt;
00243   }
00244 
00245   // reduzieren
00246 
00247   double step_max = vmax_ * 0.5;
00248   /* kraft begrenzung damit die Kette nicht springt bzw
00249      der Motor ein wenig entlastet wird. bei vorgabe von max
00250      geschwindigkeit braucht es so 5 * 10 ms bevor die Maximale
00251      Kraft anliegt */
00252   if ((zl - last_zl) > step_max)
00253   {
00254     zl = last_zl + step_max;
00255   }
00256   if ((zl - last_zl) < -step_max)
00257   {
00258     zl = last_zl - step_max;
00259   }
00260   if ((zr - last_zr) > step_max)
00261   {
00262     zr = last_zr + step_max;
00263   }
00264   if ((zr - last_zr) < -step_max)
00265   {
00266     zr = last_zr - step_max;
00267   }
00268 
00269   // store old val for deviation plotting
00270   last_v_l_ist = _v_l_ist;
00271   last_v_r_ist = _v_r_ist;
00272   last_zl = zl;
00273   last_zr = zr;
00274 
00275   set_wheel_speed1(zl, zr, 0, 0);
00276 }
00277 
00278 void Kurt::set_wheel_speed2_mc(double _v_l_soll, double _v_r_soll, double _omega,
00279     double _AntiWindup)
00280 {
00281   // divisor for floating points; will only send integer values ([+-]32e3) to Kurt
00282   const long factor = 100;
00283   int left_speed = (int)(_v_l_soll * factor);
00284   int right_speed = (int)(_v_r_soll * factor);
00285   int omega = (int)(factor * _omega / M_PI);
00286   int AntiWindup = (int)_AntiWindup;
00287   int sign   = omega < 0      ? 1 : 0;
00288   int windup = AntiWindup > 0 ? 1 : 0;
00289 
00290   omega = abs(omega);
00291   omega = (omega << 1) + sign;
00292   omega = (omega << 1) + windup;
00293 
00294   can_frame frame;
00295   frame.can_id = CAN_CONTROL;
00296   frame.can_dlc = 8;
00297   frame.data[0] = SPEED_CM >> 8;
00298   frame.data[1] = SPEED_CM;
00299   frame.data[2] = left_speed >> 8;
00300   frame.data[3] = left_speed;
00301   frame.data[4] = right_speed >> 8;
00302   frame.data[5] = right_speed;
00303   frame.data[6] = omega >> 8;
00304   frame.data[7] = omega;
00305 
00306   if(!can_.send_frame(&frame))
00307   {
00308     ROS_ERROR("set_wheel_speed2_mc: Error sending speed");
00309   }
00310 }
00311 
00312 void Kurt::set_wheel_speed(double _v_l_soll, double _v_r_soll, double _AntiWindup)
00313 {
00314   if (use_microcontroller_)
00315   {
00316     //Disable AntiWindup for now as the Kurt micro controller crashes when
00317     //going from zero to full speed with it activated
00318     set_wheel_speed2_mc(_v_l_soll, _v_r_soll, 0, 1.0);
00319   }
00320   else
00321   {
00322     set_wheel_speed2(_v_l_soll, _v_r_soll, v_encoder_left_, v_encoder_right_, 0, _AntiWindup);
00323   }
00324 }
00325 
00326 // reads init data from pmw to speed experiment
00327 bool Kurt::read_speed_to_pwm_leerlauf_tabelle(const std::string &filename, int *nr, double **v_pwm_l, double **v_pwm_r)
00328 {
00329   int i;
00330   FILE *fpr_fftable = NULL;
00331   double t, v, vl, vr;
00332   int pwm;
00333 
00334   ROS_INFO("Open FeedForwardTabelle: %s", filename.c_str());
00335   fpr_fftable = fopen(filename.c_str(), "r");
00336 
00337   if (fpr_fftable == NULL)
00338   {
00339     ROS_ERROR("ERROR opening speedtable.");
00340     return false;
00341   }
00342 
00343   *nr = 1025;
00344   *v_pwm_l = (double *)calloc(*nr, sizeof(double));
00345   *v_pwm_r = (double *)calloc(*nr, sizeof(double));
00346   for (i = 0; i < *nr; i++)
00347   {
00348     if(fscanf(fpr_fftable, "%lf %lf %lf %lf %d", &t, &v, &vl, &vr, &pwm) == EOF)
00349     {
00350       ROS_ERROR("ERROR reading speedtable.");
00351       fclose(fpr_fftable);
00352       return false;
00353     }
00354     (*v_pwm_l)[pwm] = vl;
00355     (*v_pwm_r)[pwm] = vr;
00356   }
00357   fclose(fpr_fftable);
00358   return true;
00359 }
00360 
00361 // generate reverse speedtable
00362 void Kurt::make_pwm_v_tab(int nr, double *v_pwm_l, double *v_pwm_r, int nr_v, int **pwm_v_l, int **pwm_v_r, double *v_max)
00363 {
00364   int i, index_l, index_r;
00365   //int last_l, last_r;
00366   double v_max_l = 0.0, v_max_r = 0.0;
00367   *v_max = 0.0;
00368 
00369   *pwm_v_l = (int *)calloc(nr_v + 1, sizeof(int));
00370   *pwm_v_r = (int *)calloc(nr_v + 1, sizeof(int));
00371 
00372   // check max wheel speed left right wheel
00373   for (i = 0; i < nr; i++)
00374   {
00375     if (v_pwm_l[i] > v_max_l)
00376       v_max_l = v_pwm_l[i];
00377     if (v_pwm_r[i] > v_max_r)
00378       v_max_r = v_pwm_r[i];
00379     *v_max = std::min(v_max_l, v_max_r);
00380   }
00381 
00382   // diskretisieren
00383   for (i = 0; i < nr; i++)
00384   {
00385     index_l = std::min(nr_v, std::max(0, (int)((v_pwm_l[i]) * (nr_v) / *v_max)));
00386     (*pwm_v_l)[index_l] = i;
00387     index_r = std::min(nr_v, std::max(0, (int)((v_pwm_r[i]) * (nr_v) / *v_max)));
00388     (*pwm_v_r)[index_r] = i;
00389   }
00390   (*pwm_v_l)[0] = 0; // for a stop
00391   (*pwm_v_r)[0] = 0;
00392 
00393   // interpolate blank entries
00394   //last_l = (*pwm_v_l)[0];
00395   //last_r = (*pwm_v_r)[0];
00396   for (i = 1; i < nr_v; i++)
00397   {
00398     // interpolation simple methode, pwm must be increasing
00399     if ((*pwm_v_l)[i] < (*pwm_v_l)[i - 1])
00400       (*pwm_v_l)[i] = (*pwm_v_l)[i - 1];
00401     if ((*pwm_v_r)[i] < (*pwm_v_r)[i - 1])
00402       (*pwm_v_r)[i] = (*pwm_v_r)[i - 1];
00403     /* interpolation advancde methode
00404        if ((*pwm_v_l)[i] == 0) {
00405        for (j = i+1; j <= nr_v; j++)
00406        if ((*pwm_v_l)[j] != 0) { next_l = (*pwm_v_l)[j]; j = nr_v; }
00407        (*pwm_v_l)[i] = (next_l + last_l ) / 2;
00408        } else last_l = (*pwm_v_l)[i];
00409        if ((*pwm_v_r)[i] == 0) {
00410        for (j = i+1; j <= nr_v; j++)
00411        if ((*pwm_v_r)[j] != 0) {next_r = (*pwm_v_r)[j];j = nr_v;}
00412        (*pwm_v_r)[i] = (next_r + last_r ) / 2;
00413        } else last_r = (*pwm_v_r)[i];
00414        */
00415   }
00416 }
00417 
00418 void Kurt::odometry(int wheel_a, int wheel_b)
00419 {
00420   // time_diff in sec; we hope kurt is precise ?? !! and sends every 10 ms
00421   double time_diff = 0.01;
00422 
00423   // covered distance of wheels in meter
00424   double wheel_L = wheel_perimeter_ * wheel_a / ticks_per_turn_of_wheel_;
00425   double wheel_R = wheel_perimeter_ * wheel_b / ticks_per_turn_of_wheel_;
00426 
00427   // calc different speeds in meter / sec
00428   v_encoder_left_ = wheel_L / time_diff;
00429   v_encoder_right_ = wheel_R / time_diff;
00430   double v_encoder = (v_encoder_right_ + v_encoder_left_) * 0.5;
00431   // angular velocity in rad/s
00432   double v_encoder_angular = (v_encoder_right_ - v_encoder_left_) / axis_length_ * turning_adaptation_;
00433 
00434   // calc position deltas
00435   double local_dx, local_dz, dtheta_y = 0.0;
00436   const double EPSILON = 0.0001;
00437 
00438   if (fabs(wheel_L - wheel_R) < EPSILON)
00439   {
00440     if (fabs(wheel_L) < EPSILON)
00441     {
00442       local_dx = 0.0;
00443       local_dz = 0.0;
00444     }
00445     else // beide fast gleich (wheel_distance_a == wheel_distance_b)
00446     {
00447       local_dx = 0.0;
00448       local_dz = (wheel_L + wheel_R) * 0.5;
00449     }
00450   }
00451   else // (wheel_distance_a != wheel_distance_b) and > 0
00452   {
00453     double hypothenuse = 0.5 * (wheel_L + wheel_R);
00454 
00455     dtheta_y = (wheel_L - wheel_R) / axis_length_ * turning_adaptation_;
00456 
00457     local_dx = hypothenuse * sin(dtheta_y);
00458     local_dz = hypothenuse * cos(dtheta_y);
00459   }
00460 
00461   // Odometrie : Koordinatentransformation in Weltkoordinaten
00462   static double x_from_encoder = 0.0;
00463   static double z_from_encoder = 0.0;
00464   static double theta_from_encoder = 0.0;
00465 
00466   x_from_encoder += local_dx * cos(theta_from_encoder) + local_dz * sin(theta_from_encoder);
00467   z_from_encoder += -local_dx * sin(theta_from_encoder) + local_dz * cos(theta_from_encoder);
00468 
00469   theta_from_encoder += dtheta_y;
00470   if (theta_from_encoder > M_PI)
00471     theta_from_encoder -= 2.0 * M_PI;
00472   if (theta_from_encoder < -M_PI)
00473     theta_from_encoder += 2.0 * M_PI;
00474 
00475   comm_.send_odometry(z_from_encoder, x_from_encoder, theta_from_encoder, v_encoder, v_encoder_angular, wheel_a, wheel_b, v_encoder_left_, v_encoder_right_);
00476 }
00477 
00479 
00480 void Kurt::can_rotunit_send(double speed)
00481 {
00482   int ticks =  (int)(speed / (2.0 * M_PI) * 10240 / 20);
00483   can_frame frame;
00484   frame.can_id = CAN_SETROTUNT;
00485   frame.can_dlc = 8;
00486   frame.data[0] = (ticks >> 8); //high
00487   frame.data[1] = (ticks & 0xFF); //low
00488   frame.data[2] = 0;
00489   frame.data[3] = 0;
00490   frame.data[4] = 0;
00491   frame.data[5] = 0;
00492   frame.data[6] = 0;
00493   frame.data[7] = 0;
00494 
00495   if(!can_.send_frame(&frame))
00496   {
00497     ROS_ERROR("can_rotunit_send: Error sending rotunit speed");
00498   }
00499   else
00500   {
00501     use_rotunit_ = true;
00502   }
00503 }
00504 
00505 void Kurt::can_rotunit(const can_frame &frame)
00506 {
00507   int rot = (frame.data[1] << 8) + frame.data[2];
00508   double rot2 = rot * 2 * M_PI / 10240;
00509   comm_.send_rotunit(rot2);
00510 }
00511 
00513 
00514 void Kurt::can_encoder(const can_frame &frame)
00515 {
00516   int left_encoder = 0, right_encoder = 0;
00517   if (frame.data[0] & 0x80) // negative Zahl auf 15 Bit genau
00518     left_encoder = (frame.data[0] << 8) + frame.data[1]-65536;
00519   else
00520     left_encoder = (frame.data[0] << 8) + frame.data[1];
00521 
00522   if (frame.data[2] & 0x80) // negative Zahl auf 15 Bit genau
00523     right_encoder = (frame.data[2] << 8) + frame.data[3]-65536;
00524   else
00525     right_encoder = (frame.data[2] << 8) + frame.data[3];
00526 
00527   odometry(left_encoder, right_encoder);
00528 }
00529 
00530 int Kurt::normalize_ir(int ir)
00531 {
00532   if (ir < IR_MIN * 1000 || ir > IR_MAX * 1000) // convert m --> mm
00533   {
00534     return -1;
00535   }
00536   return (int)(pow(30000.0 / ((double)ir - 10.0), 1.0 / 1.4) - 10.0);
00537 }
00538 
00539 int Kurt::normalize_sonar(int s)
00540 {
00541   if (s < SONAR_MIN * 1000 || s > SONAR_MAX * 1000) // convert m --> mm
00542   {
00543     return -1;
00544   }
00545   return (int)((double)s * 0.110652 + 11.9231);
00546 }
00547 
00548 void Kurt::can_sonar8_9(const can_frame &frame)
00549 {
00550   int sonar1 = normalize_ir((frame.data[2] << 8) + frame.data[3]);
00551 
00552   comm_.send_sonar_leftBack(sonar1);
00553 }
00554 
00555 void Kurt::can_sonar4_7(const can_frame &frame)
00556 {
00557   int sonar0 = normalize_ir((frame.data[0] << 8) + frame.data[1]);
00558   int sonar1 = normalize_sonar((frame.data[2] << 8) + frame.data[3]);
00559   int sonar2 = normalize_ir((frame.data[4] << 8) + frame.data[5]);
00560   int sonar3 = normalize_ir((frame.data[6] << 8) + frame.data[7]);
00561 
00562   comm_.send_sonar_front_usound_leftFront_left(sonar0, sonar1, sonar2, sonar3);
00563 }
00564 
00565 void Kurt::can_sonar0_3(const can_frame &frame)
00566 {
00567   int sonar0 = normalize_ir((frame.data[0] << 8) + frame.data[1]);
00568   int sonar1 = normalize_ir((frame.data[2] << 8) + frame.data[3]);
00569   int sonar2 = normalize_ir((frame.data[4] << 8) + frame.data[5]);
00570 
00571   comm_.send_sonar_back_rightBack_rightFront(sonar0, sonar1, sonar2);
00572 }
00573 
00574 void Kurt::can_tilt_comp(const can_frame &frame)
00575 {
00576   double a0, a1;
00577   unsigned int t0, t1;
00578 
00579   t0 = (frame.data[0] << 8) + (frame.data[1]);
00580   t1 = (frame.data[2] << 8) + (frame.data[3]);
00581   //int comp1 = (frame.data[4] << 8) + frame.data[5];
00582   //int comp2 = (frame.data[6] << 8) + frame.data[7];
00583 
00584   // calculate g values (offset and sensitivity correction)
00585   a0 = ((double)t0 - 32768.0) / 3932.0;
00586   a1 = ((double)t1 - 32768.0) / 3932.0;
00587 
00588   // calculate angles and convert do deg
00589   double tilt_lr = asin(a0);
00590   double tilt_fb = asin(a1);
00591 
00592   double roll = tilt_lr * 180.0 / M_PI;
00593   double pitch = tilt_fb * 180.0 / M_PI;
00594   comm_.send_pitch_roll(pitch, roll);
00595 }
00596 
00597 void Kurt::can_gyro_mc1(const can_frame &frame)
00598 {
00599   static int gyro_offset_read = 0;
00600   static double offset, delta; // initial offset
00601   signed long gyro_raw;
00602 
00603   gyro_raw = (frame.data[0] << 24) + (frame.data[1] << 16)
00604     + (frame.data[2] << 8)  + (frame.data[3]);
00605   double theta = (double)gyro_raw / 4992511.0 * M_PI / 180.0;
00606 
00607   double sigma_deg = (double)((frame.data[4] << 24) + (frame.data[5] << 16)
00608       + (frame.data[6] << 8)  + (frame.data[7])) / 10000;
00609 
00610   double tmp = (sqrt(sigma_deg) * M_PI / 180.0);
00611   double sigma = tmp * tmp;
00612 
00613   // wait until gyro is stable
00614   if (gyro_offset_read++ < 100)
00615     return;
00616 
00617   if(gyro_offset_read == 100)
00618   {
00619     offset = theta;
00620     delta = offset / 100.0;
00621     if (delta >  M_PI) delta -= 2.0 * M_PI;
00622     if (delta < -M_PI) delta += 2.0 * M_PI;
00623   }
00624 
00625   offset += delta;
00626   if (offset >  M_PI) offset -= 2.0 * M_PI;
00627   if (offset < -M_PI) offset += 2.0 * M_PI;
00628 
00629   theta -= offset;
00630 
00631   if (theta >  M_PI) theta -= 2.0 * M_PI;
00632   if (theta < -M_PI) theta += 2.0 * M_PI;
00633 
00634   comm_.send_gyro(theta, sigma);
00635 }
00636 
00637 int Kurt::can_read_fifo()
00638 {
00639   can_frame frame;
00640 
00641   if(!can_.receive_frame(&frame))
00642     return -1;
00643 
00644   switch (frame.can_id) {
00645     case CAN_ADC00_03:
00646       can_sonar0_3(frame);
00647       break;
00648     case CAN_ADC04_07:
00649       can_sonar4_7(frame);
00650       break;
00651     case CAN_ADC08_11:
00652       can_sonar8_9(frame);
00653       break;
00654     case CAN_ENCODER:
00655       can_encoder(frame);
00656       break;
00657     case CAN_TILT_COMP:
00658       can_tilt_comp(frame);
00659       break;
00660     case CAN_GYRO_MC1:
00661       can_gyro_mc1(frame);
00662       break;
00663     case CAN_GETROTUNIT:
00664       can_rotunit(frame);
00665       break;
00666     /*case CAN_CONTROL:
00667       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (control message)", frame.can_id);
00668       break;
00669     case CAN_INFO_1:
00670       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (info message)", frame.can_id);
00671       break;
00672     case CAN_ADC12_15:
00673       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (analog input channels: 12 - 15)", frame.can_id);
00674       break;
00675     case CAN_BUMPERC:
00676       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (bumpers and remote control)", frame.can_id);
00677       break;
00678     case CAN_BDC00_03:
00679       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (analog input channels: 0 - 3)", frame.can_id);
00680       break;
00681     case CAN_BDC04_07:
00682       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (analog input channels: 4 - 7)", frame.can_id);
00683       break;
00684     case CAN_BDC08_11:
00685       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (analog input channels: 8 - 11)", frame.can_id);
00686       break;
00687     case CAN_BDC12_15:
00688       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (analog input channels: 12 - 15)", frame.can_id);
00689       break;
00690     case CAN_GYRO_MC2:
00691       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (data from gyro connected to 2nd C167)", frame.can_id);
00692       break;
00693     case CAN_DEADRECK:
00694       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X (position (dead reckoning))", frame.can_id);
00695       break;
00696     case CAN_GETSPEED:
00697       ROS_DEBUG("can_read_fifo: Unused CAN message ID: %X current transl. and rot. speed)", frame.can_id);
00698       break;
00699     default:
00700       ROS_DEBUG("can_read_fifo: Unknown CAN ID: %X", frame.can_id);*/
00701   }
00702 
00703   return frame.can_id;
00704 }


kurt_base
Author(s): Jochen Sprickerhof
autogenerated on Mon Oct 6 2014 01:39:03