Revision f336542d components/Odometry.cpp

View differences:

components/Odometry.cpp
19 19 

  		




  20
  20
  
    

  		




  21
  21
  
    
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)


  		




  22
  
  
    
    : BaseStaticThread<1024>(),


  		




  
  22
  
    
    : BaseStaticThread<512>(),


  		




  23
  23
  
    
      motorIncrements(mi),


  		




  24
  24
  
    
      gyro(gyroscope),


  		




  25
  25
  
    
      eventSource(),


  		




  ......




  82
  82
  
    

  		




  83
  83
  
    
void Odometry::setError(float* Cp3x3) {


  		




  84
  84
  
    
  chSysLock();


  		




  85
  
  
    
//   float** test;


  		




  86
  85
  
    
    Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);


  		




  87
  
  
    
//     Matrix::copy<float>(Cp3x3,3,3, test,3,3);


  		




  88
  86
  
    
  chSysUnlock();


  		




  89
  87
  
    
}


  		




  90
  88
  
    

  		




  91
  89
  
    
void Odometry::resetError() {


  		




  92
  
  
    
//   float zeroMatrix[9] = {};


  		




  93
  
  
    
//   this->setError(&(zeroMatrix[0]));


  		




  94
  90
  
    
  Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);


  		




  95
  91
  
    
}


  		




  96
  92
  
    

  		




  ......




  105
  101
  
    
  while (!this->shouldTerminate()) {


  		




  106
  102
  
    
    time += MS2ST(this->period);


  		




  107
  103
  
    

  		




  108
  
  
    
    // Update the base distance, because it may change after a calibration


  		




  
  104
  
    
    // Update the base distance, because it may have changed after a calibration


  		




  109
  105
  
    
    this->updateWheelBaseDistance();


  		




  110
  106
  
    

  		




  111
  107
  
    
    // Get the actual speed


  		




  ......




  120
  116
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);


  		




  121
  117
  
    
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");


  		




  122
  118
  
    

  		




  123
  
  
    
  if (time >= System::getTime()) {


  		




  124
  
  
    
      chThdSleepUntil(time);


  		




  
  119
  
    
    if (time >= System::getTime()) {


  		




  
  120
  
    
        chThdSleepUntil(time);


  		




  125
  121
  
    
    } else {


  		




  126
  
  
    
      chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");


  		




  127
  
  
    
  }


  		




  
  122
  
    
        chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");


  		




  
  123
  
    
    }


  		




  128
  124
  
    
  }


  		




  129
  125
  
    

  		




  130
  126
  
    
  return true;


  		




  ......




  134
  130
  
    

  		




  135
  131
  
    
  // Get the temporary position and error


  		




  136
  132
  
    
  float Cp3x3[9];


  		




  137
  
  
    
  uint32_t angular_ud;


  		




  
  133
  
    
  int32_t angular_ud;


  		




  
  134
  
    
  int32_t angularRate_udps;


  		




  138
  135
  
    
  chSysLock();


  		




  139
  136
  
    
    float pX = this->pX;


  		




  140
  137
  
    
    float pY = this->pY;


  		




  141
  138
  
    
    float pPhi = this->pPhi;


  		




  142
  139
  
    
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);


  		




  143
  
  
    
    // Get the differentail gyro information and reset it


  		




  144
  
  
    
    angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);


  		




  145
  
  
    
  gyro->angularReset();


  		




  
  140
  
    
    // TODO Get the gyro (or gyro rate) information and do something with it


  		




  
  141
  
    
    // angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);


  		




  
  142
  
    
    // angularRate_udps = gyro->getAngularRate_udps(L3G4200D::AXIS_Z);


  		




  146
  143
  
    
  chSysUnlock();


  		




  147
  144
  
    

  		




  148
  145
  
    
  ////////////////


  		




  ......




  150
  147
  
    
  ////////////////


  		




  151
  148
  
    

  		




  152
  149
  
    
  // TMP: Rotated angular


  		




  153
  
  
    
  // float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;


  		




  154
  
  
    
  float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;


  		




  
  150
  
    
  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;


  		




  
  151
  
    
  // TODO Calculate the differential angel dPhi from either the angular (1. line) or angular rate (2.+3. line)


  		




  
  152
  
    
  // float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;


  		




  
  153
  
    
  // const float angular_md = float((angularRate_udps * this->period / constants::millisecondsPerSecond) * 1e-3);


  		




  
  154
  
    
  // float dPhi = ((angular_md * M_PI) * 1e-3) / 180.0f;


  		




  155
  155
  
    

  		




  156
  156
  
    
  // TMP: Moved distance


  		




  157
  157
  
    
  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;


  		












Also available in:  Unified diff