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