//Move Forward a Square void ForwardSQ(void)//<--------------------------------------------------change distance forward { //Move Forward a Square nMotorEncoder[motorB]=0; nMotorEncoder[motorC]=0; setMotorTarget(motorB,220,20); setMotorTarget(motorC,220,20); waitUntilMotorStop(motorB); waitUntilMotorStop(motorC); }//end ForwardSQ()
// void leftparking()// did all this assuming motorA is left, motorB is right, colorSensorA is left, colorSensorB is right task sonarFunction() { while(true) { moveMotorTarget(motorC, 90, 50); waitUntilMotorStop(motorC); moveMotorTarget(motorC, -90, -50); waitUntilMotorStop(motorC); motor[motorC] = 0; } }
task Rijden() { k=0; int var=0; while(true){ if(SensorValue[WIT] < 55) var=1; if((SensorValue[ZWART]==BLACKCOLOR)||(SensorValue[ZWART]==GREENCOLOR)) var=2; if(((SensorValue[ZWART]==BLACKCOLOR)||(SensorValue[ZWART]==GREENCOLOR))&&((SensorValue[WIT] <55))) var=3; if(((SensorValue[ZWART]!=BLACKCOLOR)&&(SensorValue[ZWART]!=GREENCOLOR))&&((SensorValue[WIT] >55))) var=4; switch(var) { case 1: setMotor(motorB,50); setMotor(motorA,0); var=0; k=0; break; case 2: setMotor(motorB,0); setMotor(motorA,50); var=0; k=0; break; case 3: setMotor(motorB,0); setMotor(motorA,0); wait1Msec(100); setMotorTarget(motorA,85,-50); setMotorTarget(motorB,85,-50); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); waitUntil(k==1); var=0; k=0; break; case 4: setMotor(motorB,50); setMotor(motorA,50); var=0; k=0; break; } } }
task main() { int var=0; while(true){ nxtDisplayCenteredBigTextLine(1,"%d",k);//laat getallen zien op NXT scherm if(SensorValue[WIT] < 55) var=1; if((SensorValue[ZWART]==BLACKCOLOR)||(SensorValue[ZWART]==GREENCOLOR)) var=2; if(((SensorValue[ZWART]==BLACKCOLOR)||(SensorValue[ZWART]==GREENCOLOR))&&((SensorValue[WIT] <55))) var=3; switch(var) { case 1: setMotor(motorB,30); setMotor(motorA,0); var=0; break; case 2: setMotor(motorB,0); setMotor(motorA,30); var=0; break; case 3: setMotor(motorB,0); setMotor(motorA,0); wait1Msec(100); setMotorTarget(motorA,85,-30); setMotorTarget(motorB,85,-30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); bluetooth(); var=0; break; default: setMotor(motorB,20); setMotor(motorA,20); var=0; break; } } }
/// drive a certain distance in inches void driveInches(float inches, int driveSpeed) { resetMotorEncoder(leftMotor); resetMotorEncoder(rightMotor); setMotorTarget(leftMotor, inches/7.8, driveSpeed); setMotorTarget(rightMotor, inches/7.8, driveSpeed); waitUntilMotorStop(leftMotor); setMotorSpeed(leftMotor, 0); setMotorSpeed(rightMotor, 0); sleep(200); }
/// drive the robot forward using motor encoders void driveWithEncoders(int degrees){ resetMotorEncoder(leftMotor); resetMotorEncoder(rightMotor); setMotorTarget(leftMotor, degrees, DRIVE_SPEED); setMotorTarget(rightMotor, degrees, DRIVE_SPEED); waitUntilMotorStop(leftMotor); setMotorSpeed(leftMotor, 0); setMotorSpeed(rightMotor, 0); sleep(200); }
//autonomous main task main() { //drives forward until the distance sensor detects that it is 200 mm or closer to the wall driveWithUltraSonic(460, MOTOR_SPEED); lift(250); driveForwardWithEncoder(80, 50, 1000); setMotorTarget(CLAW_MOTOR, 360, 50); waitUntilMotorStop(CLAW_MOTOR); //uses the smart motor as a servo to move the lift motor up lift(200); rotate(-175); driveForwardWithEncoder(955, 50, 2350); lift(-300); }
task rightparking() { if(SensorValue[sonarSensor] <= 75) { motor[motorL] = 75; // lego group x*y x=diamter, y=width motor[motorR] = 75; } else { motor[motorL] = 90; motor[motorR] = 0; wait1Msec(100); moveMotorTarget(motorL, 2122, 75); moveMotorTarget(motorR, 2122, 75); waitUntilMotorStop(motorR); motor[motorL] = 0; motor[motorR] = 0; } }
/// move arm to certain degrees void setArmDegrees(float degrees) { resetMotorEncoder(armMotor); setMotorTarget(armMotor, degrees/360, ARM_SPEED); waitUntilMotorStop(armMotor); }
task Rijden() { // Geluid dat de robot maakt tijdens het rijden, in loop. k = 0; // Reset kruispuntboolean naar 0 //l = 0; int var = 0; // variabele die naar een waarde wordt geset afhankelijk van welke sensor een signaal geeft. while(true){ playSoundFile("hehe.rso"); // Linker sensor: Trigger wanneer de zwart-wit sensor (links) te weinig wit registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar links. if(SensorValue[WIT] < 55) var=1; // Rechter sensor: Trigger wanneer de kleurensensor (rechts) zwart registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar rechts. if((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor)) var=2; // kruispuntherkenning: Trigger wanneer zowel de kleurensensor als de zwartwit sensor getriggerd worden. Voer case 3 uit: stop, rij achteruit, wacht op commando. if(((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))&&((SensorValue[WIT] <55))) var=3; // Tegenovergestelde van kruispuntherkenning: Trigger als de sensoren alleen wit registreren. Voer case 4 uit: rij naar voren. if(((SensorValue[ZWART]!=blackcolor)&&(SensorValue[ZWART]!=greencolor))&&((SensorValue[WIT] >55))) var=4; // Obstakelsensor: Trigger wanneer de sonar een obstakel registreert. Voer case 5 uit: geef geluid en stop. if (SensorValue[sonar] <30) var=5; switch(var) { case 1: //stuur naar links setMotor(motorB,30); setMotor(motorA,-5); var=0; k=0; break; case 2: //stuur naar rechts setMotor(motorB,-5); setMotor(motorA,30); var=0; k=0; break; case 3: //stoppen, ga achteruit, wacht op BlueTooth signaal setMotor(motorB,0); setMotor(motorA,0); wait1Msec(100); clearSounds(); setMotorTarget(motorA,85,-30); // ga achteruit setMotorTarget(motorB,85,-30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); waitUntil(k==1); // wacht op een BlueTooth signaal dat links, rechts of vooruit zegt en k=1 triggert. var=0; k=0; break; case 4: // rij naar voren setMotor(motorB,30); setMotor(motorA,30); var=0; k=0; break; case 5: // als sonar wat ziet: schreeuw, rem rustig af en roteer 180*. clearSounds(); playSoundFile("scream4.rso"); for (int i=3000;i>0 ; i--) { setMotor(motorA,i/100); setMotor(motorB,i/100); } //waitUntil(l==1); //nxtDisplayTextLine(1,"%d",i); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); // oorspronkelijk hadden we code om een object te ontwijken, zowel linksom als rechtsom, // met als alternatief om 180* te draaien. Dit kregen we echter niet op tijd volledig // aan de praat, waardoor we hebben gekozen om hardcoded 180* te draaien. // zie hiervoor de AvoidObject.c en AvoidObject.h files. // //AvoidMain(); //waitUntil(avoidDone==1); //waitUntil(l==1); //avoidDone = 0; // // einde ontwijkcode // hardcoded rotate robot 180* //setMotorTarget(motorA, 360, 30); // rechtsom draaien //setMotorTarget(motorB, 360, -30); //waitUntilMotorStop(motorA); //waitUntilMotorStop(motorB); // end hardcoded rotate robot 180* setMotorTarget(motorA, 360, 30); waitUntilMotorStop(motorA); setMotorTarget(motorA, 135, 30); setMotorTarget(motorB, 135, 30); waitUntilMotorStop(motorA); setMotorTarget(motorB, 360,30); waitUntilMotorStop(motorB); setMotorTarget(motorA, 540, 30); setMotorTarget(motorB, 540, 30); waitUntilMotorStop(motorA); setMotorTarget(motorB, 360,30); waitUntilMotorStop(motorB); setMotorTarget(motorA, 135, 30); setMotorTarget(motorB, 135, 30); waitUntilMotorStop(motorA); var=0; l=0; break; } } }
task main() { // Gekregen code voor BlueTooth string sig = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; // Einde gekregen code voor BlueTooth while(true) // Main loop { /* * Gekregen code voor BlueTooth * leest berichten die BlueTooth stuurt uit en stopt die in een string. */ nSizeOfMessage = cCmdMessageGetSize(INBOX); if (nSizeOfMessage > kMaxSizeOfMessage) nSizeOfMessage = kMaxSizeOfMessage; if (nSizeOfMessage > 0){ nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; string s = ""; stringFromChars(s, (char *) nRcvBuffer); displayCenteredBigTextLine(4, s); /* * Einde gekregen code voor BlueTooth */ sig = nRcvBuffer;// zet bluetoothsignaal in variabele //writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven // naar voren rijden zonder lijndetectie if(sig == "U"){ setMotor(motorA, 30); setMotor(motorB, 30); wait1Msec(10); } // naar achter rijden zonder lijndetectie if(sig=="D"){ setMotor(motorA, -30); setMotor(motorB, -30); wait1Msec(10); } // maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten) if(sig=="L"){ setMotorTarget(motorA, 0, 0); setMotorTarget(motorB, 360, 30); waitUntilMotorStop(motorB); startTask(Rijden); k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 } // maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten) if(sig=="R"){ setMotorTarget(motorA, 360, 30); setMotor(motorB, 0); waitUntilMotorStop(motorA); startTask(Rijden); k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 } // rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten) if(sig=="F"){ setMotorTarget(motorA, 180, 30); setMotorTarget(motorB, 180, 30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); k=1; // zet anti-lijnhump op 1 startTask(Rijden); } // Stoppen if(sig=="A"){ setMultipleMotors(motorA, motorB, 0); stopTask(Rijden); clearSounds(); } // Zet lijndetectie aan if(sig=="C"){ k=1; // zet anti-lijnhump op 1 l=1; // zet anti-lijnhump op 1 startTask(Rijden); } /* * nog 1 knop ongebruikt (B). */ // Na elk commando, wacht 0,1s. wait1Msec(100); } } }
task main() { string sig = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while (true) { // Check to see if a message is available nSizeOfMessage = cCmdMessageGetSize(INBOX); writeDebugStream("%c\n", nRcvBuffer); if (nSizeOfMessage > kMaxSizeOfMessage) nSizeOfMessage = kMaxSizeOfMessage; if (nSizeOfMessage > 0){ nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; string s = ""; stringFromChars(s, (char *) nRcvBuffer); displayCenteredBigTextLine(4, s); sig = nRcvBuffer; writeDebugStream(sig); if(sig == "U"){ setMotor(motorA, 50); setMotor(motorB, 50); wait1Msec(10); } if(sig=="D"){ setMotor(motorA, -50); setMotor(motorB, -50); wait1Msec(1); } if(sig=="L"){ setMotorTarget(motorA,0, 0); setMotorTarget(motorB,360, 50); waitUntilMotorStop(motorB); startTask(Rijden); k=1; } if(sig=="R"){ setMotorTarget(motorA,360, 50); setMotor(motorB, 0); waitUntilMotorStop(motorA); startTask(Rijden); } if(sig=="F"){ setMotorTarget(motorA, 180,30); setMotorTarget(motorB,180, 30); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); k=1; startTask(Rijden); } if(sig=="A"){ setMultipleMotors(motorA, motorB, 0); stopTask(Rijden); } if(sig=="C"){ startTask(Rijden); } wait1Msec(100); } } }
void bluetooth() { string kutzooi = ""; TFileIOResult nBTCmdRdErrorStatus; int nSizeOfMessage; ubyte nRcvBuffer[kMaxSizeOfMessage]; while (true) { // Check to see if a message is available nSizeOfMessage = cCmdMessageGetSize(INBOX); writeDebugStream("%c\n", nRcvBuffer); if (nSizeOfMessage > kMaxSizeOfMessage) nSizeOfMessage = kMaxSizeOfMessage; if (nSizeOfMessage > 0){ nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX); nRcvBuffer[nSizeOfMessage] = '\0'; string s = ""; stringFromChars(s, (char *) nRcvBuffer); displayCenteredBigTextLine(4, s); kutzooi = nRcvBuffer; writeDebugStream(kutzooi); if(kutzooi == "U"){ setMotor(motorA, 30); setMotor(motorB, 30); wait1Msec(10); break; } if(kutzooi=="D"){ setMotorTarget(motorA,1080, 100); setMotorTarget(motorB,1080, 100); waitUntilMotorStop(motorA); waitUntilMotorStop(motorB); wait1Msec(1); break; } if(kutzooi=="L"){ setMotorTarget(motorA,0, 0); setMotorTarget(motorB,360, 25); waitUntilMotorStop(motorB); break; } if(kutzooi=="R"){ setMotorTarget(motorA,360, 25); setMotor(motorB, 0); waitUntilMotorStop(motorA); break; } if(kutzooi=="F"){ setMotorTarget(motorA, 1080,100); setMotor(motorB, 0); waitUntilMotorStop(motorA); setMotor(motorA, 0); setMotorTarget(motorB, 1080,100); break; } if(kutzooi=="A"){ setMultipleMotors(motorA, motorB, 0); } wait1Msec(100); } } }