void home() {
if (current > 1) {
sendSpeed(0.0, 0.0);
return;
}
// If leaving, go forward and turn
if (direction == 1) {
straightBlind(5.5 * 12);
int chk = turn90('R');
//printf("Turn returned %d\n", chk);
currentRoom = -1;
current++;
return;
}
// If returning, turn and move forward
if (direction == -1) {
int chk = turn90('L');
straightBlind(4.5 * 12);
currentRoom = 0;
current = 0;
return;
}
}
task main()
{
while(true)
{
if(nNxtButtonPressed == 3)
{
wait1Msec(1000);
turn90(-90);
wait1Msec(100);
turn90(-90);
wait1Msec(100);
turn90(-90);
wait1Msec(100);
turn90(-90);
}
if(nNxtButtonPressed == 1)
encoderTicksPerDegree--;
if(nNxtButtonPressed == 2)
encoderTicksPerDegree++;
}
}
/* main function */
int main() {
int i;
/* register_proximity_callback(3, LIMIAR, right); */
/* register_proximity_callback(4, LIMIAR, left); */
set_motors_speed(SPEED,SPEED);
/* add_alarm(stop, 10); */
while(1) {
for(i = 0; i < 1000000; i++)
turn90();
}
return 0;
}
task main(){
#else
void competitionBotRCmain(){
#endif
static int B7UReleased = true;
while(true){
setMotorsToRC();
if(vexRT[Btn7U]){
B7UReleased = true;
QePidLoopContents(1000,false);//ridulously high number
}else if (B7UReleased) {
B7UReleased = false;
resetWheelQEs();
}
if(vexRT[Btn7R]){
turn90(RIGHT,true);
}
if(vexRT[Btn7L]){
turn90(LEFT,true);
}
}
}
bool doTask(int task) {
switch (task) {
case GO_FORWARD:
return goForward(false);
case GO_BACKWARD:
return goBackward();
case CALC_NEXT_LINE:
return calcNextLine();
case GO_FORWARD_AND_COUNT:
return goForwardAndCount();
case TURN_LEFT:
return turn90(LEFT);
case TURN_RIGHT:
return turn90(RIGHT);
case TURN_90:
// turn this based on the side
return true;
case TURN_180:
return turn180();
case TO_VERTICAL:
return craneToVer();
case TO_HORIZONTAL:
return craneToHor();
case SLIDE_OUT:
return slideOut();
case SLIDE_IN:
return slideIn();
case OPEN_GRIPPER:
return openGripper();
case CLOSE_GRIPPER:
return closeGripper();
case SWITCH_SIDE:
return switchSide();
default:
return true;
}
}
int
main () {
FILE *fin = fopen ("transform.in", "r");
FILE *fout = fopen ("transform.out", "w");
int num;
int equal[8]={0,0,0,0,0,0,0,1};
fscanf (fin, "%d\n", &num);
char square1[11][11];
char square2[11][11];
int p = 0 ;
while (p < num){
int q = 0;
while (q < num){
char c;
fscanf(fin,"%c",&c);
square1[p][q] = c;
q++;
}
fscanf(fin,"\n");
p++;
}
p = 0;
while (p < num){
int q = 0;
while (q < num){
char c;
fscanf(fin,"%c",&c);
square2[p][q] = c;
q++;
}
fscanf(fin,"\n");
p++;
}
if(cmp(square1,square2,num) == 1)
equal[6] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[1] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[2] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[3] = 1;
turn90(square1,num);
reflect(square1,num);
if(cmp(square1,square2,num) == 1)
equal[4] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[5] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[5] = 1;
turn90(square1,num);
if(cmp(square1,square2,num) == 1)
equal[5] = 1;
int type = 1;
while(type < 9){
if(equal[type] == 1)
break;
type++;
}
fprintf (fout, "%d\n", type);
return 0;
}
void LevineLobby() {
if (direction == 1) {
// Go straight until wall are back
currentRoom = -1;
straightBlind(10*12);
turn90('L');
// Check elevators
currentRoom++;
if (current == destination && destinationRoom == currentRoom) return;
straightBlind(2.5*12);
correctAngle('R');
straightBlind(7*12);
turn90('R');
straightBlind(5*12);
correctAngle('R');
straightBlind(5*12);
correctAngle('R');
// Check for Levine Lobby
currentRoom++;
if (current == destination && destinationRoom == currentRoom) return;
/*
double currWall = 0;
int lostR = 0;
int lostL = 0;
double wallL, wallR;
// One-wall off the right wall at current distance until wall Lost
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
printf("Curr wall dist: %lf", currWall);
while (lostR == 0) {
printf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
straightBlind(6*12);
turn90('R');
straightBlind(7*12);
correctAngle('R');
// Set up position tracking
int lCount, rCount;
pollSensors(&lCount, &rCount);
position = 0.0;
lostL = lostR = 0;
// One-wall off the right wall at current distance for x feet
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
printf("Curr wall dist: %lf", currWall);
while (position < 12 * 12) {
printf("Waiting to go 12ft.\n");
lCount = rCount = 0;
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
correctAngle('R');
*/
currentRoom = -1;
current++;
return;
}
if (direction == -1) {
correctAngle('L');
double currWall = 0;
int lostR = 0;
int lostL = 0;
double wallL, wallR;
// One-wall off the left wall at current distance until wall Lost
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
char string[50];
sprintf(string, "Curr wall dist: %lf", currWall);
Xprintf(string);
while (lostR == 0) {
Xprintf("Waiting to lose left wall.\n");
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
straightBlind(5*12);
turn90('L');
correctAngle('R');
urgStart(&urg);
// One-wall off the right wall at current distance until wall Lost
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
char string2[50];
sprintf(string2, "Curr wall dist: %lf", currWall);
Xprintf(string2);
while (lostR == 0) {
Xprintf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
straightBlind(3.5*12);
turn90('R');
urgStart(&urg);
while (lostL || fabs(wallL) > (5*feet2mm)) {
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) sendSpeed(100.0, 100.0);
else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
straightBlind(12);
correctAngle('L');
current--;
currentRoom = -1;
return;
}
}
void LevineHallway() {
if (direction == 1) {
currentRoom = -1;
correctAngle('R');
straightBlind(3*12);
currentRoom++;
// Stop for Recycling Center
if (current == destination && currentRoom == destinationRoom) return;
moveToCenter(0.0);
correctAngle('L');
straightBlind(8*12);
moveToCenter(0.0);
correctAngle('B');
// Keep going until left wall too far
int lostL, lostR;
lostL = 0;
double wallL, wallR;
urgStart(&urg);
while (lostL != 1) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until left lost\n");
}
urgStop(&urg);
current++;
currentRoom = -1;
return;
}
if (direction == -1) {
moveToCenter(0.0);
correctAngle('B');
// Keep going until hall width < 7ft
int lostL, lostR;
lostL = 0;
double wallL = 10000;
double wallR = 10000;
urgStart(&urg);
while (fabs(wallL) + fabs(wallR) > (7 * feet2mm)) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until hallway narrows to 7ft\n");
}
urgStop(&urg);
moveToCenter(0.0);
correctAngle('B');
// Go until right wall breaks
urgStart(&urg);
while (lostR != 1) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until right wall breaks\n");
}
urgStop(&urg);
straightBlind(20);
turn90('R');
// Go straight until walls are back
lostL = lostR = 1;
urgStart(&urg);
while (!(!lostL && !lostR)) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until right lost\n");
}
urgStop(&urg);
sendSpeed(0.0, 0.0);
current--;
currentRoom = -1;
return;
}
}
void MooreLobby() {
if (direction == 1) {
// Go forward for 3 feet
straightBlind(36.0);
// Correct Angle
correctAngle('B');
// Straight Blind for 3 feet, check SIG Center
straightBlind(36);
if (destination == 1 && destinationRoom == 0) {
currentRoom = 0;
return;
}
correctAngle('R');
// One-wall off the right wall at current distance until wall Lost
int lostL, lostR;
double wallL, wallR;
double currWall = 0;
urgStart(&urg);
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0);
lostR = 0;
while (!lostR) {
Xprintf("Waiting to lose Right wall.\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Go blind straight 7 feet
straightBlind(7*12.0);
// Correct angle between the white pillars
correctAngle('B');
// Go blind straight 5.5 feet
straightBlind(5.5*12.0);
// Turn left
turn90('L');
// Check Moore 100
if (destination == 1 && destinationRoom == 1) {
currentRoom = 1;
return;
}
// Go straight until right wall comes within range
wallR = 100000;
lostR = 1;
urgStart(&urg);
while(lostR || fabs(wallR) > (34* feet2mm)) {
Xprintf("Going straight until right wall closer than 3ft . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
// Straight to view right wall better
straightBlind(12);
// Align against right wall
correctAngle('R');
// Go straight until walls are less than 6 feet apart
wallL = wallR = 100000;
lostL = lostR = 1;
currWall = 0;
urgStart(&urg);
scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0.0);
while (((fabs(wallL) + fabs(wallR)) > (6 * feet2mm)) || lostL) {
Xprintf("Searching for both walls to be less than 6ft apart . . .\n");
scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
currentRoom = -1;
current++; // Enter next feature
return;
}
if (direction == -1) {
// Correct angle against left wall
correctAngle('L');
// One wall along the left wall until it breaks
int lostL, lostR;
double wallL, wallR, currWall;
lostL = 0;
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
while(!lostL) {
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Turn Right
turn90('R');
// Go straight between the pillars
straightBlind(5*12);
// Correct angle
correctAngle('B');
// Straight blind into the opening in the left wall
straightBlind(10*12);
// Keep going straight blind until the left wall returns
wallL = 100000;
lostL = 1;
urgStart(&urg);
while(lostL) {
Xprintf("Going straight until left wall returns . . .\n");
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) {
sendSpeed(100.0, 100.0);
} else sendSpeed(0.0, 0.0);
}
urgStop(&urg);
// Correct angle
correctAngle('L');
// Keep going at current distance until Detkin lab door wall breaks
urgStart(&urg);
scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0);
lostL = 0;
while(!lostL) {
scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1);
}
urgStop(&urg);
// Straight
straightBlind(12);
current--;
currentRoom = -1;
return;
}
}
void GRWHallway() {
if (direction == 1) {
// Set up distance counting
int lCount, rCount;
int lostL = 0;
int lostR = 0;
double wallL, wallR;
pollSensors(&lCount, &rCount);
position = 0.0;
lostL = lostR = 0;
currentRoom = -1;
int roomCount = 0;
// Move to center
moveToCenter(0.0);
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
correctAngle('B');
pollSensors(&lCount, &rCount);
urgStart(&urg);
// Keep going until both walls are lost or room found
while (!lostL || !lostR) {
if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1) != -10) {
lCount = rCount = 0;
pollSensors(&lCount, &rCount);
position = position + ((lCount + rCount) / 2.0) * ticks2inches;
char string[70];
sprintf(string, "Going Straight. Pos: %lf\n", position);
Xprintf(string);
}
if (position > array[current].roomDist[roomCount]*12) {
currentRoom++;
roomCount++;
}
if (current == destination && currentRoom == destinationRoom) return;
}
urgStop(&urg);
straightBlind(1.5 * 12);
turn90('L');
sendSpeed(0.0, 0.0);
current++;
currentRoom = -1;
return;
}
if (direction == -1) {
straightBlind(6);
correctAngle('B');
moveToCenter(0.0);
correctAngle('B');
// Keep going down center of hall until wall both disappear
int lostL, lostR;
double wallL, wallR;
lostL = lostR = 0;
wallL = wallR = 1;
urgStart(&urg);
while(!(lostR || lostL || ((wallL > 5 * feet2mm) && (wallR > 5 * feet2mm)))) {
scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1);
Xprintf("Scan centering until both walls further than 5ft or either is lost\n");
}
urgStop(&urg);
current--;
currentRoom = -1;
return;
}
}
void MappingControllerThread(void) {
int16_t backLength; // the distance to the center of a crossing
int16_t w_right, w_left, w_front, w_rear; // the widths of a crossing
// needed for collecting us sensor data for the information about edges
int8_t us_pos = 0;
uint8_t wait_period = 1;
int8_t k = 1;
map_node_info node_info; // info about the crossing that shall be saved in the map
for (;;) {
// Seg_Hex(0x8);
os_wait(250);
// description of the algorithm for collecting this data:
// for every edge each 10 ( = NUMBER_US_VALUES) values of us left and right sensor data
// shall be saved. Therefore the array us_values is filled alternately with
// left (even positions) and right (odd positions) us sensor data. If the array is full,
// every second value will be deleted and afterwards only every second measured
// us sensor data is saved in the array to keep the distance between two values constant.
// and so on.
// --> The array will always be at least half-full and contain us sensor values with
// equal distances between them.
if (k == wait_period && getWidthFront()==0) {
// collect us values for edge information
if (us_pos < 2 * NUMBER_US_VALUES) {
us_values[us_pos] = Us_Data.Left_Distance; // even value: left
us_values[us_pos + 1] = Us_Data.Right_Distance; // odd value: right
us_pos += 2;
} else {
// take every second value and increment the wait period
// the array is half-full afterwards
uint8_t n;
for (n = 0; n < NUMBER_US_VALUES; n += 2) {
us_values[n] = us_values[2 * n];
us_values[n + 1] = us_values[2 * n + 1];
}
us_pos /= 2;
wait_period = wait_period<<=1; // wait_period * 2
}
k = 1; // reset k
} else
k++;
// ---------------------
// START of crossing
if (getCrossingStarted() == 1) {
uint8_t edge_length;
// calculate length of last edge
edge_length = calculateDrivenLen(os_getTime() - getTimeEdgeOld());
// add edge to map and increase the currentNodeNumber
addEdge(currentNodeNumber, ++currentNodeNumber, edge_length);
// reset value
resetCrossingStarted();
}
// ---------------------
// END of crossing
if (getCrossingDetected() == 0)
continue;
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Crossing detected, MappingController is handling it...\r\n");
#endif
stopCrossingAnalyzer();
stopWallFollow();
// get the widths and add this information to the node info
w_left = getWidthLeft();
w_right = getWidthRight();
w_front = getWidthFront();
w_rear = getWidthRear();
node_info.w0 = w_front;
node_info.w1 = w_left;
node_info.w2 = w_rear;
node_info.w3 = w_right;
// Filter open doors etc.
// the width of a corridor must be bigger than 40cm
if (w_front < 40)
w_front = 0;
if (w_rear < 40)
w_rear = 0;
if (w_left < 40)
w_left = 0;
if (w_right < 40)
w_right = 0;
// Drive backwards to center of crossing
if (w_left != 0 && w_right != 0)
backLength = (w_right + w_left) / 4;
else
backLength = (w_right + w_left) / 2;
if (backLength == 0) // dead end
backLength = 20;
if (w_rear == 0)
backLength -= 20;
backLength *= -1; // backwards driving
#ifdef MAPPINGCONTROLLER_DEBUG
printf("Driving %dcm\r\n", backLength);
#endif
driveLen(backLength);
// Handle different crossing types
// width_front = direction the car came from
// width_rear = direction the car was heading to
// side_end: 1: coming up, 2: left, 3: straight, 4: right
// add information about the edge to the map and resume driving
if (w_left == 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(DeadEnd);
addEdgeInfo(last_edge_side, 1, NUMBER_US_VALUES, us_values);
last_edge_side = 1;
node_info.crossing_type = DeadEnd;
turn180(RIGHT, BACKWARDS);
resumeDriving(0);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear != 0) {
noteCrossing(LeftStraight);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = LeftStraight;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(RightStraight);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightStraight;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left == 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(RightOnly);
addEdgeInfo(last_edge_side, 4, NUMBER_US_VALUES, us_values);
last_edge_side = 4;
node_info.crossing_type = RightOnly;
turn90(RIGHT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right == 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftOnly);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftOnly;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear == 0) {
noteCrossing(LeftRight);
addEdgeInfo(last_edge_side, 2, NUMBER_US_VALUES, us_values);
last_edge_side = 2;
node_info.crossing_type = LeftRight;
turn90(LEFT, BACKWARDS);
resumeDriving(w_rear / 2 + 50);
} else if (w_left != 0 && w_right != 0 && w_front != 0 && w_rear != 0) {
noteCrossing(All);
addEdgeInfo(last_edge_side, 3, NUMBER_US_VALUES, us_values);
last_edge_side = 3;
node_info.crossing_type = All;
// Behavior: drive straight ahead!
resumeDriving(w_left / 2 + 50);
} else { // dunno
resumeDriving(0);
}
// add information about the current node
addNodeInfo(currentNodeNumber, node_info);
// reset values
for (k = 0; k < 2 * NUMBER_US_VALUES; k++) {
us_values[k] = 0;
}
}
}
task main()
{
//start of autonmous
int orientFactor = -1; //assume the robot is facing backward
if (SensorValue[sonar] >= 100){ //if wall is more than 1 meter behind, then the robot is facing backwards
orientFactor = 1;
}
resetEncoders();
servo[bucket] = 130;
int distance1 = 24; //the distance from the starting point to the first bucket
int distance2 = 0; //the distance from the first bucket to the turning point
int distance3 = 0; //the distance from the turning point to the bottom of the ramp
int distance4 = 0; //the distance from the bottom of the ramp to the parking point
int bucketDistance11 = 0; //the distance from the first bucket to the first bucket (This will always be 0; it's just a placeholder variable for now.)
int bucketDistance12 = 10; //the distance from the first bucket to the second bucket
int bucketDistance13 = 29; //the distance from the first bucket to the third bucket
int bucketDistance14 = 39; //the distance form the first bucket to the fourth bucket
int turnRadius = 0; //the distance each center wheel must move to complete a 90-degree turn
waitForStart();
setMovementPower(100 * orientFactor, 0); //start moving forward
while(distanceTraveled() < distance1){}
resetEncoders();
while(SensorValue[infrared] != 5){}
setMovementPower(0,0);
int targetPosition = 0;
int minDistance = 1000;
if (abs(distanceTraveled() - bucketDistance11) < minDistance){ //find the closest bucket
minDistance = abs(distanceTraveled() - bucketDistance11);
targetPosition = bucketDistance11;
}
if (abs(distanceTraveled() - bucketDistance12) < minDistance){
minDistance = abs(distanceTraveled() - bucketDistance12);
targetPosition = bucketDistance12;
}
if (abs(distanceTraveled() - bucketDistance13) < minDistance){
minDistance = abs(distanceTraveled() - bucketDistance13);
targetPosition = bucketDistance13;
}
if (abs(distanceTraveled() - bucketDistance14) < minDistance){
minDistance = abs(distanceTraveled() - bucketDistance14);
targetPosition = bucketDistance14;
}
if (targetPosition == bucketDistance11 || targetPosition == bucketDistance12) //move to the closest bucket
targetPosition = targetPosition + 2;
if (targetPosition == bucketDistance13 || targetPosition == bucketDistance14)
targetPosition = targetPosition - 2;
if (targetPosition < distanceTraveled()){
setMovementPower(-100 * orientFactor,0);
while (targetPosition < distanceTraveled()){}
setMovementPower(0,0);
}
if (targetPosition > distanceTraveled()){
setMovementPower(100 * orientFactor,0);
while (targetPosition > distanceTraveled()){}
setMovementPower(0,0);
}
motor[bucketRaise] = 100; //deposit the block
wait1Msec(1500);
motor[bucketRaise] = 0;
wait10Msec(5);
servo[bucket] = 210;
wait1Msec(1000);
servo[bucket] = 130;
wait10Msec(50);
motor[bucketRaise] = -100;
wait1Msec(750);
motor[bucketRaise] = 0;
setMovementPower(-100 * orientFactor,0); //move to end of bucket row
while (distanceTraveled() > 0){}
resetEncoders();
while (distanceTraveled() > distance2){};
setMovementPower(0,0);
turn90(-orientFactor);
setMovementPower(-100 * orientFactor, 0); //move to bottom of ramp
while (distanceTraveled() < distance3){};
setMovementPower(0,0);
turn90(-orientFactor);
setMovementPower(-100 * orientFactor,0); //park on the ramp
while (distanceTraveled() < distance4){};
setMovementPower(0,0);
}
