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