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!"); }