Revision b4885314 components/Odometry.cpp

View differences:

components/Odometry.cpp
15 15 
using namespace constants::DiWheelDrive;
16 16 

  		




  17
  17
  
    

  		




  18
  
  
    
Odometry::Odometry(MotorIncrements* mi)


  		




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


  		




  19
  19
  
    
    : BaseStaticThread<512>(),


  		




  20
  20
  
    
      motorIncrements(mi),


  		




  
  21
  
    
      gyro(gyroscope),


  		




  21
  22
  
    
      eventSource(),


  		




  22
  23
  
    
      period(50),


  		




  23
  24
  
    
      incrementsPerRevolution(incrementsPerRevolution),


  		




  ......




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


  		




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


  		




  103
  104
  
    

  		




  104
  
  
    
    // Update the base distance, because it may change after an calibration


  		




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


  		




  105
  106
  
    
    this->updateWheelBaseDistance();


  		




  106
  107
  
    

  		




  107
  108
  
    
    // Get the actual speed


  		




  ......




  109
  110
  
    

  		




  110
  111
  
    
    // Calculate the odometry


  		




  111
  112
  
    
    this->updateOdometry();


  		




  
  113
  
    
		


  		




  112
  114
  
    

  		




  113
  115
  
    
//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);


  		




  114
  116
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  115
  117
  
    
//     chprintf((BaseSequentialStream*) &SD1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);


  		




  116
  118
  
    
//     chprintf((BaseSequentialStream*) &SD1, "\r\n");


  		




  117
  119
  
    

  		




  118
  
  
    
    chThdSleepUntil(time);


  		




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


  		




  
  121
  
    
      chThdSleepUntil(time);


  		




  
  122
  
    
    } else {


  		




  
  123
  
    
      chprintf((BaseSequentialStream*) &SD1, "WARNING Odometry: Unable to keep track\r\n");


  		




  
  124
  
    
	}


  		




  119
  125
  
    
  }


  		




  120
  126
  
    

  		




  121
  127
  
    
  return true;


  		




  ......




  125
  131
  
    

  		




  126
  132
  
    
  // Get the temporary position and error


  		




  127
  133
  
    
  float Cp3x3[9];


  		




  
  134
  
    
  uint32_t angular_ud;


  		




  128
  135
  
    
  chSysLock();


  		




  129
  136
  
    
    float pX = this->pX;


  		




  130
  137
  
    
    float pY = this->pY;


  		




  131
  138
  
    
    float pPhi = this->pPhi;


  		




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


  		




  
  140
  
    
    // Get the differentail gyro information and reset it


  		




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


  		




  
  142
  
    
	gyro->angularReset();


  		




  133
  143
  
    
  chSysUnlock();


  		




  134
  144
  
    

  		




  135
  145
  
    
  ////////////////


  		




  ......




  137
  147
  
    
  ////////////////


  		




  138
  148
  
    

  		




  139
  149
  
    
  // TMP: Rotated angular


  		




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


  		




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


  		




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


  		




  141
  152
  
    

  		




  142
  153
  
    
  // TMP: Moved distance


  		




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


  		












Also available in:  Unified diff