void StelMovementMgr::handleKeys(QKeyEvent* event)
{
if (event->type() == QEvent::KeyPress)
{
// Direction and zoom deplacements
switch (event->key())
{
case Qt::Key_Left:
turnLeft(true); break;
case Qt::Key_Right:
turnRight(true); break;
case Qt::Key_Up:
if (event->modifiers().testFlag(Qt::ControlModifier)){
zoomIn(true);
} else {
turnUp(true);
}
break;
case Qt::Key_Down:
if (event->modifiers().testFlag(Qt::ControlModifier)) {
zoomOut(true);
} else {
turnDown(true);
}
break;
case Qt::Key_PageUp:
zoomIn(true); break;
case Qt::Key_PageDown:
zoomOut(true); break;
case Qt::Key_Shift:
moveSlow(true); break;
case Qt::Key_Space:
if (event->modifiers().testFlag(Qt::ControlModifier))
setDragTimeMode(true);
break;
default:
return;
}
}
else
{
// When a deplacement key is released stop moving
switch (event->key())
{
case Qt::Key_Left:
turnLeft(false); break;
case Qt::Key_Right:
turnRight(false); break;
case Qt::Key_Up:
zoomIn(false);
turnUp(false);
break;
case Qt::Key_Down:
zoomOut(false);
turnDown(false);
break;
case Qt::Key_PageUp:
zoomIn(false); break;
case Qt::Key_PageDown:
zoomOut(false); break;
case Qt::Key_Shift:
moveSlow(false); break;
case Qt::Key_Control:
// This can be all that is seen for anything with control, so stop them all.
// This is true for 4.8.1
turnRight(false);
turnLeft(false);
zoomIn(false);
zoomOut(false);
turnDown(false);
turnUp(false);
setDragTimeMode(false);
break;
default:
return;
}
}
event->accept();
}
task main()
{
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
PlaySound(soundBlip);
wait1Msec((2 * PI) * 1000); //TAUUUU
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
float irval = acS3L;
StringFormat(irvalres, "%3.0f", irval);
WriteText(irFileHandle, IOResult, "Test");
WriteString(irFileHandle, IOResult, irvalres);
WriteByte(irFileHandle, IOResult, 13);
WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
motor[motorLeft] = 0;
motor[motorRight] = 0;
irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
driveToEnd(timeToEnd - roundTime);//drive to end of ramp
}
wait1Msec(300);
//turn left, forward, turn left, forward onto ramp
turnLeft(1.1, 100);
wait1Msec(300);
moveForward(1, 100);
wait1Msec(300);
turnLeft(1.1, 100);
wait1Msec(300);
moveForward(2.7, 100);
wait1Msec(300);
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
while (koth) {
wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
PlaySound(soundDownwardTones);
}//task main
void Autonomous()
{
DriverStationLCD *screen = DriverStationLCD::GetInstance();
while ((IsAutonomous()))
{
HSLImage* imgpointer; // declares an image container as an HSL (hue-saturation-luminence) image
imgpointer = camera.GetImage(); //tells camera to capture image
backpack.Set(Relay::kForward); //turns ringlight on
BinaryImage* binIMG = NULL; // declares a container to hold a binary image
binIMG = imgpointer -> ThresholdHSL(0, 255, 0, 255, 235, 255); // thresholds HSL image and places in the binary image container
delete imgpointer; // deletes the HSL image to free up memory on the cRIO
Image* Kirby = imaqCreateImage(IMAQ_IMAGE_U8, 0); //create 8 bit image
Image* KirbyTwo = imaqCreateImage(IMAQ_IMAGE_U8, 0); // creates the second 8-bit image that we can use separately for counting particles.
// (The first image gets eaten by the measureparticle function)
float pLower = 175; // min height of rectangle for comparison
float pUpper = 500; // max height of rectangle for comparison
int criteriaCount = 1; // number of elements to include/exclude at a time
int rejectMatches = 1; // when set to true, particles that do not meet the criteria are discarded
int connectivity = 1; // declares connectivity value as 1; so corners are not ignored
int Polturgust3000; // removes small blobs
int borderSetting; // variable to store border settings, limit for rectangle
int cloningDevice; // we create another image because the ParticleMeasuring steals the image from particlecounter
int borderSize = 1; // border for the camera frame (if you don't put this, DriverStation gets mad at you)
ParticleFilterCriteria2 particleCriteria;
ParticleFilterOptions2 particleFilterOptions;
int numParticles;
particleCriteria.parameter = IMAQ_MT_AREA; //The Morphological measurement we use
particleCriteria.lower = pLower; // The lower bound of the criteria range
particleCriteria.upper = pUpper; // The upper bound of the criteria range
particleCriteria.calibrated = FALSE; // We aren't calibrating to real world measurements. We don't need this.
particleCriteria.exclude = TRUE; // Remove all particles that aren't in specific pLower and pUpper range
particleFilterOptions.rejectMatches = rejectMatches; // Set to 1 above, so images that do not meet the criteria are discarded
particleFilterOptions.rejectBorder = 0; // Set to 0 over here so border images are not discarded
particleFilterOptions.connectivity8 = connectivity; // Sets the image image to 8 bit
Polturgust3000 = imaqParticleFilter4(Kirby, binIMG -> GetImaqImage(), &particleCriteria, criteriaCount, &particleFilterOptions, NULL, &numParticles); //The Particle Filter Function we use. (The ones before it are outdated)
borderSetting = imaqSetBorderSize(Kirby, borderSize); // Sets a border size
cloningDevice = imaqDuplicate(KirbyTwo, Kirby); //Officially creating a duplicate of the first image to count the number of particles.
delete binIMG; //Deletes the Binary image
int ParticleCounter; // stores number of particles
int* countparticles; // stores the number of particles for the measure particle function
ParticleCounter = imaqCountParticles(Kirby, TRUE, countparticles); // Counts the number of particles to be sent off later to the MeasureParticle function. Then it gets eaten by the measureparticle function
int TinyRuler; // TRULY ARBITRARY name of the first measuring particle function (specifically for particle #1)
int BabyYardstick; // TRULY ARBITRARY Name of the second measuring particle function (specifically for particle #2)
double* unowidth; // TRULY ARBITRARY name of the first particle it find
double* doswidth; // TRULY ARBITRARY name of the second particle it finds
TinyRuler = imaqMeasureParticle(Kirby, 0, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, unowidth); // Function of measuring rectangle width is applied to particle 1 (unowidth)
BabyYardstick = imaqMeasureParticle(Kirby, 1, FALSE, IMAQ_MT_BOUNDING_RECT_WIDTH, doswidth); // Function of measuring width is applied to particle 2 (doswidth)
screen->PrintfLine(DriverStationLCD::kUser_Line3,"W1: %f",*unowidth); // Prints the applied information to particle 1. (Rectangle width)
screen->PrintfLine(DriverStationLCD::kUser_Line4,"W2: %f",*doswidth);
imaqDispose(Kirby);
imaqDispose(KirbyTwo);
if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Left Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
// Even this needs to be tested
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRight();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Left Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
//both on
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 1))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeftMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else if (((togglebuttonOne.Get()) == 0) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Middle Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnRightMore();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Middle Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
driveForward();
Wait(3);
stopDriving();
}
}
//Left button on && right off
else if (((togglebuttonOne.Get()) == 1) && ((togglebuttonTwo.Get()) == 0))
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position");
if (*unowidth > 20) // The target should be hot. Now it goes to the other goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
turnLeft();
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
else // The target isn't hot. So it starts going toward this not hot goal.
{
screen -> PrintfLine(DriverStationLCD::kUser_Line6,"Right Position Not Hot");
// These DEFINITELY need to be tested. All of them. Forreal.
driveForward();
Wait(3);
stopDriving();
shootCatapult();
}
}
Wait(0.005);
screen -> UpdateLCD();
}
}
}
void proceduralDFS() {
int found = false;
int depth = 0;
int branch = 0;
int pathChosen[50][2];
int nodeFound;
int i;
while (!found) {
//find a node
depth++;
displayTextLine(depth,"Finding for node..");
nodeFound = moveToColor();
//if got blue.. then success
if (nodeFound == blue) {
displayTextLine(depth,"Success",depth);
turnBack();
found = true;
}
//if got red, do backtrack
else if (nodeFound == red) {
displayTextLine(depth,"Failure..");
depth--;
//Cari Branch Selanjutnya
backToPreviousNode();
displayTextLine(depth+1,"");
branch = alignToBranch(pathChosen[depth-1][0]);
//Jika tak ada, backtrack ke node sebelumnya
while (branch == 4) {
depth--;
displayTextLine(depth+1,"");
moveToColor();
moveForward(300);
branch = alignToBranch(pathChosen[depth-1][0]);
}
//Pilih jalan
displayTextLine(depth,"lv%d: Branching to %d",depth,branch);
pathChosen[depth-1][0] = branch;
if (branch <0) {
pathChosen[depth-1][0] *= -1;
pathChosen[depth-1][1] = 1;
}
else {
pathChosen[depth-1][1] = 0;
}
}
//if got green, prepare for deeper depth..
else if (nodeFound == green){
moveForward(300);
branch = alignToBranch(0);
displayTextLine(depth,"lv%d: Branching to %d",depth,branch);
pathChosen[depth-1][0] = branch;
if (branch <0) {
pathChosen[depth-1][0] *= -1;
pathChosen[depth-1][1] = 1;
}
else {
pathChosen[depth-1][1] = 0;
}
}
}
//Tampilkan rute ditemukan
depth--;
for (i=0; i<depth ; i++) {
displayTextLine(i+1,"path chosen -> %d",pathChosen[i][0]);
}
displayTextLine(i+9,"Mission Accomplished..");
//Kembali pulang berdasarkan rute tercatat
for (int i=depth-1 ; i>=0 ; i--) {
moveToColor();
moveForward(300);
switch(pathChosen[i][0]) {
case 3 : turnRight();
moveForward(200);
break;
case 2 : if (pathChosen[i][1] == 1) turnRight();
moveForward(400);
break;
case 1 : turnLeft();
moveForward(200);
break;
}
moveToColor();
}
}
void turnBack()
{
turnLeft();
turnRight();
}
void classic15Red()
{
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1550;
int dead1 = 1000;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
//encoder values
int encoder1 = 1125; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 275; //back up
int encoder5 = 800; //back up across the barrier again
int encoder6 = 90; // turn towards center large ball
int encoder7 = 220; // hit it off the barrier
int encoder8 = 350; // drive back
int encoder9 = 110; // turn towards other large ball
int encoder10 = 681; // hit it off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 150; // ass to wall
int encoder13 = 800;
int encoder14 = 400;
int encoder15 = 700;
int encoder16 = 400;
int encoder17 = 100;
int encoder18 = 400;
int encoder19 = 700;
int encoder20 = 400;
int encoder21 = 100;
int encoder22 = 400;
int encoder23 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
driveForward(encoder1); // drive to goal
armUp (goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead (dead1); //drive slow, adjust to 90 degrees
outtake (1500);
driveBack(encoder4); // back up
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
turnRight(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
driveBack(encoder8); // drive back
turnLeft(encoder9); // turn towards other large ball
driveForward(encoder10); // hit it off the barrier
driveBackSlowDead (dead2); // drive back to square
turnLeftArc (encoder12); // ass to wall
driveBackSlowDead (dead2) ; // wall align 90degrees
armDown (armMin); // arm down to floor
intakeDead (); // start intaking
driveForwardSlow (encoder13) ;// drive towards barf and intake whatever
turnRightArc (encoder14); // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder15); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //hump da bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder16); //face the barf
driveForwardSlow (encoder17) ;// drive towards barf and intake whatever
turnRightArc (encoder18) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder19); // drive to barrier
stopDrive(); // stop at barrier
outtake (1500); //drop the barf
driveBackSlowDead(dead2); //drive to and align on bump
armDown (armMin); // arm down to floor
intakeDead (); //start intaking
turnLeftArc (encoder20); //face the barf
driveForwardSlow (encoder21) ;// drive towards barf and intake whatever
turnRightArc (encoder22) ; // rotate towards the barrier
stopIntake();
armUp(wallHeight); // arm up to wall height
driveForwardSlow (encoder23); // drive to barrier
outtake (1500); //drop the barf
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
void pickNode(int armNo, int side)
{
lcd("pickNode");
arm[armNo] = term[ct][side];
if (ct == ot)
{
if (((ct == 0 || ct == 1) && dir == 0) || ((ct == 2 || ct == 3) && dir == 2))
{
if (armNo != side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
if (armNo == side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 1) || ((ct == 2 || ct == 3) && dir == 3))
{
if (armNo != side)
turnRight();
}
else if (((ct == 0 || ct == 1) && dir == 3) || ((ct == 2 || ct == 3) && dir == 1))
if (armNo == side)
turnRight();
pick(armNo);
term[ct][side] = -1;
}
else
{
ot = ct;
if (dir == 0 || dir == 2)
{
if ((dir == 0 && (ct == 0 || ct == 1)) || (dir == 2 && (ct == 2 || ct == 3)))
{
if (armNo == side)
front();
else
{
front();
turn();
}
}
else
{
if (armNo == side)
{
turn();
front();
}
else
{
backward();
}
}
}
else
{
if (armNo == side)
{
if (ct == 0 || ct == 3)
turnRight();
else turnLeft();
front();
}
else
{
if (ct == 0 || ct == 3)
turnLeft();
else turnRight();
backward();
}
}
pick(armNo);
term[ct][side] = -1;
}
//..printf("Arm %d picked %d from term[%d][%d]\n", armNo, arm[armNo], ct, side);
}
void rushBlue ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead (8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 630; //turn to goal + enclose it
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft (encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet (motor1, 10) ; // no inertia
motorSet (motor2, 10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (85);
//driveBackDead(); // push them
//delay (300);
stopDrive ();
delay (7000); // wait till auto end
driveBackDead ();// push them
delay (800);
stopDrive();
delay(100);
driveForwardDead(); // driveback towards goal
delay(300);
stopDrive ();
intakeDead();
armUp(goalHeight); //arm up
scrapeLeft (encoder4); //encase the goal
driveForwardSlowDead ();
delay(700);
stopDrive();
outtake (3000);
stopAll ();
}
void classic15Blue()
{
int armMin = 290;
int wallHeight = 1000;
int goalHeight = 1700;
int dead1 = 1200;
int dead2 = 2000;
int dead3 = 3000;
int pot = analogRead (8);
//encoder values
int encoder1 = 900; //drive to goal
int encoder2 = 325; //keep going towards goal
int encoder3 = 0; //drive slow, adjust to 90 degrees
int encoder4 = 325; //back up
int encoder5 = 1350; //back up across the barrier again
int encoder6 = 80; // turn towards center large ball
int encoder7 = 600; // hit 1st ball off the barrier
int encoder8 = 350; // drive back
int encoder9 = 200; // turn towards other large ball
int encoder10 = 400; // hit 2nd ball off the barrier
int encoder11 = 300; // drive back to square
int encoder12 = 290; // turn to barf
int encoder13 = 800; // drive to barf + pickup
int encoder14 = 400;
int encoder15 = 700;
// begin routine
intakeDead();
delay(1000);
stopIntake();
// driveforward (some curve)////////////////////////////////////////////////////
int counts = 0;
imeReset(0); // rest rightLine ime
imeGet(0, &counts);
while (abs(counts) < encoder1)
{
motorSet (motor1, 110) ; // slight curve cuz friction
motorSet (motor2, 110) ;
motorSet (motor10, 127) ;
motorSet (motor9, 127) ;
imeGet(0, &counts); // keep getting the value
}
motorSet (motor1, 0) ;
motorSet (motor2, 0) ;
motorSet (motor10, 0) ;
motorSet (motor9, 0) ;
delay(200);
///////////////////////////////////////////////////////////////////////////////
//driveForward(encoder1); // drive to goal
armUp (goalHeight); //raise arm after cross barrier
driveForwardSlow(encoder2); //keep going towards goal
driveForwardSlowDead (); //drive slow, adjust to 90 degrees
delay(1000);
outtake (1700);// outtake
driveBack(encoder4); // back up
delay(300);
armDown(armMin); // lower arm
driveBack(encoder5); //back up across the barrier again
delay(300);
turnLeft(encoder6); // turn towards center large ball
armUp(wallHeight); // arm up to wall height
driveForward(encoder7); // hit it off the barrier
delay (500);
driveBack(encoder8); // drive back
delay(300);
turnRight(encoder9); // turn towards other large ball
driveForward(encoder10); // hit 2nd ball off the barrier
delay(500);
driveBack(encoder11); // drive back to square
delay(600);
turnLeft(encoder12); // turn to barf
delay(300);
armDown(armMin);
intakeDead(); // pick up barf
driveForward(encoder13); // drive towards barf + intake it
delay(500);
//end of routine
stopAll () ;
delay(60000);///////////////////////////////////////////////////////////////////////////////////
}
void kickstand()
{
moveBackward(1.5);
turnLeft(90);
moveForward(3);
}
// drive the robot
// -------------------------------------------------------------------[ drive ]
void WRTbotDrive::drive( const uint16_t driveThisWay, wrt_motor_t &motor )
{
// --- motor ON by default
// -----------------------
motor.left.control |= wrt_motorControl_power_ON;
motor.right.control |= wrt_motorControl_power_ON;
// --- no drive by light
// --------------------------
this->driveByLightFactor = 0;
switch( driveThisWay )
{ default:
// ------------------------------------------------------------[ stop ]
case wrt_stop:
{
motor.left.control &= wrt_motorControl_power_OFF;
motor.right.control &= wrt_motorControl_power_OFF;
// ---
motor.left.speed = 0;
motor.right.speed = 0;
// ---
drive( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward:
{
this->useNominal = true;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward_slow:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_min;
motor.left.speed = wrt_code_motor_speed_min;
// ---
driveForward( motor );
} break;
// ----------------------------------------------------[ fast forward ]
case wrt_forward_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ reverse ]
case wrt_reverse:
{
this->useNominal = true;
// ---
driveReverse( motor );
} break;
// ----------------------------------------------------[ fast reverse ]
case wrt_reverse_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveReverse( motor );
} break;
// -------------------------------------------------------[ left turn ]
case wrt_drive_left:
{
driveLeft( motor );
} break;
// --------------------------------------------------[ fast left turn ]
case wrt_turn_left_fast:
{
turnLeftFast( motor );
} break;
// --------------------------------------------------[ slow left turn ]
case wrt_turn_left:
{
this->useNominal = true;
turnLeft( motor );
} break;
// ------------------------------------------------------[ right turn ]
case wrt_drive_right:
{
driveRight( motor );
} break;
// -------------------------------------------------[ fast right turn ]
case wrt_turn_right_fast:
{
turnRightFast( motor );
} break;
// -------------------------------------------------[ slow right turn ]
case wrt_turn_right:
{
this->useNominal = true;
turnRight( motor );
} break;
// -----------------------------------------[ differential right turn ]
case wrt_turnRightDiff:
case wrt_turnRightDiff_fast:
{
turnRightDiferential( driveThisWay, motor );
} break;
// -----------------------------------------[ differential left turn ]
case wrt_turnLeftDiff:
case wrt_turnLeftDiff_fast:
{
turnLeftDiferential( driveThisWay, motor );
} break;
}
} // done drive()
void stepLeft(){
turnLeft();
__delay_cycles(1600000);
stop();
}
void Executer::executeTurnLeft(TreeNode* node) {
// //qDebug() << "called";
if (!checkParameterQuantity(node, 1, 20000+Token::TurnLeft*100+90) ||
!checkParameterType(node, Value::Number, 20000+Token::TurnLeft*100+91)) return;
emit turnLeft(node->child(0)->value()->number());
}
void bot1(void)
{
/* Puts bot in motion (`forward()`) */
forward();
/* Loops until collision (boolean is set in ISR) */
while (!hitWall) {
for (int i(0); i < NUM_BUMPERS; ++i) {
if (digitalRead(bumpers[i])) {
hitWall = true;
}
}
delay(1);
}
Serial.println("Hit wall");
/* Moves backward until red and stops */
actionUntilColor(RED, backward);
turn(-80); /* negative to turn right */
actionUntilColor(RED, turnRight);
/* Flash a red LED */
flashLed(RED_LED); /* pauses for 1 second to flash */
/* Follows red until centre yellow */
while (!followColorUntilColor(RED, YELLOW)) {}
Serial.println("Found yellow");
forward();
delay(PUSH_CONST);
turn(-20);
stop();
/* Stops on yellow, flashing yellow LED twice */
flashLed(YELLOW_LED); /* pauses for 1 second to flash */
flashLed(YELLOW_LED); /* pauses for 1 second to flash */
/* experimentally necessary to avoid thick black line */
forward();
delay(75);
/* Follows yellow */
Serial.println("Following yellow until red");
while (!followColorUntilColor(YELLOW, RED)) {}
forward();
delay(PUSH_CONST);
stop();
/* Follows red */
Serial.println("Following red until yellow");
while (!followColorUntilColor(RED, YELLOW)) {}
/* Stops on yellow, turns on yellow LED, turns 180 degrees */
Serial.println("Turning until red");
stop();
digitalWrite(YELLOW_LED, HIGH);
turn(220); /* ensures we escape red and actually turn */
actionUntilColor(RED, turnLeft);
/* Follows red back to the middle */
while (!followColorUntilColor(RED, YELLOW)) {}
Serial.println("Found yellow and about to communicate");
forward();
delay(PUSH_CONST);
stop();
/* Communicates to Bot 2: `START` */
transmit(startMsg, MSG_LEN, TIMEOUT);
Serial.println("flashing green LED");
/* Flashes green LED twice */
flashLed(GREEN_LED); /* pauses for 1 second to flash */
flashLed(GREEN_LED); /* pauses for 1 second to flash */
/* Follows yellow */
while (!followColorUntilColor(YELLOW, RED)) {}
forward();
delay(PUSH_CONST);
stop();
/* Follows red, stops on yellow */
while (!followColorUntilColor(RED, YELLOW)) {}
/* Turns on green LED */
digitalWrite(GREEN_LED, HIGH);
/* Waits for `TOXIC` */
Serial.println("Receiving toxic message");
turnLeft(45);
while (!receive(recMsg, MSG_LEN)) {}
stop();
delay(2 * TIMEOUT); /* don't double receive a message */
/* Flash yellow LED continuously */
digitalWrite(YELLOW_LED, HIGH);
/* Waits for `STOP_YELLOW` */
turnLeft(45);
while (!receive(recMsg, MSG_LEN)) {}
/* Turns off yellow LED */
digitalWrite(YELLOW_LED, LOW);
delay(TIMEOUT); /* don't double receive a message */
/* Waits for `DONE` */
Serial.println("Receiving toxic message");
turnLeft(45);
while (!receive(recMsg, MSG_LEN)) {}
stop();
/* Flashes green LED */
while (1) { flashLed(GREEN_LED); }
}
void calibrateTurn(Servo *leftWheel, Servo *rightWheel){
forward(leftWheel, rightWheel, 500);
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);
forward(leftWheel, rightWheel, 500);
turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES);
}
void rejectionBlue ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead (8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 100; //turn to goal + enclose it
int encoder5 = 450; //hit 1st blue ball off
int encoder6 = 350; // drive back
int encoder7 = 175; // turn to 2nd blue ball
int encoder8 = 550; // hit 2nd ball off
int encoder9 = 250; // drive back to position
int encoder10 = 370; // rotate to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft (encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet (motor1, 10) ; // no inertia
motorSet (motor2, 10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (85);
armUp(wallHeight);// arm up
turnRight (encoder4); // turn towards center blue ball
driveForward(encoder5); // hit 1st ball off
delay(700);
driveBack(encoder6); // back
delay (700);
turnRight(encoder7); // turn towards 2nd blue ball
delay (700);
driveForward(encoder8); // hit off 2nd blue ball
delay (700);
intakeDead();
driveBack(encoder9); // position
delay(700);
turnLeft(encoder10); // encase opponet goal
delay(700);
armUp(goalHeight);
findLineRight();
//followLine(300);
driveForwardDead (); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll ();
delay(60000);
}
/**
Turn-left fascade
*/
void Movement::turnLeft() { turnLeft(STEP_SIZE); }
void kakitRed ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1650;
int pot = analogRead (8);
int encoder00 = 250; // back up abit to row
int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
int encoder1 = 162; // rotate towards 2 buckys
int encoder2 = 150; // drive towards buckys
int encoder3 = 400; // back up to bump
int encoder4 = 200; // back up towards bridge
int encoder5 = 360; // rotate 180 degrees to the large ball
int encoder6 = 900; // go under bridge and knock out large ball
int encoder7 = 90; // turn to goal
int encoder8 = 200; // drive to goal
int encoder9 = 75;
// begin routine
intake(300);
armDownTrim();
driveForwardDead (); //ram big balls
delay (1500);
stopDrive();
delay(500);
driveBack(encoder00); // back up to row abit
intakeDead();
turnLeft (encoder0); // turn 360 degrees
driveBackDead(); // drive back + wall align
delay(1300);
stopDrive();
delay(500);
turnRight (encoder1); // turn to two buckys
intakeDead();
followLine(300); // make sure ur straight
driveForwardSlowDead (); // drive towards buckys
delay(500);
stopDrive();
delay(600);
driveForwardDead(); //get the 2nd ball
delay (200);
stopDrive ();
delay (600);
stopIntake ();
driveBack(encoder3); // back up to bump
armUpDead();// armup
delay(50);
stopArm();
stopIntake();
driveBackSlowDead (); // allign the bump
delay (300);
stopDrive();
delay (500);
driveBackDead(); // over the bump
delay(1000);
stopDrive();
delay(500);
driveForwardSlowDead(); // alighn to bump
delay(800);
stopDrive ();
delay (500);
driveBackSlow(encoder4); // back up towards bridge
turnLeft (encoder5); // rotate 180 degrees to the large ball
armDown(armMin);
armDownTrim();
driveForward (encoder6) ; // go under the bridge + knock large ball
armUp (goalHeight);
turnRight(encoder7); // turn to goal
findLineRight();
followLine(300);
driveForwardSlowDead (); // drive to goal
delay (700);
stopDrive();
outtake (7000); // outtake 3
stopAll ();
}
void Predator::updateAI(){
if (!isMoving()){
_moveTrigger = 0;
if (((Hamster *)CharacterManager::getInstance()->_player)->_caught == true \
||GameManager::getInstance()->_caughtNum >=7){
return;
}
Hamster * nearestSurvivedHamster = nullptr;
int nearestDistanceSq=0;
for (int i = 0 ;i < CharacterManager::getInstance()->_hamsters.size(); i++){
if (CharacterManager::getInstance()->_hamsters.at(i) != nullptr\
&& ((Hamster *)CharacterManager::getInstance()->_hamsters.at(i))->_caught == false\
&& CharacterManager::getInstance()->_hamsters.at(i)->_state != (int)Hamster::State::LOSE){
int offTileX = (int)CharacterManager::getInstance()->_hamsters.at(i)->getPositionX() - (int)this->getPositionX();
int offTileY = (int)CharacterManager::getInstance()->_hamsters.at(i)->getPositionY() - (int)this->getPositionY();
if (nearestSurvivedHamster == nullptr){
nearestSurvivedHamster = (Hamster *)CharacterManager::getInstance()->_hamsters.at(i);
nearestDistanceSq = offTileX*offTileX + offTileY*offTileY;
}
else{
if (offTileX*offTileX + offTileY*offTileY<nearestDistanceSq){
nearestSurvivedHamster = (Hamster *)CharacterManager::getInstance()->_hamsters.at(i);
nearestDistanceSq = offTileX*offTileX + offTileY*offTileY;
}
/*if (((Hamster *)CharacterManager::getInstance()->_hamsters.at(i))->_ranking \
>nearestSurvivedHamster->_ranking){
nearestSurvivedHamster = (Hamster *)CharacterManager::getInstance()->_hamsters.at(i);
}*/
}
}
}
if (nearestSurvivedHamster != nullptr){
if (nearestSurvivedHamster->_caught == false\
&& nearestSurvivedHamster->_state != (int)Hamster::State::LOSE)
{
int offTileX = (int)nearestSurvivedHamster->getPositionX() / 32 - (int)this->getPositionX() / 32;
int offTileY = (int)nearestSurvivedHamster->getPositionY() / 32 - (int)this->getPositionY() / 32;
if (offTileY > 0){
if (offTileY <= _vision){
if (offTileX<0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_LEFT)){
_moveTrigger = CHARACTER_DIRECTION_LEFT;
}
}
else if (offTileX > 0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_RIGHT))
{
_moveTrigger = CHARACTER_DIRECTION_RIGHT;
}
}
else if (offTileX == 0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_UP))
{
_moveTrigger = CHARACTER_DIRECTION_UP;
}
else{
turnUp();
if (abs(nearestSurvivedHamster->getPositionY() - this->getPositionY()) <= 32){
//MessageBox("Caught a hamster!", "test");
nearestSurvivedHamster->_caught = true;
}
}
}
}
else{
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_UP))
{
_moveTrigger = CHARACTER_DIRECTION_UP;
}
else if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_LEFT)){
_moveTrigger = CHARACTER_DIRECTION_LEFT;
}
else if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_RIGHT)){
_moveTrigger = CHARACTER_DIRECTION_RIGHT;
}
}
}
else if (offTileY < 0){
if (-offTileY <= _vision){
if (offTileX<0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_LEFT)){
_moveTrigger = CHARACTER_DIRECTION_LEFT;
}
}
else if (offTileX > 0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_RIGHT))
{
_moveTrigger = CHARACTER_DIRECTION_RIGHT;
}
}
else if (offTileX == 0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_DOWN))
{
_moveTrigger = CHARACTER_DIRECTION_DOWN;
}
else{
turnDown();
if (abs(nearestSurvivedHamster->getPositionY() - this->getPositionY()) <= 32){
//MessageBox("Caught a hamster!", "test");
nearestSurvivedHamster->_caught = true;
}
}
}
}
else{
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_DOWN))
{
_moveTrigger = CHARACTER_DIRECTION_DOWN;
}
else if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_LEFT)){
_moveTrigger = CHARACTER_DIRECTION_LEFT;
}
else if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_RIGHT)){
_moveTrigger = CHARACTER_DIRECTION_RIGHT;
}
}
}
else if (offTileY == 0){
if (offTileX<0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_LEFT)){
_moveTrigger = CHARACTER_DIRECTION_LEFT;
}
else{
turnLeft();
if (abs(nearestSurvivedHamster->getPositionX() - this->getPositionX()) <= 32){
nearestSurvivedHamster->_caught = true;
}
}
}
else if (offTileX > 0){
if (canMove(_destPosition.x, _destPosition.y, CHARACTER_DIRECTION_RIGHT))
{
_moveTrigger = CHARACTER_DIRECTION_RIGHT;
}
else{
turnRight();
if (abs(nearestSurvivedHamster->getPositionX() - this->getPositionX()) <= 32){
nearestSurvivedHamster->_caught = true;
}
}
}
else if (offTileX == 0){
//impossible
}
}
}
}
}
}
void rejectionBlueSkills ()
{
// variables
int armMin = 300;
int wallHeight = 1000;
int goalHeight = 1700;
int pot = analogRead (8);
int midLine = analogRead(2);
int encoder1 = 1800; //drive under wall
int encoder2 = 130; // rotate 90 degrees
int encoder3 = 1400; // backwards to the opponets goal
int encoder4 = 270; //turn to center of large balls
int encoder5 = 200; //hit 1st blue ball off
int encoder55 = 300; // back a bit
int encoder6 = 450; //hit 2nd ball off
int encoder7 = 450; // drive back past line
int encoder8 = 160; // turn to line
// begin routine
intakeDead(); // unfold
delay(300); // needs to clear the wall..
driveBack (encoder1); // drive backwards to under the bridge
stopIntake();
turnLeft (encoder2); // turn ass to opponets goal
driveBackDead(encoder3); // drive to opponets goal
delay(500);
midLine = 3000;
while (midLine > 2000)
{
midLine = analogRead(2);
}
motorSet (motor1, 10) ; // no inertia
motorSet (motor2, 10) ;
motorSet (motor10, 10) ;
motorSet (motor9, 10) ;
delay (85);
armUp(wallHeight);// arm up
turnRight (encoder4); // turn towards center of large balls
driveForwardSlowDead(); // go between large balls
delay(1200);
stopDrive ();
delay(300);
turnLeft (encoder5); //hit center ball
driveBackSlow(encoder55);
turnRight (encoder6); // hit far ball
driveBackSlow (encoder7); // back up past line
turnRight (encoder8); // tun to line
armUp (goalHeight);
findLineLeft();
//followLine(300);
driveForwardDead (); // drive to goal
delay(700);
stopDrive();
outtake(3000);
stopAll ();
delay(60000);
}
void nodeDrop(int armNo, int side)
{
lcd("nodeDrop");
term[ct][side] = arm[armNo];
if (ct == ot)
{
if (((ct == 0 || ct == 1) && dir == 0) || ((ct == 2 || ct == 3) && dir == 2))
{
if (armNo != side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
if (armNo == side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 1) || ((ct == 2 || ct == 3) && dir == 3))
{
if (armNo != side)
turnRight();
}
else if (((ct == 0 || ct == 1) && dir == 3) || ((ct == 2 || ct == 3) && dir == 1))
if (armNo == side)
turnRight();
drop(armNo);
arm[armNo] = -1;
}
else
{
ot = ct;
if (dir == 0 || dir == 2)
if ((dir == 0 && (ct == 0 || ct == 2)) || (dir == 2 && (ct == 1 || ct == 3)))
if (armNo == side)
front();
else
{
front();
turn();
}
else
{
if (armNo == side)
{
turn();
front();
}
else
{
backward();
}
}
else
{
if (armNo == side)
{
if (ct == 0 || ct == 3)
turnRight();
else turnLeft();
front();
}
else
{
if (ct == 0 || ct == 3)
turnLeft();
else turnRight();
backward();
}
}
drop(armNo);
arm[armNo] = -1;
}
ot = ct;
//..printf("Arm %d dropped %d on term[%d][%d]\n", armNo, term[ct][side], ct, side);
sorted++;
}
void AVATAR::run() //Führt einen Simulationsschritt durch
{
if(death!=0) //Avatar=Tot
{ /*deathProgress+=DIEINGSPEED;
if(deathProgress>=100) //Jetzt das Spiel beenden
deathProgress=100;
*/
return;
}
movement.blocking=0; //Falls der Avatar blockiert wird dieser immer auf moving=1 und -1 gesetzt --> collision-Animation muss verhindert werden können
if(movement.moving!=0)
{ if(movement.moving==-1 && propRicCmp(movement.richtung,game->getFieldProperty(OBJ_AVATAR,position))) //Der Avatar prallt (genau jetzt) ab, darf aber nicht zurückkehren, weil der Feldtyp die Bewegung sofort wieder starten würde. (Ansonsten wackelt der Avatar immer hin und her)
{ //Prüfen, ob das Hinderniss bereits aus dem Weg ist und der Weg fortgesetzt werden darf:
movement.moving=1;
//movement.progress=OccupiedLimit-1;//Damit das Zielfeld nicht von diesem Objekt blockiert wird (kommt vor, wenn mehrere Kugeln im Start-Beamer sind und der Avatar über den Zielbeamer fährt)
movement.blocking=1;
movement.progress=Wkgl;
//Wenn nein, wird automatisch innerhalb dieses if's wieder abgeblockt und im nächsten Durchgang landet man hier
}else
movement.progress+=movement.moving*AVATAR_SPEED; //Vor bzw. Zurück Bewegen
POS next=game->getTargetFieldCoord(position,movement.richtung);
if(movement.moving==1 && movement.richtung!=BEAM && movement.progress >= Wkey && movement.progress < Wkey+AVATAR_SPEED)//Bei genau diesem Schritt könnte ein Schlüssel berührt und das Schloss geöffnert werden
{ LOCK *touchedLock=game->KeyOnField(next);
if(touchedLock != NULL)
{ game->openLock(touchedLock); //Öffnet (=löscht) das Schloss
game->addGameLogEvent(LOCKOPENED, NONE); //Ereignis berichten
}
}
if(movement.moving==1 && movement.richtung!=BEAM && movement.progress >= Wkgl && movement.progress < Wkgl+AVATAR_SPEED)//Bei genau diesem Schritt könnte angestoßen werden
{ KUGEL *touchedKugel=game->KugelOnField(next,NULL);
if(touchedKugel != NULL) //Im nächsten Feld ist eine Kugel
{ ///IM NÄCHSTEN FELD IST EINE KUGEL --> Es muss etwas passieren
POS next2=game->getTargetFieldCoord(next,movement.richtung); //Feld nach der Kugel
if(movement.blocking || /*vorheriger Bedingungsteil: siehe unten*/game->getFieldProperty(OBJ_KUGEL,next)==fx || game->checkPos(next2)) //Kugel fixiert ODER Übernächste Position außerhalb des Spielfeldes
{ movement.moving=-1; //Der Avatar prallt sofort ab (Die Kugel wird sich nicht bewegen)
if(!movement.blocking)
{ game->addFieldEffect(position,COLLISION,movement.richtung,AVATARCOLLITIONCOLOR);
/*Überprüfen, ob sich ein anderes Objekt gerade in dieses Feld bewegen will, in die der Avatar jetzt zurückkehren muss:*/
game->stopMovementsTo(position,Wava); //Alle Objekte nach Wava % abprallen lassen (sofern möglich.)
}
//Grund, warum oben movement.blocking in der Bedingung steht:
// Wenn der Avatar gegen ein Feld gedrückt wird das nicht betreten werden kann, und sich in diesem Feld eine Kugel befindet,
// Dann würde diese automatisch weggeschoben werden. Das passiert bei jeder Kugel die in ein solches Feld kommt.
// Damit dass verhindert wird kann die Kugel nicht angestoßen werden, wenn der Avatar blockiert.
// Sie kann nur dann angestoßen werden, wenn der Benutzer den Avatar in die entsprechende Richtung steuert. Auch wenn dies die Richtung ist, in die er gedrückt und blockiert wird. So kann und darf die Kugel dann angestoßen werden
//
//Wann kann es vorkommen (Feldwerte laut altem Levelformat):
// 07 <--- Hier ist die Kugel
// 07
// 12 11 01 <--- In dieses Freie Feld soll die Kugel nur dann geschoben werden, wenn der Spieler es will, nicht automatisch
// ^ 25
// |
// Hier ist der Avatar, der gegen das Feld drückt in das die Kugel kommen wird
}else
{ //Übernächstes Feld überprüfen:
int limit2=game->isWalkable(OBJ_KUGEL,next2); //Wie weit sich die Kugel weiterbewegen kann
if(limit2>0) //Kugel kann sich bewegen -> Anstoßen
{ touchedKugel->move(movement.richtung); //Kugel anstoßen
}
if(limit2<100) //Die Kugel kann sich nicht ewig weiterbewegen --> Der Avatar prallt irgendwann zurück (darf nicht ins nächste Feld)
{ if(movement.limit > Wkgl + limit2)
{ movement.limit = Wkgl + limit2; //Limit setzen
}else if(movement.limit >= 100) //ZB. Die Kugel kann sich 80% bewegen und prallt dann ab. Der Avatar ist aber schon bei 22% --> 102%, obwohl der Avatar zurückprallen muss
{ movement.limit=99;
}
}
}
}
}
if(movement.moving==1 && movement.limit<100 && movement.progress>=movement.limit) //Prüfen, ob er jetzt vlt. abprallt
{ movement.moving=-1; //Abprallen
movement.progress=movement.limit; //Genau an die Kante setzen
if(!movement.blocking)
{ game->addFieldEffect(position,COLLISION,movement.richtung,AVATARCOLLITIONCOLOR);
/*Überprüfen, ob sich ein anderes Objekt gerade in dieses Feld bewegen will, in die der Avatar jetzt zurückkehren muss:*/
game->stopMovementsTo(position,Wava); //Alle Objekte nach Wava % abprallen lassen (sofern möglich.)
}
}
///Prüfen, ob die Bewegung schon abgeschlossen wurde:
if(movement.moving==-1 && movement.progress<=0) //Ins Startfeld zurückgekehrt
{ movement.moving=0;
movement.lastRichtung=NONE;
}
if(movement.moving==1 && movement.progress>=100) //Im Zielfeld angekommen
{ position=next;
movement.moving=0;
movement.lastRichtung=movement.richtung;
game->addGameLogEvent(userinput?USERAVATARMOVE:AVATARMOVE,movement.richtung); //Ereignis berichten
userinput=0;
}
if(movement.blocking && movement.moving!=-1)
{ movement.blocking=0; //Der Avatar hat blockiert, wurde jetzt aber wieder freigegeben
}
}
if(movement.moving==0) //Steht still --> prüfen, ob sich der Avatar auf einem Spezialfeld befindet
{ switch(game->getFieldProperty(OBJ_AVATAR,position)) //Dieses Feld hat vlt. eine spezielle Eigenschaft
{ case up: move(UP); break;
case dn: move(DOWN); break;
case lt: move(LEFT); break;
case rt: move(RIGHT); break;
case ic: move(movement.lastRichtung); break; //Weiter rutschen
case bm: move(BEAM); break; //Zum Ziel-Beamer versetzen
case tr: move(turnRight(movement.lastRichtung)); break; //Nach rechts (90° im Uhrzeigersinn) weiterruthschen
case tl: move(turnLeft (movement.lastRichtung)); break; //Nach links (90° gegen Uhrzeigersinn) weiterruthschen
case dt:{ /*Tödlich*/
death=-1;
int feld=game->getField(position); //Feldtyp herausfinden
switch(feld)
{ case TILE_LAVA: death=game->addFieldEffect(position,AVATARLAVA); break; /*lava*/
default: error("AVATAR::run()","Ubekanntes, toedliches Feld. Es wird keine Animations ausgegeben. feld: %d",feld);
}
game->addGameLogEvent(AVATARDEATH, NONE); //Ereignis berichten
game->addGameLogEvent(GAMEOVER, NONE); //Ereignis berichten
}break;
case fx: /*do nothing*/ break;
case nm: /*do nothing*/ break;
default: error("AVATAR::run()","Unbekannte Feldeigenschaft gefunden. Verarbeite wert als \"nm\" (Normal). fieldproperty=%d",game->getFieldProperty(OBJ_KUGEL,position));
}
}
}
void moveToTau(Servo *leftWheel, Servo *rightWheel, Servo *penServo){
forward(leftWheel, rightWheel, DISTANCE*(.417 + .1)); //Moving over to the next letter
turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES / 2 - 30);
}
task autonomous()
{
// .....................................................................................
// Insert user code here.
// .....................................................................................
if(programs ==0)
{
StartTask(intakeStart);
/*while(SensorValue[bumperLeft]==0)
{
}
*/
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
motor[secondTier]=-127;
wait10Msec(50);
motor[secondTier]=0;
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(150);
//intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,-10,650);
moveStraightDistance(127,100);
turnRight(127,100);
stopPid(0.6,0.3);
wait10Msec(50);
//moveFirstTierUp(127,1800);
//moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 1000);
moveSharpRight(127,0,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
StopTask(intakeStart);
}
else if(programs ==1)
{
}
else if(programs ==2)
{
}
}
void position(int armNo,int side)
{
if (ct == ot)
{
if (((ct == 0 || ct == 1) && dir == 0) || ((ct == 2 || ct == 3) && dir == 2))
{
if (armNo != side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
if (armNo == side)
turn();
}
else if (((ct == 0 || ct == 1) && dir == 1) || ((ct == 2 || ct == 3) && dir == 3))
{
if (armNo != side)
turnRight();
else turnLeft();
}
else if (((ct == 0 || ct == 1) && dir == 3) || ((ct == 2 || ct == 3) && dir == 1))
if (armNo == side)
turnRight();
else turnLeft();
}
else
{
if (dir == 0 || dir == 2)
{
if ((dir == 0 && (ct == 0 || ct == 1)) || (dir == 2 && (ct == 2 || ct == 3)))
{
if (ot != 4 && ot != 5)
{
turnLeft();
front();
}
if (armNo == side)
{
front();
}
else
{
front();
turn();
}
}
else
{
if (armNo == side)
{
if (ot != 4 && ot != 5)
{
turnLeft();
front();
}
else turn();
front();
}
else
{
if (ot != 4 && ot != 5)
{
turnLeft();
front();
}
front();
}
}
}
ot = ct;
}
}
/*-----------------------------------------------------------------------------
* Function name: learn_track
* Description: This function navigates through the maze and solves it
* Inputs: maze = Maze * = pointer to Maze structure
* robot = RobotModel * = pointer to RobotModel
* turns = char * = array of characters to store the turns made to solve
* the maze (passed by simulated reference)
* curTurnIndex = int * = the index position where the next turn
* made by the robot will be stored in turns array
* (passed by simulated reference)
* code = int * = the last administrative code collected
* (passed by simulated reference)
* targetStation = int = the targetStation input by the user
* Output: int = 0 if the targetStation is not found
* targetStation otherwise
*----------------------------------------------------------------------------*/
int learn_track(Maze *maze, RobotModel *robot, char *turns, int *curTurnIndex, int *code, int targetStation)
{
int done = 0;
int uTurned = 0;
int traversalMode = 2; /* e.g. 0: leftHandOnWallMode, 1: rightHandOnWallMode, 2: mainLoopMode */
int station = 0; // returns the station reached
int bToL, bToR, bInF; // valiables indicating black to left(L), right(R), or frontF)
int onB, i; // valiables indicating on black
while (!done)
{
clear();
printMazePlusCurrentPos(*maze, *robot);
/*
TODO:
Determine the values of bToL, bToR, bInF, onB
Determine if the robot has reached the packing station (which means learn track is done but the targetStation was not found)
If the robot is not at a junction and can only move straight, move straight
otherwise
if traversalMode indicates mainLoopMode
decide the appropriate turn following mainLoopMode
NOTE: since the main loop has no dead-end, it is always posible to make a L, R, or S turn
apply the turn on the robot and save it into turns array
else if traversalMode indicates leftHandOnWallMode
decide the appropriate turn following leftHandOnWallMode
decide u-turn when neither of L, R, or S turn is possible (i.e., a dead-end and a new storage station has been reached)
apply the turn on the robot and save it into turns array
else if traversalMode indicates rightHandOnWallMode
decide the appropriate turn following rightHandOnWallMode
decide u-turn when neither of L, R, or S turn is possible (i.e., a dead-end and a new administrative station has been reached)
apply the turn on the robot and save it into turns array
Update the current traversalMode
*/
/*Lets the robot know where the location of the paths of the maze are with respect to itself*/
bToL = blackToLeft(*maze, *robot);
bToR = blackToRight(*maze, *robot);
bInF = blackInFront(*maze, *robot);
onB = onBlack(*maze, *robot);
/*checks to make sure that the robot is not surrouned by open paths because the only time that would happen is when it is at an invalid spot*/
/*if this is true, the program ends the learn_maze function and lets the main function know that there was an error by changing the station variable to 0*/
if(bToL && bToR && bInF && onB)
{
done = 1;
station = 0;
}
/*Checks to see if the only way that the robot can go is forward and if that is true, it moves the robot forward*/
else if(bToL == 0 && bToR == 0 && onB == 1)
{
moveStraight(robot);
}
/*If the previous comparsions are false, the robot checks to see what type of traversal mode it is in*/
/*In "Left hand mode" that robot tries to first turn left, then move straight, then turn right, and then finally U-turn to continue on the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time a u-turn is recorded, the station number is incremented unless the robot is at the target station at which point the function is complete*/
else if(traversalMode == 0)
{
if(bToL == 1)
{
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else if(bInF == 1)
{
moveStraight(robot);
}
else if(bToR == 1)
{
turnRight(robot);
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else
{
uTurn(robot);
station++;
if(station == targetStation)
{
done = 1;
}
turns[*curTurnIndex] = 'U';
if(!done)
{
*curTurnIndex += 1;
}
uTurned = 1;
}
}
/*In "Right hand mode" that robot tries to first turn right, then move straight, then turn left, and then finally U-turn to continue on the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time a u-turn is recorded, the function asks the user for an administrative code. It will not let the user continue until a code greater than 0 is entered. Each time a new code is entered, the old one is deleted*/
else if(traversalMode == 1)
{
if(bToR == 1)
{
turnRight(robot);
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else if(bInF == 1)
{
moveStraight(robot);
}
else if(bToL == 1)
{
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else
{
uTurn(robot);
turns[*curTurnIndex] = 'U';
*curTurnIndex += 1;
*code = 0;
do
{
printw("\nGive Administrative Code (>0): ");
scanw("%d", code);
}while(*code <= 0);
uTurned = 1;
}
}
/*In "Main loop mode" that robot tries to first turn left, then turn right, and then move straight the path of the maze*/
/*Each turn is recorded in the turns array at the index of curTurnIndex, which is incremented after each turn except for the last turn to let the main function know how many turns there are*/
/*Each time the robot turns, it checks to see if it is at an intersection to see if it needs to change the traversal mode for the appropriate branch of the maze*/
else if(traversalMode == 2)
{
if(bToL == 1)
{
if(bInF == 1 || bToR == 1)
{
traversalMode = 0;
}
turnLeft(robot);
turns[*curTurnIndex] = 'L';
*curTurnIndex += 1;
}
else if(bToR == 1)
{
turnRight(robot);
if(bInF == 1 || bToL ==1)
{
traversalMode = 1;
}
turns[*curTurnIndex] = 'R';
*curTurnIndex += 1;
}
else
{
moveStraight(robot);
}
}
/*This switch case checks to see if the robot needs to be put back in the main loop and there fore needs to be put back into main loop traversal mode*/
switch(traversalMode)
{
case 0:
if(bToL == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
uTurned = 0;
}
/*This is a special case to check to see if the robot needs to travel straight and that needs to be recored for the program to simplifiy the instructions*/
else if(bInF == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
turns[*curTurnIndex] = 'S';
(*curTurnIndex) += 1;
uTurned = 0;
}
break;
case 1:
if(bToL == 1 && bToR == 1 && uTurned == 1)
{
traversalMode = 2;
uTurned = 0;
}
break;
default:
break;
}
getch();
}
return station;
}
void autonSkills(){ //autonFiveLEft grabs the five stack, starts on left tile.
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeRight(127, .3 * clickspermeters);
drive( .1 * clickspermeters, FORWARD, 70);
wait1Msec(20);
wrist(-127, 1100);
//AArm(300 , LOWER, 60);
wait1Msec(400);
drive( .7 * clickspermeters, FORWARD, 80);
StartTask(five);
wrist(127, 600);
wait1Msec(600);
driveBackward(100, 550);
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
strafeLeft(100, clickspermeters);
wait1Msec(20);
turnLeft(60, 600);
wait1Msec(20);
drive( .43 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 400);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
turnLeft(60, 100);
wait1Msec(20);
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2800){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(100);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -35;
motor[leftWrist] = -35;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(2000);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 850);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeRight(127, .9 * clickspermeters);
turnRight(100, 200);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
drive( .4 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(300);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
SensorValue[rightIEM]=0;
SensorValue[leftIEM]=0;
drive( .37 * clickspermeters, FORWARD, 70);
wait1Msec(20);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 650);
wait1Msec(500);
driveBackward(0, 20);
wait1Msec(20);
while(SensorValue[WristPot1] < 2200){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
strafeLeft(127, .5 * clickspermeters);
driveBackward(127 , 700);
wait1Msec(5000);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .25 * clickspermeters, FORWARD, 70);
while(SensorValue[ArmPot1] < 3900){
motor[leftTopArm] = -127;
motor[leftBottomArm] = -127;
motor[rightTopArm] = -127;
motor[rightBottomArm] = -127;
}
wait1Msec(20);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
while(SensorValue[WristPot1] > 2200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
wait1Msec(20);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
wait1Msec(20);
drive( .7 * clickspermeters, FORWARD, 70);
wait1Msec(20)
while(SensorValue[WristPot1] <2700){
motor[leftWrist] = 127;
motor[rightWrist] = 127;
}
wait1Msec(100);
motor[leftWrist] = 0;
motor[rightWrist] = 0;
driveBackward(100, 900);
wait1Msec(10000);
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
while(SensorValue[ArmPot1] > 2100){
motor[leftTopArm] = 127;
motor[leftBottomArm] = 127;
motor[rightTopArm] = 127;
motor[rightBottomArm] = 127;
motor[rightWrist] = -27;
motor[leftWrist] = -27;
}
driveBackward(0, 20);
wait1Msec(50);
motor[leftTopArm] = 0;
motor[leftBottomArm] = 0;
motor[rightTopArm] = 0;
motor[rightBottomArm] = 0;
wait1Msec(20);
strafeLeft(127, clickspermeters * .5);
drive( 1.15 * clickspermeters, FORWARD, 70);
while(SensorValue[WristPot1] > 200){
motor[leftWrist] = -127;
motor[rightWrist] = -127;
}
motor[leftWrist] = 0;
motor[rightWrist] = 0;
}
task main()
{
initializeRobotAUTO();
resetEnc();
waitForStart();
determineFirst();
while(firstTest == true){
rotateServo();
}
while(secondTest == false){
stopDrive();
}
while(secondTest == true){
gotoIR_FORWARD();
storeEncoderValues();
resetEnc();
delayStop(500);
//switch case
moveForwardif(1.75);
delayStop(200);
turnLeft(_90DEGREES);
delayStop(500);
moveBackward(7);
resetEnc();
scoreAuto();
moveForward(3.5);
resetEnc();
delayStop(300);
turnRight(_90DEGREES);
resetEnc();
stopDrive();
delayStop(300);
gotoEnd_for();
delayStop(300);
turnLeft(_135DEGREES + 215);
delayStop(300);
moveBackward(28.9);
delayStop(300);
turnRight(_90DEGREES + 135);
delayStop(200);
moveBackward(39.5);
delayStop(200);
moveForward(3.5);
delayStop(350);
moveBackward(3.5);
delayStop(350);
retractServo();
stopDrive();
secondTest = false;
}
for(;;){}//loop forever
}
/** solveMazeBest **********************************
* Follows the left side of a maze until reaching the target while
* recording the route taken while simplifying.
*
* @params route -- an array that is filled with the route taken
* maxLen -- the maximum number of intersections that can be recorded
*
* @returns -- the number of items in route
*/
int solveMazeBest(char route[], int maxLen) {
int index = 0;
// passed to follow segment to record if a left, right or straight intersection happened
bool left = true;
bool right = true;
bool straight = true;
// keep going while there is a turn or we can go straight and we are not on the target
while (true) {
// just follow the line until something interesting happens
followSegment(left, right, straight);
// if we are finished the maze we break out of the loop
if (done) {
break;
}
// stop when we reach max turns
if (index == maxLen) {
break;
}
// left turn found, record and make the turn
if (left) {
route[index] = turn_left;
index++;
turnLeft();
}
// no left but a straight, record and move ahead a bit to get away from the intersection
else if (straight) {
route[index] = straight;
set_motors(motor_speed, motor_speed);
delay_ms(200);
set_motors(0,0);
index++;
}
// right turn, record and make the turn
else if (right) {
route[index] = turn_right;
index++;
turnRight();
}
// dead end do a 180
else {
route[index] = turn_dead_end;
index++;
// move forward a fair bit before making the turn
// this fixes a problem with a dead end being found shortly after
// making a turn
set_motors(motor_speed, motor_speed);
delay_ms(425);
// make a turn
set_motors(0,0);
makeTurn(false);
}
// remove any dead ends if found
index = trimRoute(route, index);
}
// move forward a tiny bit at the end and stop
set_motors(motor_speed, motor_speed);
delay_ms(200);
set_motors(0,0);
return index;
}
/*
* 函数名称：moveForward
* 函数功能：人向前走一步
* 返回内容：YES:移动一次 NO:没有移动
*/
bool MazePerson::moveForward()
{
//检查当前位置是否合法
if(MazeMap::checkWallOrNot(positionX,positionY))
{
//检测到当前位置是墙，提示用户设计非法并返回
gotoxy(0,23);
cout << "检测到当前设定的起始位置是墙壁，请重新设定。" << endl;
outOrNot = true;
return NO;
}
//判断如果前一个位置的坐标是10000，则直接在当前位置绘制人
if(preX == 10000 || preY == 10000)
{
gotoxy(positionX,positionY);
cout << personPic;
Sleep(5000);
}
//检查右侧是否是墙，如果是墙，检查前方是否是墙，如果不是墙则前进，前进后检查是否是出口
int rightX;
int rightY;
int forwardX;
int forwardY;
switch(forward)
{
case SOUTH: rightX = positionX - 1; rightY = positionY;
forwardX = positionX; forwardY = positionY + 1; break;
case NORTH: rightX = positionX + 1; rightY = positionY;
forwardX = positionX; forwardY = positionY - 1; break;
case WEST: rightX = positionX; rightY = positionY - 1;
forwardX = positionX - 1; forwardY = positionY; break;
case EAST: rightX = positionX; rightY = positionY + 1;
forwardX = positionX + 1; forwardY = positionY; break;
}
//如果右侧是墙
//system("PAUSE");
if(MazeMap::checkWallOrNot(rightX,rightY))
{
//检测前方也是墙壁
if(MazeMap::checkWallOrNot(forwardX,forwardY))
{
//按逆时针换一个方向
turnLeft();
return NO;
}
//检测前方是通路
else
{
//向前移动
goAhead();
return YES;
}
}
//如果右侧是通路
else
{
Sleep(3000);
turnRight();
goAhead();
return YES;
}
}
