void LeftBridgeBalance()
{
LeftBasket();
goInches(2.5, -speed);
goTurn(143, -speed);
goInches(12.5, speed);
goInches(.5, -speed);
servo[FrontHolder] = 185;
wait1Msec(800);
goInches(4, MountainSpeed);
servo[FrontHolder] = 0;
goInches(29.25, MountainSpeed);
while(1)
{
wait1Msec(2500);
int xAxis;
HTACreadX(Accel, xAxis); //stores the z value to the variable zAxis
nxtDisplayCenteredTextLine(2, "X:"+xAxis);
if (xAxis > 2)
{
goInches(.08, MountainSpeed);
}
else if (xAxis < -24)
{
goInches(.08, -MountainSpeed);
}
else
{
break;
}
}
}
task main () {
int _x_axis1 = 0;
int _y_axis1 = 0;
int _z_axis1 = 0;
int _x_axis2 = 0;
int _y_axis2 = 0;
int _z_axis2 = 0;
string _tmp;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Accel");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
// PlaySound(soundBeepBeep);
// while(bSoundActive);
while (true) {
eraseDisplay();
// You can read the three axes one by one
HTACreadX(HTAC, _x_axis1);
HTACreadY(HTAC, _y_axis1);
HTACreadZ(HTAC, _z_axis1);
// It's better to read them all at once, if you want to know
// all of them anyway. It is a lot more efficient.
if (!HTACreadAllAxes(HTAC, _x_axis2, _y_axis2, _z_axis2)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
nxtDisplayTextLine(0,"HTAC Test 1");
// We can't provide more than 2 parameters to nxtDisplayTextLine(),
// so we'll do in two steps using StringFormat()
nxtDisplayTextLine(2, "T X Y Z");
StringFormat(_tmp, "S:%4d %4d", _x_axis1, _y_axis1);
nxtDisplayTextLine(3, "%s %4d", _tmp, _z_axis1);
StringFormat(_tmp, "A:%4d %4d", _x_axis2, _y_axis2);
nxtDisplayTextLine(4, "%s %4d", _tmp, _z_axis2);
nxtDisplayTextLine(6, "S: 1 by 1");
nxtDisplayTextLine(7, "A: All at once");
wait1Msec(100);
}
}
void RobotBalance()
{
RobotMoveSpeed = 35; // go a little slower than usual
long lastSampleTime = nPgmTime;
int minError = 0;
int maxError = 0;
while ( true )
{
int reading;
HTACreadX(Accelerometer, reading);
int error = reading - kRobotAccReadingBalanced;
if ( error > maxError )
{
maxError = error;
}
if ( error < minError )
{
minError = error;
}
if ( nPgmTime > lastSampleTime + kSampleInterval )
{
if ( MAX(abs(minError), abs(maxError)) > kRobotAccThreshold ) // we're not balanced
{
if ( abs(minError) > kRobotAccThreshold && maxError > kRobotAccThreshold )
{
// we're still oscillating, wait another interval
}
else if ( abs(maxError) > kRobotAccBalancedThreshold )
{
RobotMoveDistance(.75, false); // forwards an inch
}
else if ( abs(minError) > kRobotAccBalancedThreshold )
{
RobotMoveDistance(-.75, false); // backwards an inch
}
}
// clear and prepare for next stuff
minError = 0;
maxError = 0;
lastSampleTime = nPgmTime;
}
}
}
void RightBasketFarBridge()
{
RightBasketFar();
goInches(2, speed);
goTurn(47, speed);
goInches(36, speed);
goInches(10,Minspeed);
wait1Msec(300);
goInches(2, Minspeed);
goTurn(92, speed);
goInches(10, speed);
goInches(1, -speed);
servo[FrontHolder] = 140;
wait1Msec(1500);
goInches(4, MountainSpeed);
servo[FrontHolder] = 0;
goInches(29.25, MountainSpeed);
while(1)
{
wait1Msec(3000);
int xAxis;
HTACreadX(Accel, xAxis); //stores the z value to the variable zAxis
nxtDisplayCenteredTextLine(2, "X:"+xAxis);
if (xAxis > 2)
{
goInches(.1, MountainSpeed);
}
else if (xAxis < -24)
{
goInches(.1, -MountainSpeed);
}
else
{
break;
}
}
}
