// Set the sensor to the selected frequency void setHz(tSensors link, byte hertz) { byte msg[4] = { 3, 0x02, 0x41, 0x00 }; if (hertz == 50) { msg[3] = 0x35; } else if (hertz == 60) { msg[3] = 0x36; } else { PlaySound(soundException); eraseDisplay(); nxtDisplayCenteredTextLine(3, "Wrong freq."); nxtDisplayCenteredTextLine(3, "specified"); wait1Msec(5000); StopAllTasks(); } while (nI2CStatus[link] == STAT_COMM_PENDING){ wait1Msec(5); } sendI2CMsg(link, msg[0], 0); if (nI2CStatus[link] == ERR_COMM_BUS_ERR) { PlaySound(soundException); eraseDisplay(); nxtDisplayCenteredTextLine(3, "Error setting"); nxtDisplayCenteredTextLine(3, "sensor to 50Hz"); wait1Msec(5000); StopAllTasks(); } }
int checkParameterRange (float mRot, float mRotPerSec) { if (mRot < paramMRotMin) { nxtDisplayCenteredTextLine(5, "ERR: MtrRots"); nxtDisplayCenteredTextLine(7, "%.0f (min %d)", mRot, paramMRotMin); wait1Msec(5000); StopAllTasks(); return -1; } if (mRotPerSec < paramMRotPerSecMin) { nxtDisplayCenteredTextLine(5, "ERR: MtrSpeed"); nxtDisplayCenteredTextLine(7, "%.2f (min %.2f)", mRotPerSec, paramMRotPerSecMin); wait1Msec(5000); StopAllTasks(); return -1; } if (mRotPerSec > paramMRotPerSecMax) { nxtDisplayCenteredTextLine(5, "ERR: MtrSpeed"); nxtDisplayCenteredTextLine(7, "%.2f (max %.2f)", mRotPerSec, paramMRotPerSecMax); wait1Msec(5000); StopAllTasks(); return -1; } return 1; }
void executeStep(Step *step){ writeDebugStreamLine("Beginning execution of step \"%s\"", step->name); switch (step->type) { case StepDrive: moveTo(step->target, step->power, step->forward); break; case StepTurnDrive: turnAndMoveTo(step->target, step->power, step->forward); break; case StepTurn: turnTo(step->target, step->power, step->forward); break; case StepGrab: grabGoal(); wait1Msec(500); break; case StepRelease: releaseGoal(); wait1Msec(500); break; case StepDone: writeDebugStreamLine("Finished executing Autonomous program"); StopAllTasks(); break; default: writeDebugStreamLine("Unknown step type detected (%d). Quitting now.", step->type); StopAllTasks(); } }
task main () { int _x_accel = 0; int _y_accel = 0; int _z_accel = 0; int _x_tilt = 0; int _y_tilt = 0; int _z_tilt = 0; string _tmp; nxtDisplayCenteredTextLine(0, "Mindsensors"); nxtDisplayCenteredBigTextLine(1, "ACCEL-Nx"); nxtDisplayCenteredTextLine(3, "Test 1"); wait1Msec(2000); // There are four ranges the ACCL-Nx can measure in // up to 2.5G - MSAC_RANGE_2_5 // up to 3.3G - MSAC_RANGE_3_3 // up to 6.7G - MSAC_RANGE_6_7 // up to 10G - MSAC_RANGE_10 MSACsetRange(MSAC, MSAC_RANGE_10); PlaySound(soundBeepBeep); while(bSoundActive); while (true) { eraseDisplay(); // Read the tilt data from the sensor if (!MSACreadTilt(MSAC, _x_tilt, _y_tilt, _z_tilt)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } // Read the acceleration data from the sensor if (!MSACreadAccel(MSAC, _x_accel, _y_accel, _z_accel)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } nxtDisplayTextLine(0,"MSAC Test 1"); // We can't provide more than 2 parameters to nxtDisplayTextLine(), // so we'll do in two steps using StringFormat() nxtDisplayTextLine(2, "Tilt X Y Z"); StringFormat(_tmp, " %4d %4d", _x_tilt, _y_tilt); nxtDisplayTextLine(3, "%s %4d", _tmp, _z_tilt); nxtDisplayTextLine(4, "Acceleration:"); nxtDisplayTextLine(5, "X: %5d mG", _x_accel); nxtDisplayTextLine(6, "Y: %5d mG", _y_accel); nxtDisplayTextLine(7, "Z: %5d mG", _z_accel); wait1Msec(100); } }
task main () { int _color = 0; string _tmp; int red = 0; int green = 0; int blue = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "COLOUR"); nxtDisplayCenteredTextLine(3, "SMUX Test"); nxtDisplayCenteredTextLine(5, "Connect SMUX to"); nxtDisplayCenteredTextLine(6, "S1 and CS to"); nxtDisplayCenteredTextLine(7, "SMUX Port 1"); wait1Msec(2000); eraseDisplay(); while (true) { // Read the currently detected colour from the sensor _color = HTCSreadColor(HTCOLOR); // If colour == -1, it implies an error has occurred if (_color < 0) { nxtDisplayTextLine(4, "ERROR!!"); nxtDisplayTextLine(5, "HTCSreadColor"); wait1Msec(2000); StopAllTasks(); } // Read the RGB values of the currently colour from the sensor // A return value of false implies an error has occurred if (!HTCSreadRGB(HTCOLOR, red, green, blue)) { nxtDisplayTextLine(4, "ERROR!!"); nxtDisplayTextLine(5, "HTCSreadRGB"); wait1Msec(2000); StopAllTasks(); } // Read the RGB values of the currently colour from the sensor // A return value of false implies an error has occurred nxtDisplayCenteredTextLine(0, "Color: %d", _color); nxtDisplayCenteredBigTextLine(1, "R G B"); nxtEraseRect(0,10, 99, 41); nxtFillRect( 0, 10, 30, 10 + (red+1)/8); nxtFillRect(35, 10, 65, 10 + (green+1)/8); nxtFillRect(70, 10, 99, 10 + (blue+1)/8); StringFormat(_tmp, " %3d %3d", red, green); nxtDisplayTextLine(7, "%s %3d", _tmp, blue); wait1Msec(100); } }
task main() { int adch0, adch1, adch2, adch3, adch4 = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Proto"); nxtDisplayCenteredTextLine(3, "Test 3"); nxtDisplayCenteredTextLine(5, "Connect HTPB"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); while(true) { // get the value for ADC channels, we want a 10 bit answer if (!HTPBreadAllADC(HTPB, adch0, adch1, adch2, adch3, adch4, 10)) { eraseDisplay(); PlaySound(soundException); nxtDisplayTextLine(5, "ERR READING ADC"); wait1Msec(2000); StopAllTasks(); } nxtDisplayBigTextLine(1, "ADC VALS"); nxtDisplayTextLine(3, "CH0: %4d", adch0); nxtDisplayTextLine(4, "CH1: %4d", adch1); nxtDisplayTextLine(5, "CH2: %4d", adch2); nxtDisplayTextLine(6, "CH3: %4d", adch3); nxtDisplayTextLine(7, "CH4: %4d", adch4); wait1Msec(100); } }
task main () { int keys = 0; unsigned byte key[] = {0}; int number = 0; string output; nxtDisplayCenteredTextLine(0, "Mindsensors"); nxtDisplayCenteredBigTextLine(1, "NumPad"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); eraseDisplay(); nxtDisplayCenteredTextLine(0, "Mindsensors NP"); nxtDisplayTextLine(1, "-------------------"); nxtDisplayTextLine(5, "-------------------"); nxtDisplayTextLine(7, "X: no key"); while (true) { // Which key is being pressed now? if (!MSNPscanKeys(MSNP, keys, key[0], number)) StopAllTasks(); // "convert" to string so we can print it output = key; nxtDisplayTextLine(3, "Numpad Key: %s", output); nxtDisplayTextLine(4, "Numpad Num: %d", number); wait1Msec(100); } }
task main() { //waits for the initialisation pmInitWaitForStart(); //init motion and StartTask motion_Init(); //init Sensors sensors_Init(); //init IA ia_Init(); // Starts the IA ia_start(); //GO 90s Sleep(90000); // Stop all motors //Stop tasks and Robot StopAllTasks(); }
task ohno() { while(true) { if(!everPressed && time1[T4] > 25000) { StopAllTasks(); } } }
task main() { int _chVal = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Proto"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect HTPB"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); // Setup all the digital IO ports as outputs (0xFF) if (!HTPBsetupIO(HTPB, 0xFF)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } while(true) { eraseDisplay(); // get the value for ADC channel 0, we want a 10 bit answer _chVal = HTPBreadADC(HTPB, 0, 10); nxtDisplayTextLine(4, "A0: %d", _chVal); // if _chVal is more than 512, turn on the LED, otherwise turn it off. if (_chVal > 512) { if (!HTPBwriteIO(HTPB, 0xFF)) nxtDisplayTextLine(5, "ERR WRITE"); } else { if (!HTPBwriteIO(HTPB, 0x00)) nxtDisplayTextLine(5, "ERR WRITE"); } wait1Msec(100); } }
task main() { eraseDisplay(); T("pie2"); //Calibrate(); Delete(sFileName, nIoResult); nFileSize = 21 * 10000; // room for 10,000 entries OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); if (nIoResult != ioRsltSuccess) { T("Open failed"); writeDebugStream("nAvailFlash: %d", nAvailFlash); StopAllTasks(); } PressBumperToContinue(); FollowSegment(); motor[LEFT] = 0; motor[RIGHT] = 0; LLsleep(LINELEADER); Close(hFileHandle, nIoResult); PlaySound(soundFastUpwardTones); wait1Msec(2000); }
float autoRotate(float angle){ //direct is 0,1,2,3,4,5,6,7 is like top hat: 0 forward, 1 45 to front right, etc. //distance is in inches //diagonal range is different from other directions //int enctarget,enccount,enctogo,signX,signY,power; //int usedEnc; int rotp; float fGyroTarget=fGyroAngle+angle; float angleerror=angle; expectedGyro+=angle; int safetime=(int)floor(abs(angle)*200+200); //about 1.5 second for 45 ClearTimer(T4); while(abs(fGyroAngle-fGyroTarget)>0.5 && time1[T4]<safetime){ rotp=angleerror>0?20:-20; goRot(rotp); //goGyro(0,0,fGyroTarget); angleerror=fGyroTarget-fGyroAngle; //nxtDisplayBigTextLine(1,"%f",angleerror); } allDriveStop(); if(time1[T4]>=safetime) StopAllTasks(); return fGyroTarget; }
task main () { // Get control over the buttons nNxtButtonTask = -2; eraseDisplay(); nxtDisplayTextLine(0, "HTCS Test 2"); nxtDisplayTextLine(2, "Press orange"); nxtDisplayTextLine(3, "button to start"); nxtDisplayTextLine(4, "calibration."); nxtDisplayTextLine(5, "Press grey"); nxtDisplayTextLine(6, "button to exit."); while(nNxtButtonPressed != kEnterButton) EndTimeSlice(); eraseDisplay(); nxtDisplayTextLine(3, "Starting"); nxtDisplayTextLine(4, "calibration."); // This call calibrates the white value. if (!HTCScalWhite(HTCS)) { eraseDisplay(); PlaySound(soundException); nxtDisplayTextLine(3, "ERROR!!"); nxtDisplayTextLine(5, "Calibration"); nxtDisplayTextLine(6, "failed!!"); wait1Msec(2000); StopAllTasks(); } wait1Msec(1000); }
void resetEncoders() { nMotorEncoderTarget[motorJ] = 0; nMotorEncoderTarget[motorI] = 0; motor[motorJ] = -10; motor[motorI] = 10; nMotorEncoder[motorE] = 0; nMotorEncoder[motorF] = 0; nMotorEncoder[motorG] = 0; nMotorEncoder[motorH] = 0; while(true) { if(nMotorEncoder[motorJ] <= 0) { motor[motorJ] = 0; } if(nMotorEncoder[motorI] >= 0) { motor[motorI] = 0; } wait10Msec(1); eraseDisplay(); nxtDisplayTextLine(6, "I %d", nMotorEncoder[motorI]); nxtDisplayTextLine(7, "J %d", nMotorEncoder[motorJ]); if(nMotorEncoder[motorJ] <=0 && nMotorEncoder[motorI] >=0) { StopAllTasks(); } } }
task main() { nNxtButtonTask = -2; nxtDisplayCenteredTextLine(0, "Codatex"); nxtDisplayCenteredBigTextLine(1, "RFID"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); eraseDisplay(); nxtDisplayCenteredTextLine(3, "Start single"); nxtDisplayCenteredTextLine(4, "reading loop"); wait1Msec(2000); eraseDisplay(); // loop for transponder readings in single read mode while(nNxtButtonPressed == kNoButton) { // read the transponder, if no mode (single/continuous) has been // set, it will default to single shotting the sensor. if (!CTRFIDreadTransponder(CTRFID, transponderID)) { eraseDisplay(); nxtDisplayTextLine(3, "Error reading"); nxtDisplayTextLine(4, "from sensor!"); wait10Msec(5000); StopAllTasks(); } nxtDisplayCenteredTextLine(3, "Transponder ID:"); nxtDisplayCenteredTextLine(4, "%s", transponderID); // Be sure to add about 200ms after each read // or you end up getting 0000000000 as a transponder address wait1Msec(200); } }
// // checkLink() // -- check if there is at least one connected // -- if not, try 3times to reconnnect // -- if it still fails after 3 attempts, stop all tasks // -- return to caller // -- if it is successful or already have one queue connected to a remote unit // -- void checkLink() { int nTries=0; setFriendlyName(MASTER); while (nTries<3 && nBTCurrentStreamIndex < 1) { PlaySound(soundLowBuzz); PlaySound(soundLowBuzz); eraseDisplay(); nxtDisplayCenteredTextLine(3, "BT Not Connected!"); nxtDisplayCenteredTextLine(4, "Connecting!"); btConnect(1, ENGINE); wait1Msec(1000); ++nTries; } if (nBTCurrentStreamIndex < 1)// -- check if there is at least one connected StopAllTasks(); // exit the entire process while (bBTBusy) { wait1Msec(100); } nxtDisplayCenteredTextLine(3, "StreamIndex=%d", nBTCurrentStreamIndex); nxtDisplayCenteredTextLine(4, "Connect to Q %d", Queue1); wait1Msec(1000); return; }
task main() { // Going forward at full speed go_forward(100); nxtDisplayCenteredTextLine (3, "Going Forward"); wait1Msec (Drive_Time); // Going left at half speed go_left(50); nxtDisplayCenteredTextLine (3, "Turning Left"); wait1Msec (Turn_Time); go_forward(100); nxtDisplayCenteredTextLine (3, "Going Forward"); wait1Msec (Drive_Time); go_left(50); nxtDisplayCenteredTextLine (3, "Turning Left"); wait1Msec (Turn_Time); go_forward(100); nxtDisplayCenteredTextLine (3, "Going Forward"); wait1Msec (Drive_Time); go_left(50); nxtDisplayCenteredTextLine (3, "Turning Left"); wait1Msec (Turn_Time); go_forward(100); nxtDisplayCenteredTextLine (3, "Going Forward"); wait1Msec (Drive_Time); go_left(50); nxtDisplayCenteredTextLine (3, "Turning Left"); wait1Msec (Turn_Time); StopAllTasks (); }
task main() { initializeRobotAuto(); waitForStart(); // turn towards rolling goal spinClockWise(40, 500); // move forward towards goal moveFwd(50, 4850); // back little bit moveBwd(50,1250); // turn towards bridge spinClockWise(80,4800); //move across moveBwd(80,9800); StopAllTasks(); //stop all the motors from moving. StopAllMotors(); }
void putLiftDownAuto(){ //put lift down int safetime; hogCPU(); motor[M_Lift]=-50; safetime=1000; ClearTimer(T4); while( nMotorEncoder[M_Lift]>800 && time1[T4]<safetime){}; if(time1[T4]>=safetime) { motor[M_Lift]=0; StopAllTasks(); } motor[M_Lift]=-10; safetime=1000; ClearTimer(T4); while( nMotorEncoder[M_Lift]>100 && time1[T4]<safetime){}; if(time1[T4]>=safetime) { motor[M_Lift]=0; StopAllTasks(); } motor[M_Lift]=0; releaseCPU(); }
task main() { int batteryVoltage; nI2CBytesReady[kSc8Port] = 0; int pos; buttonValue = 0; // Setup button handler nNxtButtonTask = -2; StartTask(button_handler); nxtDisplayTextLine(0, "mindsensors.com"); nxtDisplayTextLine(1, "NXTServo Module"); SensorType[kSc8Port] = sensorI2CCustom9V; /* * When the servoPos is set to 1500 mS the * servo will be in neutral position. * * The extreme servo positions lie between * 500 mS to 2500 mS on both sides of the neutral position. */ while(1) { batteryVoltage = NXTServo_Get_Batt_V(kSc8Port, kSc8ID); nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage); pos = 1000; nxtDisplayTextLine(3, "Position: %d ", pos); NXTServo_SetPosition(kSc8Port, kSc8ID, 1, pos); wait10Msec(100); pos = 2000; nxtDisplayTextLine(3, "Position: %d ", pos); NXTServo_SetPosition(kSc8Port, kSc8ID, 1, pos); // wait10Msec(100); /* * code below is same functionality as above * - using different API. */ /* pos = 100; nxtDisplayTextLine(3, "Position: %d ", pos); NXTServo_Quick_Servo_Setup(kSc8Port, kSc8ID, 1, pos); wait10Msec(100); pos = 200; nxtDisplayTextLine(3, "Position: %d ", pos); NXTServo_Quick_Servo_Setup(kSc8Port, kSc8ID, 1, pos); wait10Msec(100); */ } StopAllTasks(); }
task FollowEdge(){ if(!isDark()){ panic("Not on an edge!"); wait10Msec(1000); StopAllTasks(); } followingEdge = true; edgeFollow(followingEdge); }
void TestPSPAnalog () { psp currState; PSP_ReadButtonState(SensorPort, Addr, currState); if ((int)currState.l_j_y == 99 && (int)currState.r_j_y == 99) { PlaySound(soundDownwardTones); wait1Msec(500); StopAllTasks(); } }
task main() { signed int servoPos, servoPos2; int batteryVoltage; nI2CBytesReady[kSc8Port] = 0; int direction; buttonValue = 0; direction = 1; // Setup button handler nNxtButtonTask = -2; StartTask(button_handler); nxtDisplayTextLine(0, "mindsensors.com"); nxtDisplayTextLine(1, "NXTServo Module"); SensorType[kSc8Port] = sensorI2CCustom9V; /* * When the servoPos is set to 1500 mS the * servo will be in neutral position. * * The extreme servo positions lie between * 500 mS to 2500 mS on both sides of the neutral position. */ servoPos=1500; while(1) { if ( servoPos >= 2500 ) { direction = -1; // move towards right } if ( servoPos <= 500 ) { direction = 1; // move torwards left } servoPos += direction * 20; batteryVoltage = NXTServo_Get_Batt_V(kSc8Port, kSc8ID); nxtDisplayTextLine(2, "V batt= %d mV", batteryVoltage); nxtDisplayTextLine(3, "Servo = %d uS", servoPos); NXTServo_SetPosition(kSc8Port, kSc8ID, 1, servoPos); wait10Msec(20); /* * based on teh button pressed move the servo2 to * new position */ nxtDisplayTextLine(4, "button = %d", buttonValue); servoPos2 = 1500 + (buttonValue * 100); NXTServo_SetPosition(kSc8Port, kSc8ID, 2, servoPos2); } StopAllTasks(); }
void panic(string text) { motor[left] = 0; motor[right] = 0; PlaySound(soundException); //eraseDisplay(); nxtDisplayCenteredTextLine(1, text); wait10Msec(1000); StopAllTasks(); }
task main () { int powerLeft, powerRight; int d_left_X, d_left_Y; int d_right_X, d_right_Y; int hopper_up, hopper_down, hopper_score; int fingers_up, fingers_down, fingers_up_encoder, fingers_down_encoder; int trigger_pressed; int ok_to_drive = false; int joystick_click; psp currState; nNxtExitClicks = 3; // Triple clicking EXIT button will terminate program nI2CBytesReady[SensorPort] = 0; SensorType[SensorPort] = sensorI2CMuxController; wait10Msec (100); while (true) { PSP_ReadButtonState(SensorPort, Addr, currState); // joysticks d_left_X = (int)currState.l_j_x; d_left_Y = (int)currState.l_j_y; d_right_X = (int)currState.r_j_x; d_right_Y = (int)currState.r_j_y; // left button pad hopper_down = !(bool)currState.d; hopper_up = !(bool)currState.c || !(bool)currState.a; hopper_score = !(bool)currState.b; // right button pad fingers_up_encoder = !(bool)currState.triang; fingers_down_encoder = !(bool)currState.cross; fingers_up = !(bool)currState.square; fingers_down = !(bool)currState.circle; // any of the trigger buttons trigger_pressed = (!(bool)currState.l1) || (!(bool)currState.l2) || (!(bool)currState.r1) || (!(bool)currState.r2); joystick_click = !(bool)currState.l_j_b || !(bool)currState.r_j_b; nxtDisplayTextLine(1,"Left: %d", d_left_Y); nxtDisplayTextLine(2,"Right: %d", d_right_Y); nxtDisplayTextLine(3,"Hopper: %d", hopper_score); nxtDisplayTextLine(4,"Fingers: %d", fingers_down); nxtDisplayTextLine(5,"Triggers: %d", trigger_pressed); nxtDisplayTextLine(5,"Joy Click: %d", joystick_click); } wait10Msec (100); StopAllTasks (); }
task main() { int AnalogPins = 0; // analog input byte DigitalPins = 0; // all digital inputs int RawReading = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Proto"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayCenteredTextLine(5, "Connect HTPB"); nxtDisplayCenteredTextLine(6, "to S1"); wait1Msec(2000); // Setup all the digital IO ports as outputs (0x10) 010000 pin B4 = output, others are inputs. if (!HTPBsetupIO(HTPB, 0x10)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } while(true) { //reset the reading value RawReading = 0; //clear the NXT eraseDisplay(); //get the analog values for(int i = 0; i < 5; i++) { AnalogPins = HTPBreadADC(HTPB, i, 10); nxtDisplayTextLine(i, "A0: %d", AnalogPins); if (AnalogPins > 512) { RawReading += (1 << (4 + (4 - i)); } } //get the digital values DigitalPins = HTPBreadIO(HTPB, 0x3F); nxtDisplayTextLine(5, "D: 0x%x", DigitalPins); RawReading += (DigitalPins&0x01) << 3; RawReading += (DigitalPins&0x02) << 1; RawReading += (DigitalPins&0x04) >> 1; RawReading += (DigitalPins&0x08) >> 3; nxtDisplayBigTextLine(6, "%d", RawReadingToDegrees(RawReading)); //let other threads do shit wait1Msec(50); }
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 update_flight_record() { TFileIOResult nIoResult; static int lastMotorE = 0; static int lastMotorF = 0; static int lastMotorJ = 0; static int lastMotorI = 0; static int lastwrist = 0; static int lastMotorG = 0; static int lastMotorH = 0; string str = " "; if (motor[motorE] == lastMotorE && motor[motorF] == lastMotorF && motor[motorJ] == lastMotorJ && motor[motorI] == lastMotorI && motor[motorG] == lastMotorG && motor[motorH] == lastMotorH && ServoValue[servo1] == lastwrist) { return; } else { StringFormat(str, "{%d,", time1(T4)); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorE]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorF]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorJ]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorI]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorG]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorH]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d},\n", ServoValue[servo1]); WriteText(flightDatFH, nIoResult, str); } if (nIoResult != 0) { Close(flightDatFH, nIoResult); wait10Msec(50); StopAllTasks(); } lastMotorE = motor[motorE]; lastMotorF = motor[motorF]; lastMotorJ = motor[motorJ]; lastMotorI = motor[motorI]; lastMotorG = motor[motorG]; lastMotorH = motor[motorH]; lastwrist = ServoValue[servo1]; }
task main () { long red = 0; long green = 0; long blue = 0; bool passive = false; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Color V2"); nxtDisplayCenteredTextLine(3, "Test 2"); nxtDisplayCenteredTextLine(5, "Connect sensor"); nxtDisplayCenteredTextLine(6, "to S1"); nxtDisplayCenteredTextLine(7, "[Enter] to switch"); wait1Msec(2000); eraseDisplay(); nxtDisplayCenteredTextLine(7, "[Enter] to switch"); while (true) { while(nNxtButtonPressed != kEnterButton) { // Read the RAW RGB values from the sensor, these are // 10 bit values returned from the internal sensor. // You can read these values with or without the aid of the // ambient light cancelling white LED. This is done with // the "passive" variable. If it is set to true, the white LED // will be switched off, otherwise the LED is used to cancel // out the ambient light. // A return value of false implies an error has occurred if (!HTCS2readRawRGB(HTCS2, passive, red, green, blue)) { nxtDisplayTextLine(4, "ERROR!!"); wait1Msec(2000); StopAllTasks(); } nxtDisplayBigTextLine(0, "R: %5d", red); nxtDisplayBigTextLine(2, "G: %5d", green); nxtDisplayBigTextLine(4, "B: %5d", blue); wait1Msec(100); } // Toggle the "passive" variable to enable or disable // the ambient light cancelling white LED on the sensor if (nNxtButtonPressed == kEnterButton) { passive = !passive; PlaySound(soundBeepBeep); while (bSoundActive) { wait1Msec(10); } // Wait until no button is pressed (debounce) while (nNxtButtonPressed != kNoButton) { wait1Msec(10); } } } }
//============================================================================ void update_flight_record() { TFileIOResult nIoResult; static int lastMotorE = 0; static int lastMotorF = 0; static int lastMotorJ = 0; static int lastMotorG = 0; static int lastMotorH = 0; static int lastMotorI = 0; string str = " "; if (motor[motorE] == lastMotorE && //compare all the current values motor[motorF] == lastMotorF && //"Is E the same as last I checked AND F the same as last I checked AND..." motor[motorG] == lastMotorG && motor[motorH] == lastMotorH && motor[motorI] == lastMotorI && motor[motorJ] == lastMotorJ) { return; //Yes? All of them? return to start } else //No? Do the following { StringFormat(str, "{%d,", time1(T4)); // find the time WriteText(flightDatFH, nIoResult, str);//write it down StringFormat(str, "%d,", motor[motorE]);//What is the value of the motor? WriteText(flightDatFH, nIoResult, str);//Write it down StringFormat(str, "%d,", motor[motorF]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorJ]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorI]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorG]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d,", motor[motorH]); WriteText(flightDatFH, nIoResult, str); StringFormat(str, "%d},\n", ServoValue[servo1]); WriteText(flightDatFH, nIoResult, str); } if (nIoResult != 0) { Close(flightDatFH, nIoResult); wait10Msec(50); StopAllTasks(); } lastMotorE = motor[motorE]; lastMotorF = motor[motorF]; lastMotorG = motor[motorG]; lastMotorH = motor[motorH]; lastMotorI = motor[motorI]; lastMotorJ = motor[motorJ]; }