Exemple #1
0
void driveSpinToHeading(float targetHeading) {  //-values are clockwise, +values are cclockwise
  if (isGyroIntegrateTaskRunning != true) {
    sensorsGyroStartIntegrateTask();
  }

  int direction = targetHeading - sensorsGyroGetHeading();
  bool stopLoop = false;
  while (stopLoop != true) {      //TODO: add code to move motors when less sleepy
    sensorsGyroIntegrate();
    if (direction > 0) {  //rotate left
      stopLoop = ((targetHeading - sensorsGyroGetHeading()) > 0);
    } else {              //rotate right
      stopLoop = ((targetHeading - sensorsGyroGetHeading()) < 0);
    }
  }
  driveStop();

  stopLoop = false;
  while (stopLoop != true) {      //TODO: add code to move motors when less sleepy
    sensorsGyroIntegrate();
    if (direction > 0) {  //rotate left
      stopLoop = ((targetHeading - sensorsGyroGetHeading()) > 0);
    } else {              //rotate right
      stopLoop = ((targetHeading - sensorsGyroGetHeading()) < 0);
    }
  }
  driveStop();
}
// TODO: use correct sensor port (change to irBack)
task main() {
	calibrateGyro();

	{ // Drive to the IR basket, dump the block, drive back & square up against the wall
		int timeForward;

		ClearTimer(T1);
		driveBackward(DRIVE_SPEED);
		while (true) {
			if (time1[T1] > SEARCH_TIME)
				break;
			else if (SensorValue[irFront] == 2)
				break;
		}

		timeForward = time1[T1];

		// Keep moving to adjust for the first 2 / last 2 baskets
		if (timeForward < MIDDLE_TIME) {
			// Back up a little
			driveStop();
			wait1Msec(500);
			driveForward(FIRST_HALF_DELAY, DRIVE_SPEED);
			timeForward -= FIRST_HALF_DELAY;
		} else {
			// Back up a little
			driveStop();
			wait1Msec(500);
			driveForward(SECOND_HALF_BACKUP_TIME, DRIVE_SPEED);
			timeForward -= SECOND_HALF_BACKUP_TIME;
		}

		driveStop();
		flipper_flip();
		driveForward(timeForward + 1500, DRIVE_SPEED);
		motor[rightDrive] = DRIVE_SPEED;
		wait1Msec(500);
		driveStop();
	}

	{ // Drive next to the ramp & turn on to it
		motor[leftDrive] = -DRIVE_SPEED;
		wait1Msec(1050);
		motor[rightDrive] = -DRIVE_SPEED;
		wait1Msec(1300);
		driveStop();
		turnRightEuler(TURN_90_EULER, DRIVE_SPEED);
		PlaySound(soundBeepBeep);
		driveBackward(1500, DRIVE_SPEED);
	}
}
Exemple #3
0
void driveUltrasonicDistanceHeadingHold(int power, int dist, bool stopWhenDone) {
  if (isGyroIntegrateTaskRunning != true) {
    sensorsGyroStartIntegrateTask();
  }

  bool stopLoop = false;
  float headingToHold = sensorsGyroGetHeading();
  while (stopLoop != true) {
	  sensorsGyroIntegrate();                   //update current heading
	  float headingError = sensorsGyroGetHeading() - headingToHold; //calculate how far the robot is off

	  int powerLeft = power + (headingError*Kp_hdghold);  //P term loop for heading
 	  int powerRight = power - (headingError*Kp_hdghold); //hold
	  driveMotors(powerLeft, powerRight);

	  if (power > 0) {
	    stopLoop = (sensorsUltrasonicGetDistance() > dist); //robot is driving forward
	  } else {
	    stopLoop = (sensorsUltrasonicGetDistance() < dist); //robot is driving backwards
	  }
  }

  if (stopWhenDone) {
    driveStop();
  }
}
Exemple #4
0
int Create::translate2(int distance, int speed) {

	if (distance*speed < 0) return EXIT_FAILURE; //sign test

	bool bump = false;
	int currDistance = 0;
	sensorsDistance();
	driveDirect(speed, speed);

	int sign = distance<0?-1:1;

	while(sign*currDistance < sign*distance){
		currDistance += sensorsDistance();
		std::cout<<"currDistance: "<<currDistance<<std::endl;
		if(sensorsBump()){
			bump = true;
			break;
		}
	}
	driveStop();
	if (bump){
		translate(-currDistance, -speed);
		return EXIT_FAILURE;
	}
	return EXIT_SUCCESS;
}
void driveDistanceOp(int16_t velocity, int16_t distance) {
    // Start driving
    drive(velocity, RadStraight);
    // Halt execution of new commands on the Create until reached distance
    byteTx(WaitForDistance);
    byteTx((uint8_t)((distance >> 8) & 0x00FF));
    byteTx((uint8_t)(distance & 0x00FF));
    // Stop the Create
    driveStop();
}
void driveDistanceTFunc(int16_t velocity, int16_t distance, void (*func)(void),
        uint16_t period_ms, uint16_t cutoff_ms) {
    // Calculate the delay
    uint32_t time_ms = (1000 * (uint32_t)distance) / (uint32_t)velocity;
    // Start driving
    drive(velocity, RadStraight);
    // Wait delay
    delayMsFunc(time_ms, func, period_ms, cutoff_ms);
    // Stop the Create
    driveStop();
}
void driveAngleTFunc(int16_t velocity, int16_t radius, int16_t angle,
        void (*func)(void), uint16_t period_ms, uint16_t cutoff_ms) {
    // Calculate the delay
    uint32_t time_ms = (PIe5 * TENTH_RADIUS * (uint32_t)angle)
        / (1800 * (uint32_t)velocity);
    // Start driving
    drive(velocity, radius);
    // Wait delay
    delayMsFunc(time_ms, func, period_ms, cutoff_ms);
    // Stop the Create
    driveStop();
}
FILE *ps2_fopen(const char *fname, const char *mode) {
	if (cacheListSema == -1) {
		ee_sema_t newSema;
		newSema.init_count = 1;
		newSema.max_count = 1;
		cacheListSema = CreateSema(&newSema);
		assert(cacheListSema >= 0);
	}

	printf("ps2_fopen: %s, %s\n", fname, mode);

	if (!checkedPath && g_engine) {
		// are we playing from cd/dvd?
		const char *gameDataPath = ConfMan.get("path").c_str();
		printf("Read TOC dir: %s\n", gameDataPath);
		if (strncmp(gameDataPath, "cdfs:", 5) != 0)
			driveStop(); // no, we aren't. stop the drive. it's noisy.
		// now cache the dir tree
		tocManager.readEntries(gameDataPath);
		checkedPath = true;
	}

	if (((mode[0] != 'r') && (mode[0] != 'w')) || ((mode[1] != '\0') && (mode[1] != 'b'))) {
		printf("unsupported mode \"%s\" for file \"%s\"\n", mode, fname);
		return NULL;
	}

	bool rdOnly = (mode[0] == 'r');

	int64 cacheId = -1;
	if (rdOnly && tocManager.haveEntries())
		cacheId = tocManager.fileExists(fname);

	if (cacheId != 0) {
		Ps2File *file = findInCache(cacheId);
		if (file)
			return (FILE*)file;

		if (rdOnly) {
			bool isAudioFile = strstr(fname, ".bun") || strstr(fname, ".BUN") || strstr(fname, ".Bun");
			file = new Ps2ReadFile(cacheId, isAudioFile);
		} else
			file = new Ps2WriteFile(cacheId);

		if (file->open(fname)) {
			openFileCount++;
			return (FILE*)file;
		} else
			delete file;
	}
	return NULL;
}
Exemple #9
0
void *rpcServer(int func, void *data, int size) {
	switch (func) {
		case READ_RTC:
			return rpcReadClock(data);
		case DRIVE_STOP:
			return driveStop(data);
		case DRIVE_STANDBY:
			return driveStandby(data);
		default:
			printf("Unknown RPC command %d\n", func);
			return NULL;
	}
	return NULL;
}
void driveAngleOp(int16_t velocity, int16_t radius, int16_t angle) {
    // Wait for angle opcode compatibility
    if (radius == RadCW) {
        angle = -angle;
    }
    // Start driving
    drive(velocity, radius);
    // Halt execution of new commands on the Create until reached angle
    byteTx(WaitForAngle);
    byteTx((uint8_t)((angle >> 8) & 0x00FF));
    byteTx((uint8_t)(angle & 0x00FF));
    // Stop the Create
    driveStop();
}
Exemple #11
0
task main() {
	initializeRobot();

	bDisplayDiagnostics = false; //disable diagnostics display

	eraseDisplay();
	nxtDisplayCenteredBigTextLine(6, "L   R");
	getButton();

	switch (button) {
	case kRightButton:
		scan_direction = -1;
		break;
	case kLeftButton:
		scan_direction = 1;
		break;
	default:
		StopAllTasks();
	}

	eraseDisplay();
	nxtDisplayCenteredTextLine(4, "Both");
	nxtDisplayCenteredTextLine(6, "Block|Ramp");
	getButton();

	switch (button) {
	case kRightButton:
		ramp = true;
		block = false;
		break;
	case kLeftButton:
		ramp = false;
		block = true;
		break;
	case kEnterButton:
		ramp = true;
		block = true;
		break;
	default:
		StopAllTasks();
	}

	if (block && ramp) {

		eraseDisplay();
		nxtDisplayCenteredTextLine(4, "Ramp Side?");
		nxtDisplayCenteredTextLine(6, "Far|Closest|Near");
		getButton();

		switch (button) {
		case kRightButton: //near side
			ramp_direction = -scan_direction;
			break;
		case kLeftButton: //far side
			ramp_direction = scan_direction;
			break;
		case kEnterButton: //closest side based on encoders
			ramp_direction = 0;
			break;
		default:
			StopAllTasks();
		}
	}

	eraseDisplay();
	nxtDisplayCenteredTextLine(1, "Wait for Start?");
	nxtDisplayCenteredTextLine(6, "Yes    No");
	getButton();

	switch (button) {
	case kRightButton:
		wait = false;
		break;
	case kLeftButton:
		wait = true;
		break;
	default:
		StopAllTasks();
	}

	eraseDisplay();
	nxtDisplayCenteredBigTextLine(1, "Debug?");
	nxtDisplayCenteredBigTextLine(6, "Yes   No");
	getButton();

	switch (button) {
	case kRightButton:
		debug = false;
		break;
	case kLeftButton:
		debug = true;
		break;
	default:
		StopAllTasks();
	}

	//optional delay before starting autonomous
	eraseDisplay();
	nxtDisplayCenteredBigTextLine(1, "Delay?");
	nxtDisplayCenteredBigTextLine(6,"0");

	int delay = 0; //number of seconds to delay
	bool done = false;
	while(!done) {
		nxtDisplayCenteredBigTextLine(6," %d ",delay);
		getButton();
		switch (button) {
		case kRightButton:
			delay++;
			break;
		case kLeftButton:
			if (delay > 0)
				delay--;
			break;
		case kEnterButton:
			done = true;
			break;
		default:
			StopAllTasks();
		}
	}

	eraseDisplay();
	nxtDisplayCenteredBigTextLine(0, "Ready?");
	nxtDisplayCenteredBigTextLine(6, "Go!");
	getButton();

	switch (button) {
	case kEnterButton:
		break;
	default:
		StopAllTasks();
	}

	////////////////////////////////////////////////////
	//code that actually runs during match starts here//
	////////////////////////////////////////////////////

	eraseDisplay();
	bDisplayDiagnostics = true; //reenable diagnostics display

	if (wait)
		waitForStart();

	wait1Msec(delay*1000);

	//just get on ramp via dead reckoning

	if (ramp && !block) {

		goForward(100);
		wait1Msec(900);

		driveFast(scan_direction*100,0);
		wait1Msec(2100);

		driveStop();
	}

	//score block in IR goal
	else {
		pwr_x = scan_direction*PWR_SCAN;
		driveTilted(pwr_x, pwr_y);
		while (abs(nMotorEncoder[M_DRIVE_FL]) < 6500) {
			//wait for it to reach the wall
		}
		//we are now at the near edge of the wall//
		nMotorEncoder[M_DRIVE_FL] = 0;
		while (SensorValue[SONAR] < 200) {
			//TODO: Check this value, or maybe make it use sonar
			// 			to detect end of wall instead.

			nxtDisplayString(3, "%8d     ", nMotorEncoder[M_DRIVE_FL]);
			//nxtDisplayBigStringAt(0,50, "IR:%d   ", SensorValue[IR]);

			if (SensorValue[IR] == 5 || abs(nMotorEncoder[M_DRIVE_FL]) > 6500) {
				//TODO: use sonar instead of encoder  we see the IR beacon

				scored = true;
				//wait1Msec(80);
				driveStop();
				int near_edge = abs(nMotorEncoder[M_DRIVE_FL]);
				while (SensorValue[IR] == 5){
					trackWall(35, 100);
					driveTilted(pwr_x, pwr_y);
				}
				driveStop();
				int far_edge = abs(nMotorEncoder[M_DRIVE_FL]);
				while (abs(nMotorEncoder[M_DRIVE_FL]) > (near_edge + far_edge)/2) {
					trackWall(35, 100);
					driveTilted(-pwr_x, pwr_y);
				}
				driveStop();
				nxtDisplayString(3, "%8d     ", nMotorEncoder[M_DRIVE_FL]);

				if (ramp_direction == 0) { //if we have not already chosen a direction for the ramp
					if (abs(nMotorEncoder[M_DRIVE_FL]) > 2000) //TODO: calibrate this value, should be halfway
						ramp_direction = scan_direction;
					else
						ramp_direction = -scan_direction;
				}

				pwr_x = 0;
				pwr_y = 75;
				while(true) {
					int ir = SensorValue[IR];
					if (ir < 5){
						pwr_x = -30;
						pwr_y = 0;
					}
					if (ir > 5) {
						pwr_x = 30;
						pwr_y = 0;
					}
					else {
						pwr_x = 0;
						pwr_y = 50;
					}

					if (SensorValue[SONAR] < 30)
						break;

					//trackWall(30,50);

					driveTilted(pwr_x,pwr_y);
				}

				driveStop();

				if (debug)
					wait1Msec(1000);

				wait1Msec(300);
				servo[SV_AUTO] = 130; //score the autonomous block
				wait1Msec(400);
				servo[SV_AUTO] = 24;
				wait1Msec(100);

				if (debug)
					wait1Msec(1000);

				break;
			}
			trackWall(35, 50);
			driveTilted(pwr_x, pwr_y);
		}

		driveStop();

		if (ramp) {
			//go remaining distance

			pwr_x = ramp_direction*1.5*PWR_SCAN;
			pwr_y = -10; //get a bit away from baskets
			driveTilted(pwr_x, pwr_y);
			while(SensorValue[SONAR] < 200) {
				trackWall(45,50);
				driveTilted(pwr_x, pwr_y);
			}
			servo[SV_AUTO] = 0;
			driveStop();

			if (debug)
				wait1Msec(1000);

			//get away from baskets
			if (ramp_direction == 1)
				goForward(100);
			else
				goLeft(100);
			wait1Msec(1000);
			driveStop();

			if (debug)
				wait1Msec(1000);

			//get in front of ramp
			goForwardLeft(100);
			wait1Msec(1000);
			driveStop();

			if (debug)
				wait1Msec(1000);

			//rotate to face ramp
			rotate(-100);
			wait1Msec(350);
			driveStop();

			if (debug)
				wait1Msec(1000);

			//get on ramp
			if (ramp_direction == 1)
				goLeft(100);
			else
				goRight(100);
			wait1Msec(2000);
			driveStop();

		}
	}
}