Permalink
Cannot retrieve contributors at this time
#include <Romi32U4.h> | |
#include "gyro.h" | |
#include "motors.h" | |
#include "pose_snapshotter.h" | |
#include "sound.h" | |
#include "pose.h" | |
#include "defaults.h" | |
#include "debug.h" | |
Gyro::Gyro(Sound* sound, Motors* motors, PoseSnapshotter* poseSnapshotter) : sound(sound), motors(motors), poseSnapshotter(poseSnapshotter) | |
{ | |
gyroOffset = 0; | |
gyroLastUpdateMicros = 0; | |
gyroAngleDifference = 0; | |
resetAngles(); | |
} | |
/** | |
* Whatever our current heading is is now considered 0 degrees. | |
*/ | |
void Gyro::resetAngles() | |
{ | |
gyroAngle = 0; | |
gyroAngleRad = 0; | |
} | |
void Gyro::resetLastUpdateTimeToNow() | |
{ | |
gyroLastUpdateMicros = micros(); | |
} | |
double Gyro::getAngleRad() | |
{ | |
return gyroAngleRad; | |
} | |
/** | |
* Update gyroscope heading. | |
* | |
* Similar to getHeading(), this will always update gyroAngleRad to a value between 0 and 2*PI where the value grows in the | |
* counter-clockwise direction. | |
*/ | |
void Gyro::updateHeading() | |
{ | |
while ( ! imu.readReg(LSM6::STATUS_REG) & 0x08); | |
imu.read(); | |
if (imu.timeoutOccurred()) | |
{ | |
sound->alarm(true); | |
} | |
// Update the orientation as determined by the gyroscope | |
int16_t gyroTurnRate = imu.g.z - gyroOffset; // int16_t | |
unsigned long now = micros(); // uint32_t | |
unsigned long dt = now - gyroLastUpdateMicros; | |
gyroLastUpdateMicros = now; | |
// Determine how much we've rotated around Z axis since last measurement | |
int32_t rotation = (int32_t)gyroTurnRate * dt; // signed 16 bits -> signed 32 bits multiplied by unsigned long | |
// rotation is measured in gyro digits * micro-seconds. convert to degrees and add to the current angle | |
gyroAngleDifference = ((double)rotation * GYRODIGITS_TO_DPS) * 0.000001; | |
gyroAngle += gyroAngleDifference; | |
while (gyroAngle < 0) | |
{ | |
gyroAngle += 360; | |
} | |
while (gyroAngle > 360) | |
{ | |
gyroAngle -= 360; | |
} | |
gyroAngleRad = gyroAngle * (2*M_PI / 360.0); | |
} | |
/** | |
* Our heading is so wrong we should correct with a pivot (on-the-spot) rotation rather than PID (using gyroscope). | |
*/ | |
void Gyro::correctHeadingWithPivotTurn(Pose currentPose, double headingError, uint8_t pivotTurnSpeed, bool enableRanging) | |
{ | |
motors->setSpeeds(0, 0); | |
updateHeading(); | |
double diff, delaySafeHeadingError = 0.75 * headingError, gyroStartAngleRad = gyroAngleRad; | |
bool skip; | |
int i = 0; | |
#ifdef __DEBUG__ | |
snprintf_P(report, sizeof(report), PSTR(" CORRECTING [%s] rad %s, gyroStartAngleRad [%s]"), ftoa(floatBuf1, headingError), ((headingError > 0) ? "CCW" : "CW"), ftoa(floatBuf2, gyroStartAngleRad)); | |
Serial.println(report); | |
#endif | |
delay(PIVOT_TURN_SLEEP_MS); | |
if (headingError > 0) | |
{ | |
// left backwards, right forwards (counter clockwise ("left")): radians are positive CCW | |
motors->setSpeeds(-1 * pivotTurnSpeed, pivotTurnSpeed); | |
do | |
{ | |
delay(10); | |
updateHeading(); | |
skip = false; | |
// Ignore noise, the angle SHOULD be increasing | |
if ((fabs(gyroStartAngleRad - gyroAngleRad) < 0.02) || (fabs(gyroStartAngleRad - gyroAngleRad) > ((2*M_PI) - 0.02))) | |
{ | |
#ifdef __DEBUG__ | |
// Serial.println("Ignoring noise"); | |
#endif | |
skip = true; | |
continue; | |
} | |
if (gyroAngleRad > gyroStartAngleRad) | |
{ | |
diff = gyroAngleRad - gyroStartAngleRad; | |
} | |
else | |
{ | |
diff = ((2*M_PI) - gyroStartAngleRad) + gyroAngleRad; | |
} | |
// Only record a snapshot (which requires an ADC read, which can impact timing) during the first 75% of the rotation and even then only every 400ms | |
if ((diff < delaySafeHeadingError) && ((i++ % 40) == 0)) | |
{ | |
poseSnapshotter->recordSnapshot(currentPose.heading + diff, currentPose.x, currentPose.y, enableRanging, NULL); | |
} | |
#ifdef __DEBUG__ | |
// Serial.println(diff); | |
#endif | |
} | |
while (skip || (diff < headingError)); | |
} | |
else | |
{ | |
// left forwards, right backwards (clockwise, ("right")) | |
motors->setSpeeds(pivotTurnSpeed, -1 * pivotTurnSpeed); | |
do | |
{ | |
delay(10); | |
updateHeading(); | |
skip = false; | |
// Ignore noise, the angle SHOULD be decreasing | |
if ((fabs(gyroStartAngleRad - gyroAngleRad) < 0.02) || (fabs(gyroStartAngleRad - gyroAngleRad) > ((2*M_PI) - 0.02))) | |
{ | |
#ifdef __DEBUG__ | |
// Serial.println("Ignoring noise"); | |
#endif | |
skip = true; | |
continue; | |
} | |
if (gyroAngleRad < gyroStartAngleRad) | |
{ | |
diff = gyroAngleRad - gyroStartAngleRad; | |
} | |
else | |
{ | |
diff = -1.0 * (gyroStartAngleRad + ((2*M_PI) - gyroAngleRad)); | |
} | |
// Only record a snapshot (which requires an ADC read, which can impact timing) during the first 75% of the rotation and even then only every 400ms | |
if ((diff > delaySafeHeadingError) && ((i++ % 40) == 0)) | |
{ | |
poseSnapshotter->recordSnapshot(currentPose.heading + diff, currentPose.x, currentPose.y, enableRanging, NULL); | |
} | |
#ifdef __DEBUG__ | |
// Serial.println(diff); | |
#endif | |
} | |
while (skip || (diff > headingError)); | |
} | |
motors->setSpeeds(0, 0); | |
#ifdef __DEBUG__ | |
snprintf_P(report, sizeof(report), PSTR(" CORRECTED heading error of [%s], current gyroAngleRad [%s] (diff [%s])"), ftoa(floatBuf1, headingError), ftoa(floatBuf2, gyroAngleRad), ftoa(floatBuf3, diff)); | |
Serial.println(report); | |
#endif | |
delay(PIVOT_TURN_SLEEP_MS); | |
} | |
/**************************************************************************************************************** | |
* Calibration | |
****************************************************************************************************************/ | |
/** | |
* Calibrate fixed offset out of the gyroscope. | |
*/ | |
void Gyro::calibrate() | |
{ | |
delay(5000); // give user time to place device at rest | |
sound->alarm(true); | |
#ifdef __DEBUG__ | |
Serial.println("Calibrating gyroscope ..."); | |
#endif | |
if ( ! imu.init()) | |
{ | |
#ifdef __DEBUG__ | |
Serial.println("Failed to detect or initialize IMU, halting."); | |
#endif | |
while(1); | |
} | |
imu.enableDefault(); | |
delay(500); | |
int32_t gyroTotal = 0; | |
for (uint16_t i = 0; i < 1024; i++) | |
{ | |
while ( ! imu.readReg(LSM6::STATUS_REG) & 0x08); | |
imu.read(); | |
gyroTotal += imu.g.z; // gyro values are int16_t (signed) | |
} | |
gyroOffset = gyroTotal / 1024; // average across all 1024 samples | |
#ifdef __DEBUG__ | |
Serial.println("Gyroscope calibration complete"); | |
#endif | |
sound->alarm(true); | |
} | |