Revision 47364c68
| components/Odometry.cpp | ||
|---|---|---|
| 19 | 19 |
|
| 20 | 20 |
|
| 21 | 21 |
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope) |
| 22 |
: BaseStaticThread<1024>(),
|
|
| 22 |
: BaseStaticThread<256>(),
|
|
| 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 |
//////////////// |
| ... | ... | |
| 151 | 148 |
|
| 152 | 149 |
// TMP: Rotated angular |
| 153 | 150 |
// 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; |
|
| 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<256> {
|
|
| 12 | 12 |
public: |
| 13 | 13 |
/** |
| 14 | 14 |
* Constructor |
Also available in: Unified diff