// Include headers from OS #include //Math Stuff #include #include #include #define PI 3.14159265358979323846 #define RADTODEG(RAD) (int)((RAD)*(180.0/PI)) #define THETATODEG(THETA) (int)((THETA*180.0)/2047.0) #define SHARP_M 14150 #define SHARP_C 10 #define gyroPort 23 #define LSB_US_PER_DEG 1048830 //VPS #define VPS_PER_FOOT 682.6666 extern volatile uint8_t robot_id; #define LRDistancePort 22 #define gateServoPort 0 #define leftMotorPort 0 #define rightMotorPort 1 #define leverMotorPort 2 #define captureMotorPort 3 int frobPos = 0; int bodyPos = 0; int captureDir = -1; int8_t gyroInitPosition = 0; uint8_t minedResources = 0; int8_t invert = 0; int16_t points[][2] = {{2047,0},{1791,443},{1280,1330},{1024,1773},{512,1773},{-512,1773},{-1024,1773},{-1280,1330},{-1791,443},{-2047,0},{-1791,-443},{-1280,-1330},{-1024,-1773},{-512,-1773},{512,-1773},{1024,-1773},{1280,-1330},{1791,-443},{443,256},{0,512},{-443,256},{-443,-256},{0,-512},{443,-256}}; char alphabet[] = {'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P','Q','R','S','T','U','V','W','X','Y','Z'}; // usetup is called during the calibration period. int usetup (void) { invert = frob_read_range(-1,1); printf("Invert: %d", invert); robot_id = 6; printf("Stabilizing \n"); pause(1000); gyro_init(gyroPort, LSB_US_PER_DEG, 500); irdist_set_calibration (SHARP_M, SHARP_C); bodyPos = gyro_get_degrees() - gyroInitPosition; copy_objects(); if(game.coords[0].x < 0){ captureDir = 1; } servo_set_pos(gateServoPort,375); return 0; } //General Functions int MIN(int x, int y){ if(x >= y){ return x; } else{ return y; } } int MAX(int x, int y){ if(x >= y){ return y; } else{ return x; } } uint16_t limit_vel(int vel){ int MAX_VEL = 255; if (vel > MAX_VEL){ return MAX_VEL; } else if (vel < -MAX_VEL){ return -MAX_VEL; } else{ return (int16_t)vel; } } int16_t limit_theta(int16_t theta){ while ((theta > 360) || (theta < -360)){ if(theta > 360){ theta -= 360; } else if(theta < -360){ theta += 360; } } return theta; } int8_t limit_sector(int8_t num){ while((num > 5) || (num < 0)){ if(num > 5){ num -= 6; } else if(num < 0){ num += 6; } } return num; } int16_t fixAngle(int16_t theta){ if(theta < 0){ return 360 + theta; } else{ return theta; } } int16_t getPoint(char letter, uint8_t pos){ int len=sizeof(alphabet)/sizeof(int); for(uint8_t i = 0; i 360){ bodyPos -= 360; } while (bodyPos < -360){ bodyPos += 360; } } void resetGyroWithTheta(){ copy_objects(); bodyPos = (int)THETATODEG(game.coords[0].theta); gyroInitPosition = gyro_get_degrees() - bodyPos; } float sensors_LRDistance(){ return irdist_read(LRDistancePort)/2.54; //Converts to inches } //Motors uint8_t motorPortList[] = {0,1}; int leftMotorVel = 0; int rightMotorVel = 0; uint8_t isRobotStopped(){ return ((leftMotorVel == 0) && (rightMotorVel == 0)); } void setDriveMotorVels(){ motor_set_vel(leftMotorPort,leftMotorVel); motor_set_vel(rightMotorPort,rightMotorVel); } void motors_stopDriveMotors(){ rightMotorVel = 0; leftMotorVel = 0; setDriveMotorVels(); } void motors_brakeDriveMotors(){ leftMotorVel = 0; rightMotorVel = 0; int len = sizeof(motorPortList)/sizeof(int); for (int i = 0; i2) && (get_time()-timer < 5000)){ if (heading < bodyPos){ if(stopped){ motors_turnRight(speed); } else{ motors_adjustRight(speed,abs(bodyPos-heading)*2); } } else{ if(stopped){ motors_turnLeft(speed); } else{ motors_adjustLeft(speed,abs(bodyPos-heading)*2); } } setBodyPos(); } motors_stopDriveMotors(); } //This will move along a certain heading //If the robot is already moving this will be a slower adjustment //To go straight - put heading in as bodyPos void motors_followHeading(int16_t speed, int16_t heading){ setBodyPos(); float error = MIN((fixAngle(heading)-fixAngle(bodyPos)), (heading-bodyPos)); float gain = 2; //**adjust this** int16_t correction = (int16_t)(error * gain); if (speed<0){ correction = -correction; } leftMotorVel = limit_vel(speed -= correction); rightMotorVel = limit_vel(speed += correction); //printf("Heading: %d bodyPos: %d Error: %f Speed: %d LMV: %d RMV %d \n",heading, bodyPos, error, speed, leftMotorVel, rightMotorVel); setDriveMotorVels(leftMotorVel, rightMotorVel); } // void motors_thetaSetHeading3(int16_t speed, int16_t heading){ // copy_objects(); // int16_t theta = THETATODEG(game.coords[0].theta); // while(abs(theta-heading)>2){ // motors_turnLeft(speed); // copy_objects(); // theta = THETATODEG(game.coords[0].theta); // } // motors_stopDriveMotors(); // } void motors_thetaSetHeading3(int16_t speed, int16_t heading){ copy_objects(); int16_t theta = THETATODEG(game.coords[0].theta); if(heading<-170 || heading > 170){ heading = fixAngle(heading); theta = fixAngle(theta); } //uint32_t timer = get_time(); while(abs(theta-heading)>2){ if(fixAngle(heading) 170){ theta = fixAngle(theta); } } motors_stopDriveMotors(); } void motors_thetaSetHeading(int16_t speed, int16_t heading){ copy_objects(); heading = fixAngle(heading); uint8_t stopped = isRobotStopped(); int16_t theta = THETATODEG(game.coords[0].theta); int timer = get_time(); while(abs(fixAngle(theta)-heading)>2 && (get_time()-timer < 5000)){ if (heading < theta){ if(stopped){ motors_turnRight(speed); } else{ motors_adjustRight(speed,abs(theta-heading)*2); } } else{ if(stopped){ motors_turnLeft(speed); } else{ motors_adjustLeft(speed,abs(theta-heading)*2); } } copy_objects(); theta = THETATODEG(game.coords[0].theta); } motors_stopDriveMotors(); } void motors_thetaFollowHeading(int16_t speed, int16_t heading){ copy_objects(); int16_t theta = THETATODEG(game.coords[0].theta); float error = MIN((fixAngle(heading)-fixAngle(theta)), (heading-theta)); float gain = 2; //**adjust this** int16_t correction = (int16_t)(error * gain); if (speed<0){ correction = -correction; } leftMotorVel = limit_vel(speed -= correction); rightMotorVel = limit_vel(speed += correction); setDriveMotorVels(leftMotorVel, rightMotorVel); } //setBodyPos() must be called before calling this void motors_gyroDriveStraight(int16_t speed, uint16_t milliseconds){ uint32_t timer = get_time(); setBodyPos(); int16_t curBodyPos = bodyPos; while(get_time() - timer < milliseconds){ motors_followHeading(speed,curBodyPos); } } void motors_thetaDriveStraight(int16_t speed, uint16_t milliseconds){ uint32_t timer = get_time(); copy_objects(); int16_t curBodyPos = THETATODEG(game.coords[0].theta); while(get_time() - timer < milliseconds){ motors_thetaFollowHeading(speed,curBodyPos); } } uint8_t motor_isStalled(uint8_t motor){ return (motor_get_current(motor) > 20); } void motors_stopOnStall(){ for (int8_t i = 0; i<2; i++){ if (motor_isStalled(i)){ motor_brake(i); } } leftMotorVel = -100; rightMotorVel = -100; setDriveMotorVels(); pause(1000); motors_stopDriveMotors(); } //Collision avoidance void simpleAvoidCollisions(int16_t speed){ if(sensors_LRDistance()<12.0){ motors_adjustLeft(speed,(12-sensors_LRDistance())*25); } } void avoidOtherRobot(int16_t speed){ copy_objects(); int16_t dy = game.coords[1].x - game.coords[0].x; int16_t dx = game.coords[1].y - game.coords[0].y; int16_t ourTheta = fixAngle(THETATODEG(game.coords[0].theta)); int16_t theirTheta = fixAngle(THETATODEG(game.coords[1].theta)); int16_t collisionTheta = fixAngle(RADTODEG(atan2(dy,dx))); if((abs(dy) < 1000 || abs(dx) < 1000)){ if(abs(limit_theta(theirTheta + 180)-collisionTheta)<5){ int16_t diff = limit_theta(theirTheta + 180)-collisionTheta; if(diff < 0){ motors_adjustRight(speed, MAX(50-diff,0)); } else{ motors_adjustLeft(speed, MAX(50-diff,0)); } } else if(abs(ourTheta-collisionTheta)<5){ int16_t diff = ourTheta-collisionTheta; if(limit_theta(theirTheta + 180) > ourTheta){ motors_adjustLeft(speed, MAX(50-diff,0)); } else{ motors_adjustRight(speed, MAX(50-diff,0)); } } } } void avoidWalls(int16_t speed){ //**not complete copy_objects(); int16_t curPos[] = {game.coords[0].x,game.coords[0].y,THETATODEG(game.coords[0].theta)}; int8_t dist = 100; if((curPos[0]getPoint('U',0) - dist) && (curPos[1]getPoint('W',1) - dist)){ if(curPos[0]>getPoint('S',0)){ //check the angle } else if(curPos[0]getPoint('T',1)){ } else if(curPos[1]getPoint('W',1))){ if((curPos[2]<180) && (curPos[2]>0)){ motors_adjustLeft(speed,abs(curPos[2]-90)*2); } else{ motors_adjustRight(speed,abs(curPos[2]-270)*2); } } } //Navigation void navigateToPoint(int16_t speed, int16_t goalX, int16_t goalY, int16_t err){ //Get current position copy_objects(); int16_t curX = game.coords[0].x; int16_t curY = game.coords[0].y; int16_t dy = goalY-curY; int16_t dx = goalX-curX; int16_t curTheta = THETATODEG(game.coords[0].theta); int16_t goalTheta = (int16_t)RADTODEG(atan2(dy,dx)); uint32_t timer = get_time(); //While the difference between goal and current position is greater than allowed err while (((abs(dx)>err) ||(abs(dy)>err)) && (get_time()-timer<3000)){ // avoidOtherRobot(speed); copy_objects(); curX = game.coords[0].x; curY = game.coords[0].y; dy = goalY-curY; dx = goalX-curX; curTheta = THETATODEG(game.coords[0].theta); goalTheta = (int16_t)RADTODEG(atan2(dy,dx)); //if goal and cur theta are in 2nd/3rd quadrant if((curTheta < -90 || curTheta > 90) && (goalTheta < -90 || goalTheta > 90)){ curTheta = fixAngle(curTheta); goalTheta = fixAngle(goalTheta); } float error = goalTheta - curTheta; float gain = 1.25; //**adjust this** int16_t correction = (int16_t)(error * gain); leftMotorVel = limit_vel(speed - correction); rightMotorVel = limit_vel(speed + correction); setDriveMotorVels(leftMotorVel, rightMotorVel); pause(100); } if(get_time() - timer >= 3000){ leftMotorVel = -100; rightMotorVel = -100; setDriveMotorVels(); pause(1000); motors_stopDriveMotors(); } } int8_t getSector(){ copy_objects(); int16_t curX = game.coords[0].x; int16_t curY = game.coords[0].y; int8_t curSector = 0; if(curX<0 && curY < getPoint('K',1)){ curSector = 4; } else if(curX<0 && curY > getPoint('B',1)){ curSector = 2; } else if(curX<0){ curSector = 3; } else if(curX >= 0 && curY < getPoint('K',1)){ curSector = 5; } else if(curX >= 0 && curY > getPoint('B',1)){ curSector = 1; } //else curSector remains 0 return curSector; } int8_t getOpponentSector(){ copy_objects(); int16_t curX = game.coords[1].x; int16_t curY = game.coords[1].y; int8_t curSector = 0; if(curX<0 && curY < getPoint('K',1)){ curSector = 4; } else if(curX<0 && curY > getPoint('B',1)){ curSector = 2; } else if(curX<0){ curSector = 3; } else if(curX >= 0 && curY < getPoint('K',1)){ curSector = 5; } else if(curX >= 0 && curY > getPoint('B',1)){ curSector = 1; } //else curSector remains 0 return curSector; } void navigateToSector(int16_t speed, int8_t sector){ int8_t curSector = getSector(); int8_t nextSector = -1; if(curSector == sector){ return; } else if (abs(limit_sector(curSector + 1) - sector) > abs(limit_sector(curSector - 1) - sector)){ nextSector = limit_sector(curSector - 1); } else{ nextSector = limit_sector(curSector + 1); } if(nextSector == 0){ //0 navigateToPoint(speed,getPoint('C',0), getPoint('A',1),300); navigateToSector(speed,sector); } if(nextSector == 1){ //1 navigateToPoint(speed,getPoint('E',0), getPoint('C',1)-200,300); navigateToSector(speed,sector); } if(nextSector == 2){ //2 navigateToPoint(speed,getPoint('F',0), getPoint('C',1)-200,300); navigateToSector(speed,sector); } if(nextSector == 3){ //3 navigateToPoint(speed,getPoint('H',0),getPoint('J',1),300); navigateToSector(speed,sector); } if(nextSector == 4){ //4 navigateToPoint(speed,getPoint('F',0), getPoint('L',1),300); navigateToSector(speed,sector); } if(nextSector == 5){ //5 navigateToPoint(speed,getPoint('E',0), getPoint('L',1),300); navigateToSector(speed,sector); } } //Interaction void captureTerritory(int8_t num){ pause(1000); leftMotorVel = -175; rightMotorVel = -175; setDriveMotorVels(); uint32_t timer = get_time(); while(get_time() - timer < 3500 && game.territories[num].owner != 6){ copy_objects(); int speed = -150; if(invert != 0){ speed = -speed; } motor_set_vel(captureMotorPort, speed*captureDir); } motors_stopDriveMotors(); motor_set_vel(captureMotorPort, 0); } void mineResources(){ leftMotorVel = 175; rightMotorVel = 175; setDriveMotorVels(); motor_set_vel(leverMotorPort,75); pause(4000); motors_stopDriveMotors(); minedResources = 4; motor_set_vel(leverMotorPort,0); } void releaseResources(){ //**Not right! Can't navigate around walls.... if(captureDir == 1){ navigateToSector(175,0); motors_stopDriveMotors(); motors_thetaSetHeading(100,00); } else{ navigateToSector(175,3); motors_stopDriveMotors(); motors_thetaSetHeading(100,180); } leftMotorVel = -175; rightMotorVel = -175; setDriveMotorVels(); servo_set_pos(gateServoPort,275); pause(750*minedResources); motors_stopDriveMotors(); servo_set_pos(gateServoPort,375); minedResources = 0; } //Exploration period void explore(int16_t speed){ uint32_t timer = get_time(); copy_objects(); if(game.coords[0].x < 0){ navigateToPoint(speed,getPoint('H',0),getPoint('J',1),300); if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('F',0), getPoint('L',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('E',0), getPoint('L',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('C',0), getPoint('A',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('E',0), getPoint('C',1)-200,300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('F',0), getPoint('C',1)-200,300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('H',0), getPoint('J',1),300); } } else{ navigateToPoint(speed,getPoint('C',0), getPoint('A',1),300); if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('E',0), getPoint('C',1)-200,300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('F',0), getPoint('C',1)-200,300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('H',0), getPoint('J',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('F',0), getPoint('L',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('E',0), getPoint('L',1),300); } if(get_time() - timer < 10000){ navigateToPoint(speed,getPoint('C',0), getPoint('A',1),300); } } } void capture(int8_t point){ copy_objects(); if(game.territories[point].owner == 6){ return; } navigateToSector(150,point); motors_stopDriveMotors(); if(getSector() != point){ return; } for(int8_t i = 5; i >= 0; i-=5){ int8_t corr = i; if(point == 4){ navigateToPoint(100, getPoint('F',0),getPoint('L',1)+200,150); motors_stopDriveMotors(); copy_objects(); int dy = 1773 - game.coords[0].y; //int dy = -1773 - game.coords[0].y; int dx = -512 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,90 + i); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } if(point == 3){ navigateToPoint(100, getPoint('H',0),getPoint('A',1),150); motors_stopDriveMotors(); copy_objects(); int dy = 1330 - game.coords[0].y; int dx = 1280 - game.coords[0].x; // int dy = -443 - game.coords[0].y; // int dx = -1791 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,30 + i); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } if(point == 2){ navigateToPoint(100, getPoint('F',0),getPoint('C',1)-200,150); motors_stopDriveMotors(); copy_objects(); int dy = 512 - game.coords[0].y; int dx = 0 - game.coords[0].x; // int dy = 1330 - game.coords[0].y; // int dx = -1280 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,-30 + i); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } if(point == 1){ navigateToPoint(100, getPoint('E',0),getPoint('C',1)-200,150); motors_stopDriveMotors(); copy_objects(); int dy = -1773 - game.coords[0].y; int dx = 512 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,-90 + i); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } if(point == 0){ navigateToPoint(100, getPoint('C',0),getPoint('A',1),150); motors_stopDriveMotors(); copy_objects(); int dy = -256 - game.coords[0].y; int dx = 443 - game.coords[0].x; // int dy = 443 - game.coords[0].y; // int dx = 1791 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,-150 + i ); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } if(point == 5){ navigateToPoint(100, getPoint('E',0),getPoint('L',1)+200,150); motors_stopDriveMotors(); copy_objects(); int dy = -512 - game.coords[0].y; int dx = 0 - game.coords[0].x; // int dy = -1330 - game.coords[0].y; // int dx = 1280 - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); //motors_setHeading(100,150 + i); motors_thetaSetHeading3(50,goalTheta+corr); if(getSector() == point){ captureTerritory(point); copy_objects(); if(game.territories[point].owner == 6){ break; } } } } } void mine(int8_t point){ copy_objects(); for(int8_t i = -5; i <= 0; i+=5){ int8_t corr = -5; if(point == 4){ navigateToPoint(100, getPoint('F',0),getPoint('L',1)+200,150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('L',1) - game.coords[0].y; int dx = getPoint('L',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,-150+i); mineResources(); } if(point == 3){ navigateToPoint(100, getPoint('H',0),getPoint('A',1),150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('I',1) - game.coords[0].y; int dx = getPoint('I',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,150+i); mineResources(); } if(point == 2){ navigateToPoint(100, getPoint('F',0),getPoint('C',1)-200,150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('F',1) - game.coords[0].y; int dx = getPoint('F',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,90+i); mineResources(); } if(point == 1){ navigateToPoint(100, getPoint('E',0),getPoint('C',1)-200,150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('C',1) - game.coords[0].y; int dx = getPoint('C',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,30+i); mineResources(); } if(point == 0){ navigateToPoint(100, getPoint('C',0),getPoint('A',1),150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('R',1) - game.coords[0].y; int dx = getPoint('R',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,-30+i); mineResources(); } if(point == 5){ navigateToPoint(100, getPoint('E',0),getPoint('L',1)+200,150); motors_stopDriveMotors(); copy_objects(); int dy = getPoint('O',1) - game.coords[0].y; int dx = getPoint('O',0) - game.coords[0].x; int16_t goalTheta = RADTODEG(atan2(dy,dx)); motors_thetaSetHeading(50,goalTheta+corr); //motors_thetaSetHeading(100,-90+i); mineResources(); } } } //Main routine int umain (void) { explore(175); while(1){ copy_objects(); for(int8_t i = 0; i<6; i++){ int8_t j = i; if(captureDir == 1){ j = limit_sector(i+3); } copy_objects(); if(game.territories[j].owner != 6 && getOpponentSector() != j){ capture(j); copy_objects(); if(game.territories[j].owner == 6){ mine(j); } } } copy_objects(); releaseResources(); } return 0; }