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