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