#define GYRO IN_1
#define LEFT_MOTOR OUT_C
#define RIGHT_MOTOR OUT_A
#define MOTORS OUT_AC
#define WAIT_TIME 8 // for loop wait time in [ms]
#define WHEEL_RATIO_NXT1 1.0 // NXT 1.0 wheels (56x26 size)
#define WHEEL_RATIO_NXT2 0.7 // NXT 2.0 wheels (43x22 size)
#define WHEEL_RATIO_RCX 1.4 // RCX big wheels (81x15 size)
#define KGYROANGLE 7.5 // body integral gain [5.0, 10.0] [deg]
#define KGYROSPEED 1.15 // body proportional gain [1.2 or so] [deg/s]
#define KPOS 0.07 // motor proportional gain [non-zero] [deg]
#define KSPEED 0.1 // motor derivative gain [0.075, 0.2] [deg/s]
#define EMAOFFSET 0.0005 // Gyro offset compensation (low-pass filter constant)
#define TIME_FALL_LIMIT 1000 // Expiration time, robot fell
#define OFFSET_SAMPLES 100 // # of samples to use in calculating gyro offset
#define CONTROL_WAIT 25
// Global variables
long tCalcStart; // used to calculate interation time
float tInterval; // iteration time [sec]
float ratioWheel = WHEEL_RATIO_NXT2; // set for 43x22 sized wheels
float gOffset; // gyro offset value
float gAngleGlobal = 0; // angle calculated from gyro readings [deg]
float motorPos = 0; // accumulated motor position
long mrcSum = 0, mrcSumPrev; // used to calculate accumulated motor position
long mrcDeltaP3 = 0; // moving average to calculate accumulated motor position
long mrcDeltaP2 = 0;
long mrcDeltaP1 = 0;
void GetGyroOffset() { // compute offset by polling gyro and computing average
float gSum; // store sum of all raw gyro readings
int i; // dummy index variable
int g; // raw gyro reading [deg/s]
int gMin, gMax; // if gyro reading an outlier, then set at min or max value
ClearScreen();
TextOut(0, LCD_LINE4, "Gyro offset:");
TextOut(0, LCD_LINE5, "Lay robot flat");
Off(MOTORS); // ensure motors are off so they don't perturb gyro
do {
gSum = 0.0;
gMin = 1000;
gMax = -1000;
for (i=0; i<OFFSET_SAMPLES; i++) { // collect some gyro readings
g = SensorHTGyro(GYRO);
if (g > gMax) gMax = g;
if (g < gMin) gMin = g;
gSum = gSum + g;
Wait(5);
}
} while ((gMax - gMin) > 1); // Reject and sample again if range too large
gOffset = gSum/OFFSET_SAMPLES; // Compute average
} // end GetGyroOffset
void StartBeeps() { // 5-sec countdown until balance control begins
int c;
ClearScreen();
TextOut(20, LCD_LINE3, "Balancing in");
for (c=5; c>0; c--) { // countdown from 5 seconds
NumOut(47, LCD_LINE4, c);
PlayTone(440,100);
Wait(1000);
}
NumOut(47, LCD_LINE4, 0);
PlayTone(440,1000);
Wait(1000);
} // end of StartBeeps
void GetGyroData(float &gyroSpeed, float &gyroAngle)
{ // read gyro sensor, apply low-pass filter and return [deg/s] and [deg]
float gyroRaw; // raw gyro reading [deg/s]
gyroRaw = SensorHTGyro(GYRO);
gOffset = EMAOFFSET * gyroRaw + (1-EMAOFFSET) * gOffset; // low pass filter
gyroSpeed = gyroRaw - gOffset; // compute gyro speed [deg/s]
gAngleGlobal = gAngleGlobal + gyroSpeed*tInterval; // compute gyro angle [deg]
gyroAngle = gAngleGlobal;
} // end of GetGyroData
void GetMotorData(float &motorSpeed, float &motorPos)
{ // read motor encoder, compute accumulated angle and speed [deg], [deg/s]
long mrcLeft, mrcRight, mrcDelta; // mrc: motor rotation count
mrcLeft = MotorRotationCount(LEFT_MOTOR);
mrcRight = MotorRotationCount(RIGHT_MOTOR);
mrcSumPrev = mrcSum;
mrcSum = mrcLeft + mrcRight;
mrcDelta = mrcSum - mrcSumPrev;
motorPos = motorPos + mrcDelta; // accumulated motor position [deg]
// Implement moving average (use 4 readings) to compute motorSpeed [deg/s]
motorSpeed = (mrcDelta+mrcDeltaP1+mrcDeltaP2+mrcDeltaP3)/(4*tInterval);
mrcDeltaP3 = mrcDeltaP2;
mrcDeltaP2 = mrcDeltaP1;
mrcDeltaP1 = mrcDelta;
} // end of GetMotorData
void CalcInterval(long cLoop)
{ // compute loop iteration time [sec]
if (cLoop == 0) { // 1st time called this function called...
tInterval = 0.0055; // ...so arbitrarily set tInterval. 5.5 ms ok choice
tCalcStart = CurrentTick();
} else {
// Take average of number of times through the loop and use for tInterval
tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000.0); // [sec]
}
} // end of CalcInterval
task main() {
SetSensorLowspeed(IN_4); // Initialize ultrasonic sensor
SetSensorHTGyro(GYRO); // Initialize gyro
Wait(50);
ResetRotationCount (OUT_ABC);
OnFwdSync (OUT_BC,75, 0);
until ((SensorUS (IN_4))< 20);
OffEx (OUT_BC, RESET_ALL);
GetGyroOffset(); // Get gyro offset
StartBeeps(); // Play 5-sec countdown beep before balancing starts
// when main ends, other tasks, like whipBalance, will start
} // end of main
task whipBalance() {
Follows(main);
float gyroSpeed, gyroAngle; // [deg/s] and [deg] from gyro
float motorSpeed; // [deg/s] from motor
int power; // power level to apply to motors
long tMotorPosOK; // timing variable to use for CurrentTick
long cLoop = 0; // used to compute interval time
ClearScreen();
TextOut(0, LCD_LINE4, "Balancing");
tMotorPosOK = CurrentTick();
ResetRotationCount(LEFT_MOTOR); // reset motors ensures angle starts at 0 deg
ResetRotationCount(RIGHT_MOTOR);
while(true) {
CalcInterval(cLoop++); // compute time elapsed between samples
GetGyroData(gyroSpeed, gyroAngle); // body [deg/s] and [deg]
GetMotorData(motorSpeed, motorPos); // motor [deg/s] and [deg]
// PID equation
power = (KGYROSPEED * gyroSpeed + // body vel [deg/s]
KGYROANGLE * gyroAngle) / ratioWheel + // body pos [deg]
KPOS * motorPos + // motor pos [deg]
KSPEED * motorSpeed; // motor vel [deg/s]
if (abs(power) < 100)
tMotorPosOK = CurrentTick();
OnFwd(MOTORS, power); // apply power to both motors
if ((CurrentTick()-tMotorPosOK) > TIME_FALL_LIMIT) { // robot fell
break;
}
Wait(WAIT_TIME); // still balanced, so repeat loop
} // end of while(true). Exits if robot fell
Off(MOTORS);
TextOut(0, LCD_LINE4, "Oops... I fell");
TextOut(0, LCD_LINE8, "tInt ms:");
NumOut(6*8, LCD_LINE8, tInterval*1000);
StopAllTasks(); // kill all running tasks
} // end task whipBalance
task ultraSoundResponse() {
Follows(main);
byte usSensorValue;
while(true) {
// read us sensor
usSensorValue = SensorUS(IN_4); // US sensor value is 0 to 254 [cm]
if(usSensorValue < 30) {
PlayTone(TONE_B3, MS_50); // beep if WhIP within 30 cm of object
}
Wait(100); // wait a bit, say 100 ms, before polling US sensor again
} // end while
} // end ultraSoundResponse