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);
}
}
}
task test(){
HTSPBsetupIO(HTSPB, 0b11111000);
while(true){
HTSPBwriteIO(HTSPB, 0b00001000);
wait10Msec(100);
HTSPBwriteIO(HTSPB, 0b00000000);
wait10Msec(100);
}
}
task main() {
int inputdata;
ubyte outputdata;
int bit;
// Set B0 as output
HTSPBsetupIO(HTSPB, 0x1);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 2, 10);
float wrist = (inputdata - rMin);
wrist /= (rMax - rMin);
servo [wristMain] = 255 * wrist;
nxtDisplayTextLine(1, "A2: %d", inputdata);
nxtDisplayTextLine(3, "W: %1.2f", wrist);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
wait1Msec(50);
}
}
task main() {
int sonarvalue;
ubyte outputdata;
int bit;
// The data to be written: 0x3F = 111111 binary,
// makes all digital ports outputs.
HTSPBsetupIO(HTSPB, 0x3F);
while(true) {
// Get the value from the LEGO sound sensor.
sonarvalue = SensorValue(US_PORT);
// Set the output bit based on the analogue input value
bit = sonarvalue / 10;
if (bit > 5) bit = 5;
outputdata = 1 << bit;
eraseDisplay();
nxtDisplayTextLine(1, "%d", sonarvalue);
HTSPBwriteIO(HTSPB, outputdata);
wait1Msec(50);
}
}
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() {
// 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);
}
}
task main() {
int inputdata;
// Set B0 for output
HTSPBsetupIO(HTSPB, 0x1);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
nxtDisplayTextLine(1, "A0: %d", inputdata);
// If A0 is less than 50% of the max value
// turn off the LED, otherwise switch it on
if(inputdata < 512)
HTSPBwriteIO(HTSPB, 0x00);
else
HTSPBwriteIO(HTSPB, 0x01);
wait1Msec(50);
}
}
task main() {
HTSPBsetupIO(HTSPB, 0x10);
while(true) {
// Read the value from the temp sensor
inputdata = HTSPBreadADC(HTSPB, 0, 10);
// Convert to an actual temperature
temperatureC = ((inputdata*3300)/1023-600)/10.0;
nxtDisplayTextLine(1, "Temp: %d C", temperatureC);
// If we're above 28 degrees, switch on the LED
if(temperatureC > THRESHOLD) {
HTSPBwriteIO(HTSPB, 0x10);
} else {
HTSPBwriteIO(HTSPB, 0x00);
}
wait1Msec(50);
}
}
task main() {
int inputdata;
ubyte outputdata;
int bit;
// Set B0 as output
HTSPBsetupIO(HTSPB, 0xFF);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
displayTextLine(1, "A0: %d", inputdata);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
wait1Msec(50);
}
}
task main() {
short inputdata;
ubyte outputdata;
short bit;
// Set all digital IOs as outputs as output
HTSPBsetupIO(HTSPB, 0xFF);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTSPBreadADC(HTSPB, 0, 10);
displayTextLine(1, "A0: %d", inputdata);
// Set the output bit based on the analogue input value
bit = (inputdata/128);
if (bit > 5) bit = 5;
displayTextLine(2, "Bit: %d", bit);
outputdata = 1 << bit;
HTSPBwriteIO(HTSPB, outputdata);
sleep(50);
}
}
void RSLoff(){
HTSPBwriteIO(HTPB,0b00010000);
wait1Msec(5);
HTSPBwriteIO(HTPB,0b00010000);
}
task main()
{
//Holonomic Drive Integers
int x1 = 0;
int y1 = 0;
int x2 = 0;
const int t = 8;
const float driveScale = 0.7;
initializeRobot ();
while ( true ) {
getJoystickSettings(joystick);
//Holonomic Drive
x1 = ( abs( joystick.joy1_x1 ) > t ) ? joystick.joy1_x1 : 0;
y1 = ( abs( joystick.joy1_y1 ) > t ) ? joystick.joy1_y1 : 0;
x2 = ( abs( joystick.joy1_x2 ) > t ) ? joystick.joy1_x2 : 0;
motor[frontLeft] = (y1 + x2 + x1) * driveScale;
motor[frontRight] = (y1 - x2 - x1) * driveScale;
motor[rearLeft] = (y1 + x2 - x1) * driveScale;
motor[rearRight] = (y1 - x2 + x1) * driveScale;
//This is the code for controlling the motorized linear slides that the ring grabber appparatus is mounted on.
//Up
if ((joy2Btn (5) == 1) && (SensorValue [liftUpSensor] == 0))
{
motor [linearSlides] = 100;
}
//Down; after the button statement is the sensor value statement so the lift doesn't go too far down.
else if ((joy2Btn (7) == 1) && (SensorValue [liftDownSensor] == 0 )) {
motor [linearSlides] = -100;
}
else motor [linearSlides] = 0;
//The following is the code for controlling the ring grabber apparatus.
//This is the code for controlling the arm that the grabber is mounted on.
if (joy2Btn (6) == 1 ) {
motor [grabberMount] = 50;
}
else if (joy2Btn (8) == 1) {
motor [grabberMount] = -50;
}
else motor [grabberMount] = 0;
/* The following is the other arm code; this allows the arm to be controlled with the joysticks (in addition to
the buttons. */
//motor [grabberMount] = (-(abs( joystick.joy2_y2 ) > t ) ? joystick.joy2_y2 : 0 );
//This is the code for controlling the grabber itself. It is 'commented' out because we currently do not use the 'active' grabber.
/*
if (joystick.joy2_x2 >110)
/*(joy2Btn (1) == 1)*/
/* if (grabberTarget >0)
{
grabberTarget -= grabberIncrement;
setGrabber (grabberTarget/100.0);
}
if (joystick.joy2_x2 <-110)
/*(joy2Btn (3) == 1)*/
/* if (grabberTarget <100)
{
grabberTarget += grabberIncrement;
setGrabber (grabberTarget/100.0);
} */
//This is the code for controlling the wrist, which the grabber is attatched to.
#if 0
if (joystick.joy2_y1 >110)
/*(joy2Btn (2) == 1)*/
if (wristTarget >0)
{
wristTarget -= wristIncrement;
setWrist (wristTarget/1000.0);
}
if (joystick.joy2_y1 <-110)
/*(joy2Btn (4) == 1)*/
if (wristTarget <1000)
{
wristTarget += wristIncrement;
setWrist (wristTarget/1000.0);
}
#else
const int rMax = 1023;
const int rMin = 407;
int inputdata = HTSPBreadADC(HTSPB, 2, 10);
float wrist = (inputdata - rMin);
wrist /= (rMax - rMin);
servo [wristMain] = (255 * wrist) + ( joystick.joy2_y1 / 3);
#endif
// LED weight sensor lights
#if 1
int inputRed = HTSPBreadADC(HTSPB, 0, 10);
int inputBlue = HTSPBreadADC(HTSPB, 1, 10);
int output = 0x00;
if ( inputRed > 512 )
output |= 0x01;
if ( inputBlue > 512 )
output |= 0x02;
if ( output != lastOutput ) {
HTSPBwriteLED( HTSPB, output );
HTSPBwriteIO( HTSPB, output );
lastOutput = output;
}
//wait1Msec(50);
#endif
/* Perhaps we will switch to this function system at the next meeting or possible time. Uncomment below,
the 'deployRamp' function above, and the latchSetting varible declaration. */
/* if ((joy1Btn (10) == 1) && (joy2Btn (10) == 1))
{
deployRamp (180);
} */
/*This conditional statement releases the ramp latch for the end game. Both 'start' buttons must be
pressed to release the ramp latch. The button combination that must be pressed is intentionally awkward
so that the ramp is not deployed by accident during the first 90 seconds of the teleop phase. */
/* Also, below there is (commented out) the or operator and an additional button combination in which
both drivers press all four colored buttons on the right side of the joysticks to deploy the ramp;
this is not currently implemented because it has not been tested and proven to work. */
if /*(*/((joy1Btn (10) == 1) && (joy2Btn (10) == 1)) /* || ((joy1Btn (1) == 1) && (joy1Btn (2) == 1) &&
(joy1Btn (3) == 1) && (joy1Btn (4) == 1) && (joy2Btn (1) == 1) && (joy2Btn (2) == 1) &&
(joy2Btn (3) == 1) && (joy2Btn (4) == 1))) */
{
servo [rampLatch] = rampLatchOpen;
}
}
}
void writeIO(){
HTSPBwriteIO(HTPB,io);
}
