Revision f336542d

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;


  		












  

    
      components/gyro/l3g4200d.cpp
    
  





  37
  37
  
    
  this->setName("l3g4200d");


  		




  38
  38
  
    

  		




  39
  39
  
    
  while (!this->shouldTerminate()) {


  		




  40
  
  
    
  time += this->period_st;


  		




  
  40
  
    
    time += this->period_st;


  		




  41
  41
  
    

  		




  42
  42
  
    
    updateSensorData();


  		




  43
  43
  
    
    calcAngular();


  		




  ......




  48
  48
  
    
      chThdSleepUntil(time);


  		




  49
  49
  
    
    } else {


  		




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


  		




  51
  
  
    
  }


  		




  
  51
  
    
    }


  		




  52
  52
  
    
  }


  		




  53
  53
  
    
  return RDY_OK;


  		




  54
  54
  
    
}


  		




  ......




  62
  62
  
    
int32_t


  		




  63
  63
  
    
L3G4200D::


  		




  64
  64
  
    
getAngularRate_udps(const uint8_t axis) {


  		




  65
  
  
    
  return uint32_t(this->angularRate[axis]) * this->udpsPerTic;


  		




  
  65
  
    
  return int32_t(this->angularRate[axis]) * this->udpsPerTic;


  		




  66
  66
  
    
}


  		




  67
  67
  
    

  		




  68
  68
  
    
int32_t


  		




  ......




  160
  160
  
    
    case L3G4200D::DR_400_HZ: this->period_us =  2500; break;


  		




  161
  161
  
    
    case L3G4200D::DR_800_HZ: this->period_us =  1250; break;


  		




  162
  162
  
    
  }


  		




  163
  
  
    
  this->period_st = US2ST(this->period_st);


  		




  
  163
  
    
  this->period_st = US2ST(this->period_us);


  		




  
  164
  
    
  this->period_ms = this->period_us * 1e-3;


  		




  164
  165
  
    

  		




  165
  166
  
    
  // Handle the new full scale


  		




  166
  167
  
    
  switch(config->ctrl1 & L3G4200D::FS_MASK) {


  		












  

    
      devices/DiWheelDrive/main.cpp
    
  





  849
  849
  
    

  		




  850
  850
  
    
  global.increments.start();  // Start the qei driver


  		




  851
  851
  
    

  		




  
  852
  
    
  // Start the three axes gyroscope


  		




  
  853
  
    
  global.l3g4200d.configure(&global.gyro_run_config);


  		




  
  854
  
    
  global.l3g4200d.start(NORMALPRIO+5);


  		




  
  855
  
    

  		




  852
  856
  
    
  global.odometry.start(NORMALPRIO + 20);


  		




  853
  857
  
    

  		




  854
  858
  
    
  global.robot.start(HIGHPRIO - 1);


  		




  ......




  867
  871
  
    
  global.lis331dlh.configure(&global.accel_run_config);


  		




  868
  872
  
    
  global.lis331dlh.start(NORMALPRIO+4);


  		




  869
  873
  
    

  		




  870
  
  
    
  // Start the three axes gyroscope


  		




  871
  
  
    
  global.l3g4200d.configure(&global.gyro_run_config);


  		




  872
  
  
    
  global.l3g4200d.start(NORMALPRIO+5);


  		




  873
  
  
    

  		




  874
  874
  
    
  // Start the user thread


  		




  875
  875
  
    
  global.userThread.start(NORMALPRIO);


  		




  876
  876
  
    

  		












  

    
      include/amiro/Odometry.h
    
  





  8
  8
  
    

  		




  9
  9
  
    
namespace amiro {


  		




  10
  10
  
    

  		




  11
  
  
    
  class Odometry : public chibios_rt::BaseStaticThread<1024> {


  		




  
  11
  
    
  class Odometry : public chibios_rt::BaseStaticThread<512> {


  		




  12
  12
  
    
  public:


  		




  13
  13
  
    
    /**


  		




  14
  14
  
    
     * Constructor


  		












Also available in:  Unified diff