00001 #define SAFETY_LEFT 0
00002 #define SAFETY_RIGHT 1
00003
00004
00005
00006 class FRICheck_legacy
00007 {
00008 private:
00009 float vel_old_[7];
00010 float pos_[7];
00011
00012 static const float lim_low[7];
00013 static const float lim_high[7];
00014 static const float lim_vel[7];
00015 static const float lim_acc[7];
00016
00017
00018 double *j5_angles;
00019 double *j6_min;
00020 double *j6_max;
00021 int length;
00023
00024 double interpolate(double x, double x1, double y1, double x2, double y2);
00025 int find_index(float j5);
00026 double min_j6(float j5, int index);
00027 double max_j6(float j5, int index);
00028 void safety_check(float *vel, float *vel_old, float *pos, float rate);
00029
00030 public:
00031
00032 int safety_set_side(int side);
00034
00035
00036 FRICheck_legacy();
00038 void setPos(float* pos);
00039
00041 float* pos() {return pos_;}
00042
00044 void adjust(float *pos, float rate);
00045 };
00046
00047
00048
