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