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; }
task compass () { int _target = 0; nxtDisplayCenteredTextLine(0, "HiTechnic"); nxtDisplayCenteredBigTextLine(1, "Compass"); nxtDisplayCenteredTextLine(3, "Test 1"); nxtDisplayTextLine(5, "Press enter"); nxtDisplayTextLine(6, "to set target"); 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(HTMC); 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(HTMC)); nxtDisplayTextLine(5, "Rel: %4d", HTMCreadRelativeHeading(HTMC)); cmps = HTMCreadRelativeHeading(HTMC); nxtDisplayTextLine(6, "Press enter"); nxtDisplayTextLine(7, "to set target"); wait1Msec(100); } } }
task main() { calibrate(); while(true) { int relCompass = HTMCreadRelativeHeading(Compass); nxtDisplayTextLine(5, "Rel: %4d", relCompass); if(relCompass < -nbound) { move(CC); } else if(relCompass > nbound) { move(CW); } else { move(ST); } } }
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); }
void turntocmps(int gcmps, int loxmps){ int lcmps; if (gcmps-HTMCreadRelativeHeading(HTMC) >= 180){lcmps = (gcmps-HTMCreadRelativeHeading(HTMC)) - 360;} if (gcmps-HTMCreadRelativeHeading(HTMC) < 180 && gcmps-HTMCreadRelativeHeading(HTMC) >= 0){lcmps = gcmps-HTMCreadRelativeHeading(HTMC) ;} if (gcmps-HTMCreadRelativeHeading(HTMC) >= -180 && gcmps-HTMCreadRelativeHeading(HTMC) < 0){lcmps = gcmps-HTMCreadRelativeHeading(HTMC);} if (gcmps-HTMCreadRelativeHeading(HTMC) < -180){lcmps = 360+(gcmps-HTMCreadRelativeHeading(HTMC)) ;} if(lcmps > 0){ if(HTMCreadRelativeHeading(HTMC) != loxmps){ flagged = false; motor[motorD] = -15; motor[motorE] = 15; motor[motorF] = -15; motor[motorG] = 15; } } // stopMotors(); if(lcmps < 0){ if(HTMCreadRelativeHeading(HTMC) != loxmps){ flagged = false; motor[motorD] = 15; motor[motorE] = -15; motor[motorF] = 15; motor[motorG] = -15; } } if (gcmps-HTMCreadRelativeHeading(HTMC) >= 180){lcmps = (gcmps-HTMCreadRelativeHeading(HTMC)) - 360;} if (gcmps-HTMCreadRelativeHeading(HTMC) < 180 && gcmps-HTMCreadRelativeHeading(HTMC) >= 0){lcmps = gcmps-HTMCreadRelativeHeading(HTMC) ;} if (gcmps-HTMCreadRelativeHeading(HTMC) >= -180 && gcmps-HTMCreadRelativeHeading(HTMC) < 0){lcmps = gcmps-HTMCreadRelativeHeading(HTMC);} if (gcmps-HTMCreadRelativeHeading(HTMC) < -180){lcmps = 360+(gcmps-HTMCreadRelativeHeading(HTMC)) ;} if(abs(loxmps-HTMCreadRelativeHeading(HTMC)) < 5){ flagged = true; } }
void readMultipleDataMsgs() { TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while (true) { // Check to see if a message is available nSizeOfMessage = cCmdMessageGetSize(kQueueID); if (nSizeOfMessage <= 0) { wait1Msec(1); // Give other tasks a chance to run break; // No more message this time } if (nSizeOfMessage > kMaxSizeOfMessage) nSizeOfMessage = kMaxSizeOfMessage; nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, kQueueID); cmps = 180+(nRcvBuffer[0]-nRcvBuffer[1]); int oxmps; if(nRcvBuffer[1] == 0){oxmps = nRcvBuffer[0];} if(nRcvBuffer[0] == 0){oxmps = -nRcvBuffer[1];} nxtDisplayTextLine(3, "button: %4d", nRcvBuffer[2]); nxtDisplayTextLine(4, "Abs: %4d", oxmps); nxtDisplayTextLine(5, "curr: %4d", HTMCreadRelativeHeading(HTMC)); if(nRcvBuffer[2]==1){ int mspeed = 0; int mangle = 0; //stopMotors(); blob_array _blobs; // combine all colliding blobs into one bool _condensed = true; //blob_array _blobs; int _l, _t, _r, _b; int _nblobs; eraseDisplay(); // Initialise the camera NXTCAMinit(cam); servo[servo1] = 35; while(true) { eraseDisplay(); _nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed); // Fetch all the blobs, have the driver combine all // the colliding blobs. if(_nblobs>0){ int cx = 176-(SIDE_CENTER(_blobs[0].x1, _blobs[0].x2)); int cy = SIDE_CENTER(_blobs[0].y1, _blobs[0].y2); float dx = cx-88; /* if(cy < C_STOP){ mspeed = (cy/C_STOP)*25; } else{ mspeed = 0; }*/ mspeed=50; // if((cx-44)>0){ float rathura = (dx*90)/88; mangle = rathura; // } // else{ //mangle = (dx/44)*-90; //} // Draw the scaled blobs _l = xscale(_blobs[0].x1); _t = yscale(_blobs[0].y1); _r = xscale(_blobs[0].x2); _b = yscale(_blobs[0].y2); nxtFillRect(_l, _t, _r, _b); nxtDisplayTextLine(1, "%d", _nblobs); nxtDisplayTextLine(2, "%d", mangle); nxtDisplayTextLine(3, "%d", (int)dx); runMotorSpeeds(mSD, mSE, mSF, mSG, mangle, mspeed); _nblobs = 0; wait1Msec(100); } } flagged = true; } // else{ // stopMotors(); else{ if (abs(oxmps-HTMCreadRelativeHeading(HTMC)) > 5) { flagged = false; } } if(flagged == true){ stopMotors(); } else{ turntocmps(cmps, oxmps); } // } //} } return; }
task main(){ calibrate(); while(true){ length = USreadDist(USBack); rightWidth = USreadDist(USRight); leftWidth = USreadDist(USLeft); haveBall = TSreadState(HaveBaller); frontIRValue = HTIRS2readACDir(IRFront); currentRelCompass = HTMCreadRelativeHeading(Compass); HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue); isWhite = (currentRed > whiteThreshold && currentGreen > whiteThreshold && currentBlue > whiteThreshold); nxtDisplayTextLine(1, "Com: %4d", currentRelCompass); nxtDisplayTextLine(2, "IR: %4d", frontIRValue); nxtDisplayTextLine(3, "Len: %4d", length); nxtDisplayTextLine(4, "RWid: %4d", rightWidth); nxtDisplayTextLine(5, "LWid: %4d", leftWidth); nxtDisplayTextLine(6, "Ball: %4d", haveBall); if(isWhite){ nxtDisplayTextLine(7, "White: Yes"); } else{ nxtDisplayTextLine(7, "White: No"); } if(currentRelCompass < 0 - nbound){ move(CW, turnPower); } else if(currentRelCompass > 0 + nbound){ move(CC, turnPower); } else{ switch(frontIRValue){ case 0: move(ST); break; case 1: move(L); break; case 2: move(L); break; case 3: move(L); break; case 4: move(L); break; case 5: if(leftWidth < 45 && leftWidth != 0){ if(length < 20){ move(F); } else{ move(R); } } else if(rightWidth < 45 && rightWidth != 0){ if(length < 20){ move(F); } else{ move(L); } } else{ if (length > fbound){ move(B); } else if(length < 8){ move(F); } else{ move(ST); } } break; case 6: move(R); break; case 7: move(R); break; case 8: move(R); break; case 9: move(R); break; } wait1Msec(50); } } }
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 getRelativeHeading() { return HTMCreadRelativeHeading(COMPASS); }
int calc(){ int length = SensorValue[USBack]; int width = SensorValue[USRight]; bool haveBall = TSreadState(HaveBaller); int frontIRValue = HTIRS2readACDir(IRFront); int backIRValue = HTIRS2readACDir(IRBack); int currentRelCompass = HTMCreadRelativeHeading(Compass); HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue); nxtDisplayTextLine(3, "Abs: %4d", currentRelCompass); nxtDisplayTextLine(4, "Dir: %4d", frontIRValue); if(currentRelCompass < -nbound){ return CC; } else if(currentRelCompass > nbound){ return CW; } else{ if(!haveBall){ if(IRFront != 0){ switch(frontIRValue){ case 1: return B; break; case 2: return BL; break; case 3: return L; break; case 4: return FL; break; case 5: return F; break; case 6: return FR; break; case 7: return R; break; case 8: return BR; break; case 9: return B; break; } } else{ switch(backIRValue){ case 0: return ST; break; case 1: return BR; break; case 2: return BR; break; case 3: return B; break; case 4: return BL; break; case 5: return BL; break; case 6: return BR; break; case 7: return B; break; case 8: return BL; break; case 9: return BL; break; } } } else{ hashtagyoloswagdiem(); return F; } } }