void init(){//**********************************INIT****************** HTSPBsetupIO(HTPB,0b00111000);//0 in 1 out //light up for battery servo[bucket]=120; ubyte in=HTSPBreadIO(HTPB,0b00001111);//get autonomous settings! bDisplayDiagnostics=false; nMotorEncoder[back]=0; nMotorEncoder[grab]=0; nMotorEncoder[motorC]=0; ClearTimer(T4); HTSPBwriteIO(HTPB,turning); while(nNxtButtonPressed!=3){ if(HTSPBreadIO(HTPB,0b01000000)>0)STARTINGPOS=1; else STARTINGPOS=0; delay=(int)(30000.0*((float)readAnalogInput(HTPB,0)/1023.0)); nxtDisplayCenteredBigTextLine(0,"WAITING!"); nxtDisplayCenteredBigTextLine(2,"%d",HTSPBreadIO(HTPB,0b11111111)); nxtDisplayCenteredBigTextLine(4,"%d",STARTINGPOS); nxtDisplayCenteredBigTextLine(6,"%d",delay); HTSPBwriteAnalog(HTPB,HTSPB_DACO0,DAC_MODE_SQUAREWAVE,1,620); } HTSPBwriteAnalog(HTPB,HTSPB_DACO0,DAC_MODE_DCOUT,1,0); eraseDisplay(); nxtDisplayCenteredBigTextLine(0,"READY!"); calibrateGyro(); reset(); }
task graph(){ while(true){ nxtDisplayCenteredBigTextLine(0,"%d",HTSPBreadIO(HTPB,doneTurning)); nxtDisplayCenteredBigTextLine(3,"%d",HTSPBreadIO(HTPB,haveTube)); wait1Msec(100); } }
task main() { // The data to be written: 0x30 = 110000 binary, // makes B4,B5 digital ports outputs. HTSPBsetupIO(HTSPB, 0x03); while(true) { // Turn off the LED HTSPBwriteIO(HTSPB, 0x00); eraseDisplay(); nxtDisplayTextLine(2, "running"); // Wait a random time between 3 and 8 seconds. wait1Msec(random(5000) + 3000); // Switch on the LED and reset the timer HTSPBwriteIO(HTSPB, 0x01); time1[T1] = 0; // Wait for user to press the stop button while (HTSPBreadIO(HTSPB, 0x30) != 0x10) { wait1Msec(5); } eraseDisplay(); nxtDisplayTextLine(2, "Time: %d", time1[T1]); // Wait for user to reset while (HTSPBreadIO(HTSPB, 0x30) != 0x30) { wait1Msec(5); } } }
bool hasTube(){ bool have=false; if(HTSPBreadIO(HTPB,0b00000010)>=1){ have=false; } else{ if(HTSPBreadIO(HTPB,0b00000010)==0)have=true; } return have; }
task input(){ while(true){ ubyte in=HTSPBreadIO(HTPB,0b00000010); int gyro=readAnalogInput(HTPB,1); int l=readAnalogInput(HTPB,0); //int acc=readAnalogInput(HTPB,0); //float z=readAnalogVolts(HTPB,0); // nxtDisplayCenteredBigTextLine(0,"%d",in); // nxtDisplayCenteredBigTextLine(2,"%d",l); // nxtDisplayCenteredBigTextLine(4,"%d",SensorValue[color]); nxtDisplayCenteredBigTextLine(0,"%d",nMotorEncoder[arm]); nxtDisplayCenteredBigTextLine(2,"%d",e); nxtDisplayCenteredBigTextLine(4,"%d",de); nxtDisplayCenteredBigTextLine(6,"%d",pid); wait1Msec(10); if(in>=1)havetube=true;//inverted else havetube=false; if(bDisconnected){ status=1; } else if(externalBattery<0){ status=2; }else status=0; } }
task graph(){ while(true){ nxtDisplayCenteredBigTextLine(0,"%d",theta); nxtDisplayCenteredBigTextLine(3,"%d",HTSPBreadIO(HTPB,0b00000010)); wait1Msec(100); } }
void init(){ HTSPBsetupIO(HTPB,0b11000000); //light up for battery batteryUpdate(HTPB); servo[bucket]=120; ubyte in=HTSPBreadIO(HTPB,0b00001111);//get autonomous settings! bDisplayDiagnostics=false; nMotorEncoder[back]=0; nMotorEncoder[grab]=0; nMotorEncoder[motorC]=0; int sum=0; for(int i=0;i<5;i++){ sum+=readAnalogInput(HTPB,1); wait1Msec(100); } stationaryVoltage=sum/5; while(nNxtButtonPressed!=3){ if(HTSPBreadIO(HTPB,0b00000100)>0)STARTINGPOS=1; nxtDisplayCenteredBigTextLine(4,"%d",STARTINGPOS); } nxtDisplayCenteredBigTextLine(0,"WAITING"); }
void rotateRightLP(ubyte amount, int pw){ setStrobe(HTPB,turnR | amount); io=io|turning; writeIO(); motor[left]=pw; motor[right]=pw; while(HTSPBreadADC(HTPB,almost,10)<600); motor[left]=20; motor[right]=20; while(!HTSPBreadIO(HTPB,doneTurning)); drive(0); io=io& ~turning; io=io|doneTurning; writeIO(); }
task main() { // The data to be written: 0x10 = 010000 binary, // makes B4 digital port an output. HTSPBsetupIO(HTSPB, 0x10); while(true) { if(HTSPBreadIO(HTSPB, 0x01) == 0) { eraseDisplay(); displayTextLine(1, "Magnet present"); HTSPBwriteIO(HTSPB, 0x10); } else { eraseDisplay(); displayTextLine(1, "Magnet absent"); HTSPBwriteIO(HTSPB, 0x00); } wait1Msec(50); } }