task main()
{
bNxtLCDStatusDisplay = true;
HTMCsetTarget(HTMC);
while(HTMCreadRelativeHeading(HTMC)>-90){
motor[motorD] = 15;
motor[motorE] = 15;
motor[motorF] = 15;
motor[motorG] = 15;
}
stopMotors();
HTMCsetTarget(HTMC);
wait10Msec(10);
while(true){
checkBTLinkConnected();
readMultipleDataMsgs();
wait1Msec(1);
}
return;
}
void calibrate() {
wait10Msec(20);
while(nNxtButtonPressed != kEnterButton) {
nxtDisplayCenteredTextLine(0, "Robonauts");
nxtDisplayCenteredTextLine(1, "Offense");
nxtDisplayCenteredBigTextLine(3, "Calibrate Compass");
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(Compass));
nxtDisplayCenteredTextLine(6, "Press Enter");
}
eraseDisplay();
nxtDisplayTextLine(2, "Setting");
nxtDisplayTextLine(3, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(Compass);
PlaySound(soundBlip);
while(bSoundActive) {
EndTimeSlice();
}
while(nNxtButtonPressed != kEnterButton) {
nxtDisplayCenteredTextLine(0, "Robonauts");
nxtDisplayCenteredTextLine(1, "Offense");
nxtDisplayCenteredBigTextLine(3, "START ROBOT");
nxtDisplayCenteredTextLine(6, "Press Enter");
}
eraseDisplay();
}
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
servo[gravityShelf] = SHELFDOWN;
servo[IRServo] = IRUP;
/*
* Assume lined up perpendicular to the pegs.
*/
HTMCsetTarget(HTMC);
nMotorPIDSpeedCtrl[driveLeft] = mtrSpeedReg;
nMotorPIDSpeedCtrl[driveRight] = mtrSpeedReg;
nMotorPIDSpeedCtrl[driveSide] = mtrSpeedReg;
/*
* Do not let the motors coast
*/
bFloatDuringInactiveMotorPWM = false;
// the default DSP mode is 1200 Hz.
tHTIRS2DSPMode mode = DSP_1200;
// set the DSP to the new mode
HTIRS2setDSPMode(IRSeeker, mode);
return;
}
task main()
{
bNxtLCDStatusDisplay = true;
HTMCsetTarget(HTMC);
wait10Msec(10);
while(true){
checkBTLinkConnected();
readMultipleDataMsgs();
wait1Msec(1);
}
return;
}
task main () {
int _target = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Compass");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
// Reset the target no more than once a second
// This also helps with debouncing the [enter] button.
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Changing");
nxtDisplayTextLine(2, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(HTCOMPASS);
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
time1[T1] = 0;
}
// Get the true heading and relative heading from the sensor and
// display them on the screen.
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
nxtDisplayTextLine(2, "Target: %4d", _target);
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(HTCOMPASS));
nxtDisplayTextLine(5, "Rel: %4d", HTMCreadRelativeHeading(HTCOMPASS));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to set target");
wait1Msec(100);
}
}
}
task main(){
int offset = 45;
int waitTime = 0;
initializeRobot();
nVolume = 4;
//allow the user to select a delay before te robot begins the autonomous routine
while(nNxtButtonPressed != 3){
if(nNxtButtonPressed == 1){
while(nNxtButtonPressed == 1){}
waitTime+=1;
}
if(nNxtButtonPressed == 2){
while(nNxtButtonPressed == 2){}
waitTime-=1;
}
if(waitTime < 0){waitTime = 0;}
nxtDisplayCenteredBigTextLine(4, "Time: %1.d", waitTime);
//play a sound until the user selects a time
if(!bSoundActive){PlaySound(soundBlip);}
}
waitTime = waitTime*1000;
nVolume = 1;
HTMCsetTarget(compass);
waitForStart();
//wait for the pre-designated time, just in case our alliance partner
//needs time
wait1Msec(waitTime);
HTMCsetTarget(compass);
//move arm to ready position
/*while(!moveTo(arm, 3000, 30, 100, 20)){}
for(int i = 0; i < 3; i++){
motor[arm] = 100;
wait1Msec(500);
motor[arm] = -100;
wait1Msec(600);
}
motor[arm] = 0;
polarDrive(x1,x2,y1,y2,100,offset+0,0);
wait1Msec(300);
polarDrive(x1,x2,y1,y2,-100,offset+0,0);
wait1Msec(300);
polarDrive(x1,x2,y1,y2,0,offset+0,0);
*/
while(!moveTo(arm, 10000, 30, 100, 20)){}
//move along the crates while watching for the beacon
//when beacon encountered, slow down and dump block
bool dropped = false;
while(nMotorEncoder[x2] > -9500){
if(HTIRS2readDCDir(IR) == 6){
if (dropped == false){
stopPolarMot(x1,x2,y1,y2);
polarDrive(x1,x2,y1,y2,60,offset+90,0);
wait1Msec(400);
stopPolarMot(x1,x2,y1,y2);
servo[scoop] = 250;
wait1Msec(1000);
servo[scoop] = 0;
polarDrive(x1,x2,y1,y2,60,offset-90,0);
dropped = true;
}
}
else {
polarDrive(x1,x2,y1,y2,60,offset-90,0);
}
}
nMotorEncoder[x2] = 0;
while(nMotorEncoder[x2] > -4500){
polarDrive(x1,x2,y1,y2,75,offset+0,0);
}
stopPolarMot(x1,x2,y1,y2);
while(HTMCreadRelativeHeading(compass) > -70){
polarDrive(x1,x2,y1,y2,0,0,-75);
}
stopPolarMot(x1,x2,y1,y2);
nMotorEncoder[x2] = 0;
while(nMotorEncoder[x2] < 5000){
polarDrive(x1,x2,y1,y2,50,offset+180,0);
}
stopPolarMot(x1,x2,y1,y2);
}
task main () {
int _target = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Compass");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set target");
nxtDisplayCenteredTextLine(7, " ");
wait1Msec(2000);
eraseDisplay();
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
// Reset the target no more than once a second
// This also helps with debouncing the [enter] button.
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Changing");
nxtDisplayTextLine(2, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(msensor_S1_1);
PlaySound(soundBlip);
while(bSoundActive);
time1[T1] = 0;
}
// Get the true heading and relative heading from the sensor and
// display them on the screen.
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
nxtDisplayTextLine(2, "Target: %4d", _target);
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(msensor_S1_1));
nxtDisplayTextLine(5, "Rel: %4d", HTMCreadRelativeHeading(msensor_S1_1));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to set target");
wait1Msec(100);
}
}
}
int setCompassZero() {
HTMCsetTarget(COMPASS);
}
