void ComputeIMU(void)
{
static int16_t gyroADCprevious[3] = {0,0,0};
int16_t gyroADCp[3];
int16_t gyroADCinter[3];
GetMagData(imu.magADC);
GetAccData(imu.accADC);
GetEstimatedAttitude(&imu);
GetGyroData(imu.gyroADC);
for (byte axis = 0; axis < 3; axis++)
{
gyroADCp[axis] = imu.gyroADC[axis];
}
delayMicroseconds(650); //empirical, interleaving delay between 2 consecutive reads
GetGyroData(imu.gyroADC);
for (byte axis = 0; axis < 3; axis++)
{
gyroADCinter[axis] = imu.gyroADC[axis]+gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
imu.gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis])/3;
gyroADCprevious[axis] = gyroADCinter[axis]>>1;
//imu.accADC[axis]=0;
}
}
void InitIMU(void)
{
InitSensors();
int16_t gyroADC[3];
//Calibration
for (int i = 0; i< 600; i++)
{
GetGyroData(gyroADC);
delay(1);
}
}
//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall. The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle This is the angle of the robot, it is the results of
// integrating on the gyro value.
// Units: degrees
// gyroSpeed The value from the Gyro Sensor after offset subtracted
// Units: degrees/second
// motorPos This is the motor position used for balancing.
// Note that this variable has two sources of input:
// Change in motor position based on the sum of
// MotorRotationCount of the two motors,
// and,
// forced movement based on user driving the robot.
// Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
// motor encoders.
// Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
// power = KGYROSPEED * gyro +
// KGYROANGLE * gyroAngle +
// KPOS * motorPos +
// KSPEED * motorSpeed;
//
task taskBalance()
{
Follows(main);
float gyroSpeed, gyroAngle;
float motorSpeed;
int power, powerLeft, powerRight;
long tMotorPosOK;
long cLoop = 0;
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich beginne");
TextOut(0, LCD_LINE4, "mich nun");
TextOut(0, LCD_LINE4, "selbst zu");
TextOut(0, LCD_LINE6, "Stabilisieren!");
tMotorPosOK = CurrentTick();
// Reset the motors to make sure we start at a zero position
ResetRotationCount(LEFT_MOTOR);
ResetRotationCount(RIGHT_MOTOR);
while(true) {
CalcInterval(cLoop++);
GetGyroData(gyroSpeed, gyroAngle);
GetMotorData(motorSpeed, motorPos);
// Apply the drive control value to the motor position to get robot
// to move.
motorPos -= motorControlDrive * tInterval;
// This is the main balancing equation
power = (KGYROSPEED * gyroSpeed + // Deg/Sec from Gyro sensor
KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
KPOS * motorPos + // From MotorRotaionCount of both motors
KDRIVE * motorControlDrive + // To improve start/stop performance
KSPEED * motorSpeed; // Motor speed in Deg/Sec
if (abs(power) < 100)
tMotorPosOK = CurrentTick();
SteerControl(power, powerLeft, powerRight);
// Apply the power values to the motors
OnFwd(LEFT_MOTOR, powerLeft);
OnFwd(RIGHT_MOTOR, powerRight);
// Check if robot has fallen by detecting that motorPos is being limited
// for an extended amount of time.
if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
break;
}
Wait(WAIT_TIME);
}
Off(MOTORS);
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich bin");
TextOut(0, LCD_LINE4, "hingefallen!");
TextOut(0, LCD_LINE5, "Neustart");
TextOut(0, LCD_LINE5, "erforderlich!");
}
