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; } } }