void initializeRobot()
{
//Stop DC motors
motor[Right1] = 0;
motor[Right2] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
//Stop NXT motors
motor[motorA] = 0;
motor[motorB] = 0;
motor[motorC] = 0;
//Initialize Servos to starting positions
servoChangeRate[servo1] = 10; //Set how fast servo moves. ROBOTC default is 10
servoChangeRate[servo2] = 10;
// Servo range: 0 - 255
servo[WAMRightServo] = 127; // WAM Right Side
servo[WAMLeftServo] = 127; // WAM Left Side
servo[RampServo] = 127; // Ramp
//Initialize Sensors
HTPBsetupIO(HTPB, 0x0); //initializes the protoboard
nMotorEncoder[elevatorA] = 0; //resets the elevator encoder
nMotorEncoder[RAMright] = 0;
nMotorEncoder[RAMleft] = 0;
return;
}
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);
}
}
// The following initialization code runs before the robot receives the start match command from the Field Control System
void initializeRobot()
{
//Stop DC motors
motor[Right1] = 0;
motor[Right2] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
//Stop NXT motors
motor[motorA] = 0;
motor[motorB] = 0;
motor[motorC] = 0;
//Initialize Servos to starting positions
servoChangeRate[WAMrightServo] = 10; //Set how fast servo moves. ROBOTC default is 10
servoChangeRate[WAMleftServo] = 10;
// Servo range: 0 - 255
servo[WAMrightServo] = WAMrightServoStart; // WAM Right Side
servo[WAMleftServo] = WAMleftServoStart; // WAM Left Side
servo[RampServo] = 127; // Ramp
//zeros gyros
initialize_gyro();
StartTask(process_gyro);
wait1Msec(1000);
//Initialize Sensors
HTPBsetupIO(HTPB, 0x0); //initializes the protoboard
nMotorEncoder[elevatorA] = 0; //resets the elevator encoder
return;
}
task main() {
// The data to be written: 0x30 = 110000 binary,
// makes B4,B5 digital ports outputs.
HTPBsetupIO(HTPB, 0x30);
while(true) {
// Turn off the LED
HTPBwriteIO(HTPB, 0x00);
eraseDisplay();
nxtDisplayTextLine(2, "running");
// Wait a random time between 5 and 10 seconds.
wait1Msec(random(5000) + 5000);
// Switch on the LED and reset the timer
HTPBwriteIO(HTPB, 0x10);
time1[T1] = 0;
// Wait for user to press the stop button
while (HTPBreadIO(HTPB, 0xF3) != 0x01) {
wait1Msec(5);
}
eraseDisplay();
nxtDisplayTextLine(2, "%d", time1[T1]);
// Wait for user to reset
while (HTPBreadIO(HTPB, 0xF3) != 0x03) {
wait1Msec(5);
}
}
}
// code to look at the limit switches. It might want to be it's own task and have the limit switches be globals so we can shut off the motors easily from anywhere in the code
// Note: swithes are "1" when pressed and "0" when not pressed
void ProcessProto()
{
ubyte byteInput;
int B3, B4;
HTPBsetupIO(HTPB, 0x0);
byteInput = HTPBreadIO (HTPB, 0x3f); //fetch the state of all the digital IO pins. They default as inputs.
// the "& 0b__" clears the other values except the one identified in the binary string so the only "1" is for the desired position
// the ">>" is a bit shift so that when say only the second swithc is switched, then the binary would be like "00010" but with the ">>1" opperator
// it becomes "0001". this makes it so that the value of each of the variables is a 0 or 1 regardless of what other switches are pressed.
//B0 input
bottomLimit = ((byteInput)& 0b001); //currently, this is the bottom limit switch
//B1 input
ringLimit = ((byteInput) & 0b010) >> 1; //currently, is the switch fror the gripper
//B2 input
ramLimit = ((byteInput) & 0b100)>> 2;
//B3 input
B3 = ((byteInput) & 0b1000) >> 3;
//B4 input
B4 = ((byteInput) & 0b10000)>> 4;
//for debugging purpose, to make sure the two switches are working electrically
//nxtDisplayTextLine(0, "%d", (int) bottomLimit);
//nxtDisplayTextLine(1, "%d", (int) ringLimit);
// wait10Msec(10);
}
task main() {
// Local variables
int soundlevel;
ubyte outputdata;
// The data to be written: 0x3F = 111111 binary,
// makes all digital ports outputs.
HTPBsetupIO(HTPB, 0x3F);
while(true) {
// Get the value from the LEGO sound sensor.
soundlevel = 1023 - SensorValue[SOUND_PORT];
eraseDisplay();
nxtDisplayTextLine(1, "%d", soundlevel);
// Depending on the input voltage on A0,
// turn on the corresponding LED.
outputdata = 0x01;
outputdata = 0x01;
if (soundlevel > 65) outputdata = 0x02;
if (soundlevel > 108) outputdata = 0x04;
if (soundlevel > 180) outputdata = 0x08;
if (soundlevel > 300) outputdata = 0x10;
if (soundlevel > 500) outputdata = 0x20;
HTPBwriteIO(HTPB, outputdata);
wait1Msec(50);
}
}
task main() {
// Local variables
int inputdata;
ubyte outputdata;
// The data to be written: 0x3F = 111111 binary,
// makes all digital ports outputs.
HTPBsetupIO(HTPB, 0x3F);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTPBreadADC(HTPB, 0, 10);
eraseDisplay();
nxtDisplayTextLine(1, "%d", inputdata);
// Depending on the input voltage on A0,
// turn on the corresponding LED.
outputdata = 0x01;
if (inputdata > 31) outputdata = 0x02;
if (inputdata > 63) outputdata = 0x04;
if (inputdata > 127) outputdata = 0x08;
if (inputdata > 255) outputdata = 0x10;
if (inputdata > 511) outputdata = 0x20;
HTPBwriteIO(HTPB, outputdata);
wait1Msec(50);
}
}
task main() {
// The data to be written: 0x3F = 111111 binary,
// makes all digital ports outputs.
HTPBsetupIO(HTPB, 0x3F);
while(true) {
// Switch off LED on port B0
HTPBwriteIO(HTPB, 0x00);
sleep(30);
wolight = HTPBreadADC(HTPB, 0, 10);
// Switch on LED on port B0
HTPBwriteIO(HTPB, 0x01);
sleep(30);
wlight = HTPBreadADC(HTPB, 0, 10);
// Calculate the difference
lightdelta = wlight - wolight;
eraseDisplay();
displayTextLine(1, "%4d", wlight);
displayTextLine(2, "%4d", wolight);
displayTextLine(3, "%4d", lightdelta);
sleep(30);
}
}
task main()
{
//StartTask(process_gyro);
ubyte byteInput;
int intInput;
int switchNumber = 5;
int buttonVal = 0;
int B0, B1, B2, B3, B4;
HTPBsetupIO(HTPB, 0x0);
while(true)
{
//nxtDisplayTextLine(3, "Switch Pressed");
// Fetch the state of the digital IO pins. When not explicitly
// configured as input or output, they will default to input.
byteInput = HTPBreadIO(HTPB, 0x3f);
intInput = ((int)byteInput-32);
nxtDisplayTextLine(0, "%d", intInput);
nxtDisplayTextLine(1, "------------------");
byteInput = (ubyte)intInput;
B0 = ((byteInput)<< 11) >>11;
B1 = (byteInput) >> 1;
B2 = (byteInput) >> 2;
B3 = (byteInput) >> 3;
B4 = (byteInput) >> 4;
nxtDisplayTextLine(1, "%d", (int) (B0));
if (B0 % 2 == 1 || B0 == 1)
nxtDisplayTextLine(3, "1");
else
nxtDisplayTextLine(3, "0");
if (B1 == 1)
buttonVal = 2;
else
buttonVal = 0;
if (B2 == 1)
buttonVal = 3;
else
buttonVal = 0;
if (B3 == 1)
buttonVal = 4;
else
buttonVal = 0;
if (B4 == 1)
buttonVal = 5;
else
buttonVal = 0;
wait10Msec(10);
}
}
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);
}
// The following initialization code runs before the robot receives the start match command from the Field Control System
void initializeRobot()
{
//Stop DC motors
motor[Right1] = 0;
motor[Right2] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
//Stop NXT motors
motor[motorA] = 0;
motor[motorB] = 0;
motor[motorC] = 0;
//Initialize Servos to starting positions
servoChangeRate[WAMrightServo] = 10; //Set how fast servo moves. ROBOTC default is 10
servoChangeRate[WAMleftServo] = 10;
// Servo range: 0 - 255
servo[WAMrightServo] = WAMrightServoStart; // WAM Right Side
servo[WAMleftServo] = WAMleftServoStart; // WAM Left Side
servo[colorSensorServo] = colorServoDown;
servo[RingServo] = 90;
//zeros gyros
initialize_gyro();
StartTask(process_gyro);
wait1Msec(1000);
//Initialize Sensors
HTPBsetupIO(HTPB, 0x0); //initializes the protoboard
ProcessProto();
while(bottomLimit == 0)
{
ProcessProto();
motor[elevatorA] = -50;
motor[elevatorB] = -50;
}
motor[elevatorA] = 0;
motor[elevatorB] = 0;
nMotorEncoder[elevatorA] = 0; //resets the elevator encoder
motor[RAMright] = 60;
motor[RAMleft] = 60;
wait1Msec(500);
motor[RAMright] = 10;
motor[RAMleft] = 10;
return;
}
task main()
{
int err;
// Setup all the digital IO ports as outputs (0xFF)
err = HTPBsetupIO(HTPB, 0x0F);
if (!err) {
nxtDisplayTextLine(4, "Error setting up digital outputs, %d", err);
StopAllTasks();
}
displayEndgame();
wait1Msec(1000);
}
task main() {
// The data to be written: 0x10 = 010000 binary,
// makes B4 digital port an output.
HTPBsetupIO(HTPB, 0x10);
while(true) {
if(HTPBreadIO(HTPB, 0xF3) == 0) {
eraseDisplay();
nxtDisplayTextLine(1, "Magnet present");
HTPBwriteIO(HTPB, 0x10);
} else {
eraseDisplay();
nxtDisplayTextLine(1, "Magnet absent");
HTPBwriteIO(HTPB, 0x00);
}
wait1Msec(50);
}
}
task main() {
// The data to be written: 0x3F = 111111 binary,
// makes B4 digital port an output.
HTPBsetupIO(HTPB, 0x3F);
while(true) {
// Read the value from the temp sensor
inputdata = HTPBreadADC(HTPB, 0, 10);
// Convert to an actual temperature
temperature = ((inputdata - 186) * 32 / 99);
eraseDisplay();
nxtDisplayTextLine(1, "%d", temperature);
// If we're above 28 degrees, switch on the LED
if(temperature > THRESHOLD) {
HTPBwriteIO(HTPB, 0x10);
} else {
HTPBwriteIO(HTPB, 0x00);
}
wait1Msec(50);
}
}
task main() {
// The data to be written: 0x30 = 110000 binary,
// makes B4,B5 digital ports outputs.
HTPBsetupIO(HTPB, 0x30);
while(true) {
// Turn off all outputs
HTPBwriteIO(HTPB, 0x00);
eraseDisplay();
nxtDisplayTextLine(2, "running");
wait1Msec(random(5000) + 5000);
// Turn on a random LED
if(random(1) > 0) {
HTPBwriteIO(HTPB, 0x10);
buttonmask = 0x01;
} else {
HTPBwriteIO(HTPB, 0x20);
buttonmask = 0x02;
}
time1[T1] = 0;
while ((ubyte)HTPBreadIO(HTPB, 0xF3) != buttonmask) {
wait1Msec(5);
}
eraseDisplay();
nxtDisplayTextLine(2, "%d", time1[T1]);
while (HTPBreadIO(HTPB, 0xF3) != 0x03) {
wait1Msec(5);
}
}
}
task main() {
// Local variables
int inputdata;
// The data to be written: 0x3F = 111111 binary,
// makes all digital ports outputs.
HTPBsetupIO(HTPB, 0x3F);
while(true) {
// Read a 10bit wide analogue value from A0
inputdata = HTPBreadADC(HTPB, 0, 10);
eraseDisplay();
nxtDisplayTextLine(1, "%d", inputdata);
// If A0 is less than 50% of the max value
// turn off the LED, otherwise switch it on
if(inputdata < 512)
HTPBwriteIO(HTPB, 0x00);
else
HTPBwriteIO(HTPB, 0x01);
wait1Msec(50);
}
}
task main() {
int _chVal = 0; // analog input
byte inputs = 0; // all digital inputs
int value = 0;
int touchValue = 0;
int touchValue2 = 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 (0x30) 110000 pins B4/B5 = outputs, others are inputs.
if (!HTPBsetupIO(HTPB, 0x30)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
while(true) {
value = 0;
eraseDisplay();
// get the value for ADC channel 0, we want a 10 bit answer
int j = 0;
for(int i=0; i<5; i++)
{
_chVal = HTPBreadADC(HTPB, j, 10);
switch(j) {
case 0: nxtDisplayTextLine(0, "A0: %d", _chVal);
if(_chVal>512) value+=(1<<8);
break;
case 1: nxtDisplayTextLine(1, "A1: %d", _chVal);
if(_chVal>512) value+=(1<<7);
break;
case 2: nxtDisplayTextLine(2, "A2: %d", _chVal);
if(_chVal>512) value+=(1<<6);
break;
case 3: nxtDisplayTextLine(3, "A3: %d", _chVal);
if(_chVal>512) value+=(1<<5);
break;
case 4: nxtDisplayTextLine(4, "A4: %d", _chVal);
if(_chVal>512) value+=(1<<4);
break; }
j++;
}
inputs = HTPBreadIO(HTPB, 0x3F);
nxtDisplayTextLine(5, "D: 0x%x", inputs);
value+=(inputs&0x01)<<3;
value+=(inputs&0x02)<<1;
value+=(inputs&0x04)>>1;
value+=(inputs&0x08)>>3;
nxtDisplayBigTextLine(6, "%d", mapNineBitToDegrees(value));
touchValue = SensorValue[S2];
touchValue2 = SensorValue[S3];
if(touchValue == 1 && touchValue2 == 1) { HTPBwriteIO(HTPB, 0x30); } // turn on B4 and B5
else if(touchValue == 1) { HTPBwriteIO(HTPB, 0x10); } // turn on B4
else if(touchValue2 == 1) { HTPBwriteIO(HTPB, 0x20); } // turn on B5
else { HTPBwriteIO(HTPB, 0x00); } // turn both B4 and B5 off
wait1Msec(50);
}
}
void setupI2C(short port) {
// The data to be written: 0x10 = 010000 binary,
// makes B4 digital port an output.
HTPBsetupIO(HTPB, port);
}
task main()
{
ubyte byteInput;
int intInput;
int switchNumber = 5;
int buttonVal = 0;
/*
typedef struct
{
int B0;
int B1;
int B2;
int B3;
int B4;
} proto_input;
*/
int B0, B1, B2, B3, B4;
HTPBsetupIO(HTPB, 0x0);
while(true)
{
//nxtDisplayTextLine(3, "Switch Pressed");
// Fetch the state of the digital IO pins. When not explicitly
// configured as input or output, they will default to input.
byteInput = HTPBreadIO(HTPB, 0x3f);
intInput = ((int)byteInput-32);
nxtDisplayTextLine(0, "%d", intInput);
nxtDisplayTextLine(1, "------------------");
byteInput = (ubyte)intInput;
B0 = (byteInput) ;
B1 = (byteInput) >> 1;
B2 = (byteInput) >> 2;
B3 = (byteInput) >> 3;
B4 = (byteInput) >> 4;
if (B0 % 2 == 1 || B0 == 1)
buttonVal = 1;
else
buttonVal = 0;
if (B1 != 0)
buttonVal = 2;
else
buttonVal = 0;
if (B2 == 1)
buttonVal = 3;
else
buttonVal = 0;
if (B3 == 1)
buttonVal = 4;
else
buttonVal = 0;
if (B4 == 1)
buttonVal = 5;
else
buttonVal = 0;
wait10Msec(10);
}
}
