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