//--------------------------------------------------------------------------------------
// Func: ResetCenter
// Desc: Utility for resetting tracker center. This is only called during tracker
// startup. Button press resets are handled during filtering. (Needs refactor)
//--------------------------------------------------------------------------------------
void ResetCenter()
{
resetValues = 0;
// Not sure what Dennis is doing here. Giving it
// time to stabilize, since this is called at setup time?
for (unsigned char k = 0; k < 250; k++)
{
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
FilterSensorData();
}
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
FilterSensorData();
panAngle = magAngle[2];
tiltAngle = accAngle[0];
rollAngle = accAngle[1];
TrackerStarted = 1;
}
//--------------------------------------------------------------------------------------
// Func: CalibrateMag
// Desc:
//--------------------------------------------------------------------------------------
void CalibrateMag()
{
WriteToI2C(HMC_ADDR, 0x00, 0b00010001);
// MM: Again with the loops. Not sure what purpose this serves, Dennis.
for (unsigned char i = 0; i < 40; i++)
{
UpdateSensors();
}
magPosOff[0] = magRaw[0];
magPosOff[1] = magRaw[1];
magPosOff[2] = magRaw[2];
WriteToI2C(HMC_ADDR, 0x00, 0b00010010);
for (unsigned char i = 0; i < 40; i++)
{
UpdateSensors();
}
magNegOff[0] = magRaw[0];
magNegOff[1] = magRaw[1];
magNegOff[2] = magRaw[2];
WriteToI2C(HMC_ADDR, 0x00, 0b00010000);
magGain[0] = -2500 / float(magNegOff[0] - magPosOff[0]);
magGain[1] = -2500 / float(magNegOff[1] - magPosOff[1]);
magGain[2] = -2500 / float(magNegOff[2] - magPosOff[2]);
for (unsigned char i = 0; i < 40; i++)
{
UpdateSensors();
}
#if (DEBUG)
Serial.print("Mag cal:");
Serial.print(magNegOff[0] - magPosOff[0]);
Serial.print(",");
Serial.print(magNegOff[1] - magPosOff[1]);
Serial.print(",");
Serial.print(magNegOff[2] - magPosOff[2]);
Serial.print(",");
Serial.print(magGain[0]);
Serial.print(",");
Serial.print(magGain[1]);
Serial.print(",");
Serial.println(magGain[2]);
Serial.print("Mag offset:");
Serial.print(magOffset[0]);
Serial.print(",");
Serial.print(magOffset[1]);
Serial.print(",");
Serial.println(magOffset[2]);
#endif
}
void IMU::Calibrate( float dt, bool all )
{
if ( mCalibrationAccum < 2000 ) {
bool last_pass = ( mCalibrationAccum >= 1999 );
for ( auto dev : Sensor::Devices() ) {
if ( all or dynamic_cast< Gyroscope* >( dev ) != nullptr ) {
dev->Calibrate( dt, last_pass );
}
}
} else if ( all and mCalibrationAccum < 3000 ) {
UpdateSensors( dt );
mGravity += mAcceleration / 1000.0f;
} else {
mState = CalibrationDone;
mAcceleration = Vector3f();
mGyroscope = Vector3f();
mMagnetometer = Vector3f();
mRPY = Vector3f();
mdRPY = Vector3f();
mRate = Vector3f();
gDebug() << "Calibration done !\n";
}
mCalibrationAccum++;
}
void CBuzzController::ControlStep() {
/* Update debugging information */
m_sDebug.Clear();
if(m_sDebug.Trajectory.Tracking) {
const CCI_PositioningSensor::SReading& sPosRead = m_pcPos->GetReading();
m_sDebug.TrajectoryAdd(sPosRead.Position);
}
/* Take care of the rest */
if(m_tBuzzVM && m_tBuzzVM->state == BUZZVM_STATE_READY) {
ProcessInMsgs();
UpdateSensors();
if(buzzvm_function_call(m_tBuzzVM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "[ROBOT %u] %s: execution terminated abnormally: %s\n\n",
m_tBuzzVM->robot,
m_strBytecodeFName.c_str(),
ErrorInfo().c_str());
for(UInt32 i = 1; i <= buzzdarray_size(m_tBuzzVM->stacks); ++i) {
buzzdebug_stack_dump(m_tBuzzVM, i, stdout);
}
return;
}
ProcessOutMsgs();
}
else {
fprintf(stderr, "[ROBOT %s] Robot is not ready to execute Buzz script.\n\n",
GetId().c_str());
}
}
//--------------------------------------------------------------------------------------
// Func: SetGyroOffset
// Desc: Sets the gyro offset.
//--------------------------------------------------------------------------------------
void SetGyroOffset()
{
// Not sure what the outer loops are for, Dennis. Stabilization time?
//
for (unsigned int i = 0; i < 100; i++)
{
UpdateSensors();
for (unsigned char k = 0; k < 3; k++)
{
gyroOff[k] += gyroRaw[k];
}
}
for (unsigned char k = 0; k < 3; k++)
{
gyroOff[k] = gyroOff[k] / 100;
}
#if (DEBUG)
Serial.print("Gyro offset measured:");
Serial.print(gyroOff[0]);
Serial.print(",");
Serial.print(gyroOff[1]);
Serial.print(",");
Serial.println(gyroOff[2]);
#endif
}
void CBuzzController::ControlStep() {
ProcessInMsgs();
UpdateSensors();
if(buzzvm_function_call(m_tBuzzVM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "[ROBOT %u] %s: execution terminated abnormally: %s\n\n",
m_tBuzzVM->robot,
m_strBytecodeFName.c_str(),
buzzvm_strerror(m_tBuzzVM));
buzzvm_dump(m_tBuzzVM);
}
ProcessOutMsgs();
}
void CBuzzController::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing");
try {
m_pcPos = GetSensor <CCI_PositioningSensor>("positioning");
}
catch(CARGoSException& ex) {}
/* Get the script name */
std::string strBCFName;
GetNodeAttributeOrDefault(t_node, "bytecode_file", strBCFName, strBCFName);
/* Get the script name */
std::string strDbgFName;
GetNodeAttributeOrDefault(t_node, "debug_file", strDbgFName, strDbgFName);
/* Initialize the rest */
bool bIDSuccess = false;
m_unRobotId = 0;
/* Find Buzz ID */
size_t tStartPos = GetId().find_last_of("_");
if(tStartPos != std::string::npos){
/* Checks for ID after last "_" ie. footbot_group3_10 -> 10 */
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos+1));
bIDSuccess = true;
}
/* FromString() returns 0 if passed an invalid string */
if(!m_unRobotId || !bIDSuccess){
/* Checks for ID after first number footbot_simulated10 -> 10 */
tStartPos = GetId().find_first_of("0123456789");
if(tStartPos != std::string::npos){
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos));
bIDSuccess = true;
}
}
if(!bIDSuccess) {
THROW_ARGOSEXCEPTION("Error in finding Buzz ID from name \"" << GetId() << "\"");
}
if(strBCFName != "" && strDbgFName != "")
SetBytecode(strBCFName, strDbgFName);
else
m_tBuzzVM = buzzvm_new(m_unRobotId);
UpdateSensors();
/* Set initial robot message (id and then all zeros) */
CByteArray cData;
cData << m_tBuzzVM->robot;
while(cData.Size() < m_pcRABA->GetSize()) cData << static_cast<UInt8>(0);
m_pcRABA->SetData(cData);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
float GetFilteredSensorValue(void) {
UpdateSensors();
int counter = 0;
static float previous = 0.0;
float value= 0.0f;
int i;
for (i = 0; i < 8; i++) {
if (sensors[i] == 1.0)
counter++;
}
// We consider to have passed the starting line if more than 3 sensors were hit
if (counter > 3) {
if (!lapFlag) {
lapCounter++;
sprintf(lap,"%d",lapCounter);
//updateScreen = true;
}
lapFlag = true;
previous = 0.0;
return 0.0;
}
lapFlag = false;
//Calculate a percentage of the direction based on which sensor was hit
if (sensors[0])
value = 1.0;
else if (sensors[7])
value = -1.0;
else if (sensors[1])
value = 0.7;
else if (sensors[6])
value = -0.7;
else if (sensors[2])
value = 0.35;
else if (sensors[5])
value = -0.35;
else if (sensors[3])
value = 0.0;
else if (sensors[4])
value = -0.0;
else
value = previous;
previous = value;
return value;
}
bool IMU::SensorsThreadRun()
{
if ( mState == Running ) {
bool rate = ( mMain->stabilizer()->mode() == Stabilizer::Rate );
float dt = ((float)( Board::GetTicks() - mSensorsThreadTick ) ) / 1000000.0f;
UpdateSensors( dt, rate );
mSensorsThreadTickRate = Board::WaitTick( 1000000 / 500, mSensorsThreadTickRate, -200 );
imu_fps++;
if ( Board::GetTicks() - imu_ticks >= 1000 * 1000 * 5 ) {
gDebug() << "Sampling rate : " << ( imu_fps / 5 ) << " Hz\n";
imu_fps = 0;
imu_ticks = Board::GetTicks();
}
} else {
usleep( 1000 * 100 );
}
return true;
}
void CBuzzController::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing");
/* Get the script name */
std::string strFName;
GetNodeAttribute(t_node, "bytecode_file", strFName);
/* Initialize the rest */
m_tBuzzVM = NULL;
m_unRobotId = FromString<UInt16>(GetId().substr(2));
SetBytecode(strFName);
UpdateSensors();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
//--------------------------------------------------------------------------------------
// Func: Filter
// Desc: Filters / merges sensor data.
//--------------------------------------------------------------------------------------
void FilterSensorData()
{
int temp = 0;
// Used to set initial values.
if (resetValues == 1)
{
resetValues = 0;
tiltStart = 0;
panStart = 0;
rollStart = 0;
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
panAngle = 0;
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
}
// Simple FilterSensorData, uses mainly gyro-data, but uses accelerometer to compensate for drift
rollAngle = (rollAngle + ((gyroRaw[0] - gyroOff[0]) * cos((tiltAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((tiltAngle - 90) / 57.3)) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[1] * (1 - gyroWeightTiltRoll);
tiltAngle = (tiltAngle + ((gyroRaw[1] - gyroOff[1]) * cos((rollAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((rollAngle - 90) / 57.3) * -1) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[0] * (1 - gyroWeightTiltRoll);
panAngle = (panAngle + ((gyroRaw[2] - gyroOff[2]) * cos((tiltAngle - 90) / 57.3) + (((gyroRaw[0] - gyroOff[0]) * -1) * (sin((tiltAngle - 90) / 57.3)) ) + ( ((gyroRaw[1] - gyroOff[1]) * 1) * (sin((rollAngle - 90) / 57.3)))) / (SAMPLERATE * SCALING_FACTOR)) * GyroWeightPan + magAngle[2] * (1 - GyroWeightPan);
if (TrackerStarted)
{
// All low-pass filters
tiltAngleLP = tiltAngle * tiltRollBeta + (1 - tiltRollBeta) * lastTiltAngle;
lastTiltAngle = tiltAngleLP;
rollAngleLP = rollAngle * tiltRollBeta + (1 - tiltRollBeta) * lastRollAngle;
lastRollAngle = rollAngleLP;
panAngleLP = panAngle * panBeta + (1 - panBeta) * lastPanAngle;
lastPanAngle = panAngleLP;
float panAngleTemp = panAngleLP * panInverse * panFactor;
if ( (panAngleTemp > -panMinPulse) && (panAngleTemp < panMaxPulse) )
{
temp = servoPanCenter + panAngleTemp;
channel_value[htChannels[0]] = (int)temp;
}
float tiltAngleTemp = (tiltAngleLP - tiltStart) * tiltInverse * tiltFactor;
if ( (tiltAngleTemp > -tiltMinPulse) && (tiltAngleTemp < tiltMaxPulse) )
{
temp = servoTiltCenter + tiltAngleTemp;
channel_value[htChannels[1]] = temp;
}
float rollAngleTemp = (rollAngleLP - rollStart) * rollInverse * rollFactor;
if ( (rollAngleTemp > -rollMinPulse) && (rollAngleTemp < rollMaxPulse) )
{
temp = servoRollCenter + rollAngleTemp;
channel_value[htChannels[2]] = temp;
}
}
}
//--------------------------------------------------------------------------------------
// Func: Filter
// Desc: Filters / merges sensor data.
//--------------------------------------------------------------------------------------
void FilterSensorData()
{
signed int temp = 0;
// Used to set initial values.
if (resetValues == 1)
{
#if FATSHARK_HT_MODULE
digitalWrite(BUZZER, HIGH);
#endif
resetValues = 0;
tiltStart = 0;
panStart = 0;
rollStart = 0;
UpdateSensors();
GyroCalc();
AccelCalc();
MagCalc();
panAngle = 0;
tiltStart = accAngle[0];
panStart = magAngle[2];
rollStart = accAngle[1];
#if FATSHARK_HT_MODULE
digitalWrite(BUZZER, LOW);
#endif
}
// Simple FilterSensorData, uses mainly gyro-data, but uses accelerometer to compensate for drift
rollAngle = (rollAngle + ((gyroRaw[0] - gyroOff[0]) * cos((tiltAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((tiltAngle - 90) / 57.3)) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[1] * (1 - gyroWeightTiltRoll);
tiltAngle = (tiltAngle + ((gyroRaw[1] - gyroOff[1]) * cos((rollAngle - 90) / 57.3) + (gyroRaw[2] - gyroOff[2]) * sin((rollAngle - 90) / 57.3) * -1) / (SAMPLERATE * SCALING_FACTOR)) * gyroWeightTiltRoll + accAngle[0] * (1 - gyroWeightTiltRoll);
panAngle = (panAngle + ((gyroRaw[2] - gyroOff[2]) * cos((tiltAngle - 90) / 57.3) + (((gyroRaw[0] - gyroOff[0]) * -1) * (sin((tiltAngle - 90) / 57.3)) ) + ( ((gyroRaw[1] - gyroOff[1]) * 1) * (sin((rollAngle - 90) / 57.3)))) / (SAMPLERATE * SCALING_FACTOR)) * GyroWeightPan + magAngle[2] * (1 - GyroWeightPan);
if (TrackerStarted)
{
// All low-pass filters
tiltAngleLP = tiltAngle * tiltRollBeta + (1 - tiltRollBeta) * lastTiltAngle;
lastTiltAngle = tiltAngleLP;
rollAngleLP = rollAngle * tiltRollBeta + (1 - tiltRollBeta) * lastRollAngle;
lastRollAngle = rollAngleLP;
panAngleLP = panAngle * panBeta + (1 - panBeta) * lastPanAngle;
lastPanAngle = panAngleLP;
panAngleTemp = panAngleLP * panInverse * panFactor;
/* temp=constrain((servoPanCenter+panAngleTemp),panMinPulse,panMaxPulse);
channel_value[htChannels[0]]=(int)temp;
*/
temp = servoPanCenter + (int)panAngleTemp;
if ( (temp > panMinPulse) && (temp < panMaxPulse) )
{
channel_value[htChannels[0]] = temp;
}
float tiltAngleTemp = (tiltAngleLP - tiltStart) * tiltInverse * tiltFactor;
//temp=constrain((servoTiltCenter+tiltAngleTemp),tiltMinPulse,tiltMaxPulse);
//channel_value[htChannels[1]]=(int)temp;
temp = servoTiltCenter + (int)tiltAngleTemp;
if ( (temp > tiltMinPulse) && (temp < tiltMaxPulse) )
{
channel_value[htChannels[1]] = temp;
}
float rollAngleTemp = (rollAngleLP - rollStart) * rollInverse * rollFactor;
//temp=constrain((servoRollCenter+rollAngleTemp),rollMinPulse,rollMaxPulse);
//channel_value[htChannels[2]]=(int)temp;
temp = servoRollCenter + (int)rollAngleTemp;
if ( (temp > rollMinPulse) && (temp < rollMaxPulse) )
{
channel_value[htChannels[2]] = temp;
}
}
}
