//drive forward using the motor encoders use sleepTime to wait until the motors
//stop
void driveForwardWithEncoder(int count, int speed, int sleepTime) {
resetMotorEncoder(LEFT_DRIVETRAIN_MOTOR);
resetMotorEncoder(RIGHT_DRIVETRAIN_MOTOR);
setMotorTarget(LEFT_DRIVETRAIN_MOTOR, count, speed);
setMotorTarget(RIGHT_DRIVETRAIN_MOTOR, -count, -speed);
sleep(sleepTime);
}
void moveForward(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorTarget(motorB, distance, speed);
setMotorTarget(motorC, distance, speed);
sleep(100 * distance / speed);
}
void pivotRight(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorTarget(motorB, distance, speed);
setMotorTarget(motorC, distance, -speed);
sleep(100 * distance / speed);
}
/// 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);
}
/// 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);
}
//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()
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;
}
}
}
void turnClockWise(){
resetMotorEncoder( motorA );
resetMotorEncoder( motorB );
setMotorTarget( motorA, ANGLE, VELOCITY );
setMotorTarget( motorB, ANGLE, -VELOCITY );
delay(390);
while( SensorValue[COLOR] != BLACK){};
setMotorA(0);
setMotorB(0);
current_direction = clockWiseDirection( current_direction );
}
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;
}
}
}
void turnLeft(int distance, int speed){
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
setMotorSpeed(motorB, 0);
setMotorTarget(motorC, distance, speed);
sleep(100 * distance / speed);
}
void parse_message(String message) {
if(message[0] == '(') {
int digitVal = 10*(message[1]-'0') + (message[2]-'0');
Serial.print("Setting display to: ");
Serial.print(message[1]);
Serial.println(message[2]);
setDisplay(digitVal / 10, digitVal % 10);
}
Serial.print("message is: ");
Serial.println(message);
if(message[message.length() - 1] == ')') {
double motorVal = 10*(message[message.length()-3]-'0') + (message[message.length()-2]-'0');
unsigned long motorPer = motorVal * 1000/SPEED_SLOPE;
unsigned long target = STEPS * motorPer / 100;
Serial.print("Setting motor target to: ");
Serial.println(target);
setMotorTarget(target);
}
if (message[0] == '<') {
int rgbValue = 100*(message[1] -'0') + 10*(message[2]-'0') + (message[3]-'0');
setLED(rgbValue, 255, 255);
}
}
void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
{
// convert target from body to constraint space
btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
qConstraint.normalize();
// extract "pure" hinge component
btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
vNoHinge.normalize();
btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
qHinge.normalize();
// compute angular target, clamped to limits
btScalar targetAngle = qHinge.getAngle();
if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qHinge = -(qHinge);
targetAngle = qHinge.getAngle();
}
if (qHinge.getZ() < 0)
targetAngle = -targetAngle;
setMotorTarget(targetAngle, dt);
}
//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 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);
}
}
}
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);
}
}
}
/// move arm to certain degrees
void setArmDegrees(float degrees) {
resetMotorEncoder(armMotor);
setMotorTarget(armMotor, degrees/360, ARM_SPEED);
waitUntilMotorStop(armMotor);
}
task main() {
int leftMotorSpeed, rightMotorSpeed, wristMotorSpeed, armMotorSpeed, stickAValue, stickBValue, stickCValue, stickDValue;
resetMotorEncoder(armMotor);
resetMotorEncoder(clawMotor);
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetMotorEncoder(wristMotor);
resetGyro(gyro);
startGyroCalibration(gyro, gyroCalibrateSamples2048);
eraseDisplay();
getGyroCalibrationFlag(gyro);
displayString(3, "calibrating gyro");
wait1Msec(500);
eraseDisplay();
startTask(displayMyMotorPositions);
while (true ){
/***************************************************************************************************************/
//Joystick Control:
/***************************************************************************************************************/
//Driving:
if(getJoystickValue(BtnEUp) > 0) { //Drive Straight Forward
driveStraightForward();
}
else if(getJoystickValue(BtnEDown) > 0) { //Drive Straight Backward
driveStraightBackward();
}
else {
driveForwardPressed = false;
driveBackwardPressed = false;
stickAValue = getJoystickValue(ChA);
if ((stickAValue <= 15) && (stickAValue >= -15) ) stickAValue = 0;
stickBValue = getJoystickValue(ChB);
if ((stickBValue <= 15) && (stickBValue >= -15) ) stickBValue = 0;
if (stickBValue >=0 ) stickBValue = (stickBValue / 10) * (stickBValue / 10) * 2;
else stickBValue = - (stickBValue / 10) * (stickBValue / 10) * 2;
leftMotorSpeed = stickAValue - stickBValue;
rightMotorSpeed = stickAValue + stickBValue;
if ( leftMotorSpeed > 100) leftMotorSpeed = 100;
if ( leftMotorSpeed < -100) leftMotorSpeed = -100;
if ( rightMotorSpeed > 100) rightMotorSpeed = 100;
if ( rightMotorSpeed < -100) rightMotorSpeed = -100;
if (leftMotorSpeed >=0 ) leftMotorSpeed = (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
else leftMotorSpeed = - (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
if (rightMotorSpeed >=0 ) rightMotorSpeed = (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
else rightMotorSpeed = - (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
setMotorSpeed(leftMotor, leftMotorSpeed);
setMotorSpeed(rightMotor, rightMotorSpeed);
}
/***************************************************************************************************************/
//WRIST MOTOR
stickDValue = getJoystickValue(ChD);
if ((stickDValue <= 15) && (stickDValue >= -15) ) stickDValue = 0;
wristMotorSpeed = stickDValue;
if (wristMotorSpeed >=0 ) wristMotorSpeed = (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
else wristMotorSpeed = - (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
globalWristPosition = getMotorEncoder(wristMotor);
if ((wristMotorSpeed > 0) && (globalWristPosition >= WRIST_UPPER_LIMIT)){
setMotorTarget(wristMotor, WRIST_UPPER_LIMIT, 70);
}
else if ((wristMotorSpeed < 0) && (globalWristPosition <= WRIST_LOWER_LIMIT)) {
setMotorTarget(wristMotor, WRIST_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of wrist movement
if ( abs(globalWristPosition - WRIST_LOWER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
if ( abs(globalWristPosition - WRIST_UPPER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
setMotorSpeed(wristMotor, wristMotorSpeed );
}
/***************************************************************************************************************/
//ARM MOTOR
stickCValue = getJoystickValue(ChC);
if ((stickCValue <= 15) && (stickCValue >= -15) ) stickCValue = 0;
armMotorSpeed = stickCValue;
if (armMotorSpeed >=0 ) armMotorSpeed = (armMotorSpeed / 10) * (armMotorSpeed / 10);
else armMotorSpeed = - (armMotorSpeed / 10) * (armMotorSpeed / 10);
globalArmPosition = getMotorEncoder(armMotor);
if ((armMotorSpeed > 0) && (globalArmPosition >= ARM_UPPER_LIMIT)){
setMotorTarget(armMotor, ARM_UPPER_LIMIT, 70);
}
else if ((armMotorSpeed < 0) && (globalArmPosition <= ARM_LOWER_LIMIT)) {
setMotorTarget(armMotor, ARM_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of arm movement
if ( abs(globalArmPosition - ARM_LOWER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
if ( abs(globalArmPosition - ARM_UPPER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
setMotorSpeed(armMotor, armMotorSpeed);
}
/***************************************************************************************************************/
//CLAW MOTOR
if(getJoystickValue(BtnLUp) > 0) { //CLOSE
setMotorSpeed(clawMotor, 70);
}
else if(getJoystickValue(BtnLDown) > 0) { //OPEN
globalClawPosition = getMotorEncoder(clawMotor);
if (globalClawPosition <= -87) {
setMotorTarget(clawMotor, -87, 70);
}
else {
setMotorSpeed(clawMotor, -70);
}
}
else {
globalClawPosition = getMotorEncoder(clawMotor);
setMotorTarget(clawMotor, globalClawPosition, 90);
}
/***************************************************************************************************************/
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnFUp) > 0) { //UP
moveToAimingPosition();
}
if(getJoystickValue(BtnFDown) > 0) { //GRAB a Block
moveToNeutralPosition();
}
/***************************************************************************************************************/
//Block Stacking Positions
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnRUp) > 0) { //Upper Stacking Position
moveToUpperStackingPosition();
}
if(getJoystickValue(BtnRDown) > 0) { //Lower Stacking Position
moveToLowerStackingPosition();
}
wait1Msec(50); // Give actions time to make progress and prevent over-control from taking inputs too fast
}
}
void moveToUpperStackingPosition(){
int armPowerLevel = 60;
setMotorTarget(armMotor, STACK_BLOCKS_HIGH_ARM, armPowerLevel);
setMotorTarget(wristMotor, STACK_BLOCKS_HIGH_WRIST, armPowerLevel);
}
void moveToNeutralPosition(){
int armPowerLevel = 60;
setMotorTarget(armMotor, 0, armPowerLevel);
setMotorTarget(wristMotor, 0, armPowerLevel);
}
void moveToAimingPosition(){
int armPowerLevel = 60;
setMotorTarget(armMotor, REVERSE_GRIP_ARM, armPowerLevel);
setMotorTarget(wristMotor, REVERSE_GRIP_WRIST , armPowerLevel);
}
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;
}
}
}
