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 main() {
HTSPBsetupIO(HTSPB, 0x10);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_C4, 1023);
wait1Msec(200);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_DS4, 1023);
wait1Msec(400);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_E4, 1023);
wait1Msec(200);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, 1, 0);
wait1Msec(200);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_F4, 1023);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO1, DAC_MODE_SQUAREWAVE, TONE_A4, 1023);
wait1Msec(200);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_E4, 1023);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO1, DAC_MODE_SQUAREWAVE, TONE_G4, 1023);
wait1Msec(200);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, TONE_C4, 1023);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO1, DAC_MODE_SQUAREWAVE, TONE_E4, 1023);
wait1Msec(400);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO0, DAC_MODE_SQUAREWAVE, 1, 0);
HTSPBwriteAnalog(HTSPB, HTSPB_DACO1, DAC_MODE_SQUAREWAVE, 1, 0);
}