void koch (int len, int level)
{
int newlen;
if (0 == level) {
forwd (len);
return;
}
else {
newlen = (int) ceil (len / 3.0);
koch (newlen, level - 1);
turnleft (60);
koch (newlen, level - 1);
turnright (120);
koch (newlen, level - 1);
turnleft (60);
koch (newlen, level - 1);
}
}
static
void
approachintersection(void * unusedpointer,
unsigned long carnumber)
{
int car_direction;
int turn_direction;
/*
* Avoid unused variable and function warnings.
*/
(void) unusedpointer;
(void) carnumber;
(void) gostraight;
(void) turnleft;
(void) turnright;
/*
* cardirection is set randomly.
*/
car_direction = random() % 4;
turn_direction = random() % 3;
// we enforce the FIFO ordering for the cars approaching
// the intersection from the same direction
if (car_direction == 0) {// North
// north
P(from_north);
} else if (car_direction == 1) { //East
P(from_east);
} else if (car_direction == 2) {//South
P(from_south);
} else {
assert(car_direction == 3); //West
P(from_west);
}
// Step 1: approaching an intersection
// message(APPROACHING, carnumber, car_direction, turn_direction);
// kprintf("car: %d, from: %c, turn_direction: %d\n", carnumber, directions[car_direction], turn_direction);
// after we approached the intersection, we enter it
if (turn_direction == TURN_LEFT) { //0
turnleft(car_direction, carnumber);
} else if (turn_direction == TURN_RIGHT) { //1
turnright(car_direction, carnumber);
} else {
assert(turn_direction == GO_STRAIGHT); //2
gostraight(car_direction, carnumber);
}
V(join_sem);
}
void star_6 (int len)
{
int i;
for (i = 0; i < 6; i++) {
forwd (len);
turnright (120);
forwd (len);
turnleft (60);
}
}
/*The integer for choice is based on the index of items in the keywords array in the main() function*/
void chooseMove(int choice, int duration)
{
if (choice == 0)
forward(duration);
else if (choice == 1)
reverse(duration);
else if (choice == 2)
turnright(duration);
else if (choice == 3)
turnleft(duration);
else if (choice == 4)
pause(duration);
}
task main()
{
//power,rotations
//1200 is around 1 rotation
//1200 rotations turn is 270 degrees
forward(50,1200);
wait1Msec(2000);
turnright(50,1200);
wait1Msec(2000);
turnleft(50,1200);
wait1Msec(2000);
backward(50,3600);
}
void sq_koch (int len, int level)
{
if (0 == level) {
forwd (len);
return;
}
else {
sq_koch (len / 4, level - 1);
turnleft (90);
sq_koch (len / 4, level - 1);
turnright (90);
sq_koch (len / 4, level - 1);
turnright (90);
sq_koch (len / 4, level - 1);
sq_koch (len / 4, level - 1);
turnleft (90);
sq_koch (len / 4, level - 1);
turnleft (90);
sq_koch (len / 4, level - 1);
turnright (90);
sq_koch (len / 4, level - 1);
}
}
void getinput()
{
//hit ESC to quit
if (key[KEY_ESC]) gameover = 1;
//WASD - SPACE keys control tank 1
if (key[KEY_W]) forward(0);
if (key[KEY_D]) turnright(0);
if (key[KEY_A]) turnleft(0);
if (key[KEY_S]) backward(0);
if (key[KEY_SPACE]&& loopcnt==LOOPDELAY) {fireweapon(0);
loopcnt=0;
}
//arrow - ENTER keys control tank 2
if (key[KEY_UP]) forward(1);
if (key[KEY_RIGHT]) turnright(1);
if (key[KEY_DOWN]) backward(1);
if (key[KEY_LEFT]) turnleft(1);
if (key[KEY_ENTER]&& loopcnt==LOOPDELAY) {fireweapon(1);
loopcnt=0;
}
//short delay after keypress
rest(20);
}
void _3DTree::rule1(int i)
{
if (i == 0)
drawline();
else
{
rule1(i - 1);
glPushMatrix();
turnright();
pitchdown();
rollleft();
rule1(i - 1);
glPopMatrix();
glPushMatrix();
rollleft();
turnleft();
turnleft();
pitchdown();
rule1(i - 1);
glPopMatrix();
turnaround();
turnaround();
rule1(i - 1);
glPushMatrix();
turnright();
turnright();
pitchdown();
rollright();
rule1(i - 1);
glPopMatrix();
glPushMatrix();
turnleft();
pitchdown();
rule1(i - 1);
glPopMatrix();
}
}
void hilbert_left (int len, int level)
{
int bearing;
if (level > 0) {
bearing = heading ();
turnleft (90);
hilbert_right (len, level - 1);
setheading (bearing);
forwd (len);
bearing = heading ();
hilbert_left (len, level - 1);
setheading (bearing);
turnleft (90);
forwd (len);
bearing = heading ();
turnright (90);
hilbert_left (len, level - 1);
setheading (bearing);
turnleft (90);
forwd (len);
bearing = heading ();
turnleft (90);
hilbert_right (len, level - 1);
setheading (bearing);
}
} /* hilbert_left () */
void Simple_Tree::rule1(int i)
{
if (i == 0)
drawline();
else
{
rule2(i - 1);
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
turnright();
rule1(i - 1);
}
}
float Walk::turnleft(float theta)
{
if(theta==0)
{
legRotfi=0;
supLegRotfi=0;
dribble();
return 0;
}
if(theta>15)
legRotfi=((int)leg)*15;
else
legRotfi=((int)leg)*theta;
supLegRotfi=0;
dribble();
theta=theta-legRotfi;
turnleft(theta);
}
static
void
approachintersection(void * unusedpointer,
unsigned long carnumber)
{
int cardirection;
/*
* Avoid unused variable and function warnings.
*/
(void) unusedpointer;
(void) carnumber;
(void) gostraight;
(void) turnleft;
(void) turnright;
/*
* cardirection is set randomly.
*/
//cardirection = 0 ---> from North
//cardirection = 1 ---> from East
//cardirection = 2 ---> from South
//cardirection = 3 ---> from West
cardirection = random() % 4;
//move = 0 ---> go straight
//move = 1 ---> go right
//move = 2 ---> go left
int move = random() % 3;
//lock the intersections if 3 cars are already inside
P(Slock);
if(move == 0)
gostraight(cardirection, carnumber);
else if(move == 1)
turnright(cardirection, carnumber);
else if(move == 2)
turnleft(cardirection, carnumber);
V(Slock);
}
void drawScene()
{
GLfloat x,y,z;
glColor3f(1.0f,1.0f,1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
keySpecialOperations();/*
if(keyStates['w'])
{
fprintf(stdout,"* cameraz=%f\nreferencez=%f\n",cameraz,referencez);
}*/
//gluLookAt(-5*TRACK_WIDTH-13.5,12,18,-TRACK_WIDTH-13.5,2,-1,0,1,0);
gluLookAt(camerax,cameray,cameraz,referencex,referencey,referencez,upx,upy,upz);
// tack draw
trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),-(6*TorusRadius),0);
trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),0,3*CAR_LENGTH);
bumpdraw(-(TRACK_WIDTH/2.0f)-1.5,CAR_LENGTH,1);
BumpyTrack(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),3*CAR_LENGTH,6*CAR_LENGTH,(TorusRadius/2.0f));
trackStraight(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),6*CAR_LENGTH,9*CAR_LENGTH);
bumpdraw((TRACK_WIDTH/2.0f),6*CAR_LENGTH,0);
for(GLint i=0;i<=3*GRASS_WIDTH;i+=GRASS_WIDTH)
{
grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,-(6*TorusRadius),0);
grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,1,3*CAR_LENGTH);
grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,3*CAR_LENGTH+1,6*CAR_LENGTH);
grass(-(TRACK_WIDTH/2.0f)-i,(TRACK_WIDTH/2.0f)-i,6*CAR_LENGTH+1,9*CAR_LENGTH-1);
}
turnleft(-(TRACK_WIDTH/2.0f)-1,6*CAR_LENGTH,1);
trackTurnLeft(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),9*CAR_LENGTH,13*CAR_LENGTH,0,180.0f);
trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,6*CAR_LENGTH,9*CAR_LENGTH);
BumpyTrack(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,3*CAR_LENGTH,6*CAR_LENGTH,(TorusRadius/2.0f));
trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,0,3*CAR_LENGTH);
trackStraight(-5*TRACK_WIDTH-2.25,-4*TRACK_WIDTH-2.25,-(6*TorusRadius),0);
trackTurnLeft(-(TRACK_WIDTH/2.0f),(TRACK_WIDTH/2.0f),-1.2*CAR_LENGTH,2.8*CAR_LENGTH,180.0f,355.5f);
//trackTurnRight(-(int)(TRACK_WIDTH/2.0f)-1,(int)(TRACK_WIDTH/2.0f),8*CAR_LENGTH,12*CAR_LENGTH,180.0f);
glTranslatef(0.0f,TorusRadius-(TorusRadius/2.75),0.0f);
glScalef(0.5f,0.5f,0.5f);
assembly();
glScalef(2.0f,2.0f,2.0f);
glTranslatef(0.0f,-TorusRadius+(TorusRadius/2.75),0.0f);
glFlush();
glPopMatrix();
}
void _3DTree3::rule1(int i)
{
if (i == 0)
drawellipse(2, 0,2,1);
else
{
glScaled(.6, .6, .6);
rule2(i - 1);
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
rollleft();
rollleft();
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
rollleft();
rollleft();
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
rollleft();
rollleft();
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
rollleft();
rollleft();
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
rollleft();
rollleft();
glPushMatrix();
turnleft();
rule1(i - 1);
glPopMatrix();
}
}
void turnleft(void)
{
motorALL_ON();
motor2_Forward();
motor1_OFF();
if ((checkbit(PINF,6)!=0 && checkbit(PINF,7)!=0 && checkbit(PINF,5)!=0)||(checkbit(PINF,6)!=0 && checkbit(PINF,7)==0 && checkbit(PINF,5)!=0)) //THE CROSSOVER DETECTED
{
j++;
/*lcd_gotoxy2(5);
lcd_showvalue(j);*/
PORTC=~j;
_delay_ms(500);
}
//PORTC=0x02;
if((checkbit(PINF,6)==0) && (checkbit(PINF,7)==0) && (checkbit(PINF,5)==0)||(checkbit(PINF,6)!=0) && (checkbit(PINF,7)!=0) && (checkbit(PINF,5)!=0))
turnleft();
}
task main()
{
initializeRobot();
ereset();
waitForStart();
ch = 1;
brake();
wait1Msec(300);
{
motor[motorH] = 50;
motor[motorI] = 50;
wait1Msec(300);
motor[motorH] = 0;
motor[motorI] = 0;
movebackward();
wait1Msec(2000);
servo[servo3] = 247;
brake(); //temporary stop
wait1Msec(300);
turnleft(); //turn left to face the center goal structure
wait1Msec(1100);
brake(); //temporary stop
wait1Msec(100);
turnright();
wait1Msec(1100);
brake();
wait1Msec(100);
movebackward();// move forward to the center goal structure
wait1Msec(2525);
brake();
wait1Msec(1000);
if (SensorRaw(irs) > 3 && SensorRaw(irs) < 7)//checks for the ir value to know if the center goal structure is at postion 1
{ //center goal structure is in the 1
moveforward(); // move forward to be adjacent to the kickstand
wait1Msec(650);
brake(); //temporary stop
wait1Msec(50);
turnright(); //turn right to face the kickstand
wait1Msec(1550);
brake(); //temporary stop
wait1Msec(100);
moveforward(); //moveforward to knock down the kickstand and to be adjacent to the ir beacon and center goal
wait1Msec(1250);
brake();
wait10Msec(3000);
motor[motorH] = -50;//resets the initialize
motor[motorI] = -50;
wait1Msec(300);
motor[motorH] = 0;//stops the 80/20 so that it does not move
motor[motorI] = 0;
}
else if(SensorRaw(irs) <= 3 || SensorRaw(irs) >= 7)//if the ir sensor senses a value between 3 and 7
{ //center goal is in position 2/3
movebackward();//move the robot back to be adjacent to the ir beacon
wait1Msec(900);
turnleft();//turns left to check for position 2.
wait1Msec(900);
movebackward();//moves abckwards to be next to the ir beacon
wait1Msec(1250);
ch = 0;//gives the integer chicks the value of of 0
}
if (SensorRaw(irs) >= 7 && SensorRaw(irs) <= 8)//if ir value is between 7 and 8
{ //center goal structure is in the 3 position
while(SensorRaw(irs) < 5)//if it senses a value less than 5 until it senses at 5
{
movebackward();
}
moveforward();//moves the robot forward to be adjacent to the
wait1Msec(500);
turnright();//turn right to face the kickstand
wait1Msec(900);
moveforward();//knocks down the kickstand
wait1Msec(1000);
}
else
{ //center goal structure is in the 2 position
moveforward();//move forward to be adjacent to the kickstand
wait1Msec(1259);
turnright();//turn right to face the kickstand
wait1Msec(1600);
moveforward();//move forward to knock down the kickstand
wait1Msec(1500);
}
}
}
void decidemove(player& pl)
{
if (pl.wrongsteps <= 3 &&
pl.steps <= 3)
{
return;
}
int r = rand()%30;
switch(r)
{
case 0:
if (isrightempty(pl))
{
turnright(pl);
pl.steps = 0;
}
else if (isleftempty(pl))
{
turnleft(pl);
pl.steps = 0;
}
break;
case 1:
if (isleftempty(pl))
{
turnleft(pl);
pl.steps = 0;
}
else if (isrightempty(pl))
{
turnright(pl);
pl.steps = 0;
}
break;
}
if (isaheadempty(pl))
{
return;
}
else if (r%2 == 0)
{
if (isleftempty(pl))
{
turnleft(pl);
pl.steps = 0;
}
else if (isrightempty(pl))
{
turnright(pl);
pl.steps = 0;
}
}
else
{
if (isrightempty(pl))
{
turnright(pl);
pl.steps = 0;
}
else if (isleftempty(pl))
{
turnleft(pl);
pl.steps = 0;
}
}
if (r==0)
{
turnleft(pl);
pl.steps = 0;
}
else if (r==1)
{
turnright(pl);
pl.steps = 0;
}
}
void main()
{
DDRA=0XFF;
DDRC=0xFF;
//PORTC=0XFF; //led
// PORT F Initialization
DDRF=0x00;
PORTF=0xFF; //SENSOR
DDRB=0XF0;
DDRE=0x0F; //motor enable
int i=PINF;
while(1)
{
if(checkbit(i,4)==0)
{
PORTA=0X00;
if(( checkbit(i,6)==0 && checkbit(i,7)!=0 && checkbit(i,5)!=0) || (checkbit(i,6)==0 && checkbit(i,7)==0 && checkbit(i,5)!=0))
turnright();
else if(( checkbit(i,6)!=0 && checkbit(i,7)!=0 && checkbit(i,5)==0) || (checkbit(i,6)!=0 && checkbit(i,7)==0 && checkbit(i,5)==0))
turnleft();
else if(checkbit(i,5)==0 && checkbit(i,7)!=0 && checkbit(i,6)==0)
{
motorALL_ON();
motor2_Forward();
motor1_Forward();
//PORTC=0x00;
if ((checkbit(i,6)!=0 && checkbit(i,7)!=0 && checkbit(i,5)!=0)||(checkbit(i,6)!=0 && checkbit(i,7)==0 && checkbit(i,5)!=0)) //THE CROSSOVER DETECTED
{
motorALL_ON();
motor2_Forward();
motor1_Forward();
j++;
/*lcd_gotoxy2(5);
lcd_showvalue(j);*/
PORTC=~j;
_delay_ms(500);
}
}
}
else
{
PORTA=0X0A;
//_delay_ms(500);
//PORTA=0X00;
motor2_OFF();
motor1_OFF();
motorALL_OFF();
}
}
}
static INLINE void tracepoint (int xp, int yp, int dir, unsigned int color, int xstart, int xend, int ystart, int yend)
{
unsigned char *calc;
cpixeldata_t mycolor;
int i, lookdir;
unsigned int c;
int x, y;
int periodicity = (dir & 8) != 0;
dir &= ~8;
calc = calculated + xp + yp * CALCWIDTH;
if (!(*calc & (CALCULATED | CALCULATING))) {
*calc |= CALCULATING;
mycolor = (cpixeldata_t) calculatepixel (xp, yp, periodicity);
putpixel (xp, yp, mycolor);
*calc |= CALCULATED;
*calc &= ~CALCULATING;
} else {
if (*calc & CALCULATING) {
/*Bad luck..some other procesor is working with out pixel :) try
*later.*/
addstack (xp, yp, dir, color, periodicity);
return;
}
mycolor = getpixel (xp, yp);
}
while (1) {
periodicity = (mycolor == inset || color == inset);
lookdir = turnright (dir);
for (i = 0; i < 3; i++) {
x = xp + dirrections[lookdir][0];
y = yp + dirrections[lookdir][1];
if (x >= xstart && x <= xend && y >= ystart && y <= yend) {
calc = calculated + x + y * CALCWIDTH;
if (!(*calc & (CALCULATED | CALCULATING))) {
*calc |= CALCULATING;
c = calculatepixel (x, y, periodicity);
putpixel (x, y, c);
*calc |= CALCULATED;
*calc &= ~CALCULATING;
} else {
if (*calc & CALCULATING) {
/*Bad luck..some other procesor is working with out pixel :) try
*later.*/
addstack (xp, yp, dir, color, periodicity);
return;
}
c = getpixel (x, y);
}
if (c == mycolor)
break;
if (c != color) {
int dir2 = turnright (lookdir);
int mask = (1 << dir2) + (1 << turnright (dir2));
if (!(*calc & mask)) {
addstack (x, y, dir2, mycolor, periodicity);
}
color = c;
}
}
lookdir = turnleft (lookdir);
}
x = xp + dirrections[lookdir][0];
y = yp + dirrections[lookdir][1];
if (x >= xstart && x <= xend && y >= ystart && y <= yend) {
calc = calculated + x + y * CALCWIDTH;
if (!(*calc & (1 << lookdir))) {
*calc |= (1 << lookdir);
if (size < 10) {
addstack (x, y, lookdir, color, periodicity);
return;
} else {
xp = x;
yp = y;
dir = lookdir;
calc = calculated + xp + yp * CALCWIDTH;
}
} else
return;
} else
return;
}
}
//returns true for limit pass, pass or put, false for movement or
// .. inp: a,b,c,d,e,f,g :
// a: forward, b: back, c: left, d: right,
// e: put, f: pass, g: pause
bool GTPWrapper::doThis(bool first, vector<double> inp)
{
// cout << "dothis inp: " << printvector(inp);
//check for limits if within inc counter..
if(first){
if(fcount>=thinksteps){
setPass(first);
return true;
}else
fcount++;
}
if(!first){
if(scount>=thinksteps){
setPass(first);
return true;
}else
scount++;
}
//needs conflict resolution
//first check for movement(default in kens code)
//default in con flicts; back & forward => forward, left & righ ->left
bool forward,back,west,east;
forward=back=west=east=false;
bool moving = false;
int *heading = getHeading(first);
if(inp.at(0)>0.5){//forward..
moving = true;
if(*heading == 0)
forward = true;
else if(*heading == 1)
east = true;
else if(*heading == 2)
back = true;
else if(*heading == 3)
west = true;
}
bool turned = false;
if(inp.at(1)>0.5){//turn left
turnleft(first);
turned = true;
}else if(inp.at(2)>0.5){//right..
turnright(first);
turned = true;
}
if(!moving&&!turned){
int ind = 5;
int n = 5;
// if(headingsupport)
// n++;
double max=0.5;
for(int i=3;i<n;i++){
if(inp.at(i)>max){
ind = i;
max = inp.at(i);
}
}
if(ind==5){//default
// cout << "doing nothing.." << endl;
return false;
}
if(ind==3){
// if(!justput)
//cout << "putting " << getColoredCoord(first) << endl;
// if(justput)
// secondput = true;
// justput = true;
// if(justmoved && secondput)
// cout << "!!!! second put " << getColoredCoord(first) << endl;
if(put(first)){//regardless of illegal move?? heh
moves++;
// if(moves>4)
// cout<<"four moves!!!: " << endl << getLocalBoardAscii();
unsetPass(first);
unsetCount(first);
}else{
setPass(first);
unsetCount(first);
}
return true;
}else if(ind==4){
// cout << "passing" << endl;
setPass(first);
unsetCount(first);
return true;
}
// else if(headingsupport&&ind==7){
// // cout << "turning around!!" << endl;
// unsetPass(first);
// return false;
// }
}else{
// if(!justmoved)
//cout << "fpass:"******" spass:"******" moving(fc:"<<fcount<<" sc:"<<scount<<")" << "forward: " << forward << " back: "<<back<<" west: "<<west<<" east:" << east<< endl;
unsetPass(first);
// justmoved = true;
// unsetCount(first);
if(forward)
up(first);
if(back)
down(first);
if(west)
left(first);
if(east)
right(first);
return false;
}
return true;
}
void main(void)
{
int front, left, right;
int totalerror = 0;
// Set inputs and outputs
TRISA = 0x00; // left motor
TRISB = 0b11111111; // Analog inputs (set all as input)
TRISC = 0x00; // right motor
TRISD = 0x00;
//TRISE = 0x00;
PORTD = 0b00000001;
front = FetchSensor(FRONTSENSOR);
FetchAllSensors(&left, &front, &right);
/* Wait for sign to start */
while(1)
{
if(front > 300)
{
Delay10KTCYx(1000);
break;
}
front = FetchSensor(FRONTSENSOR);
}
/* Main Program loop */
while(1)
{
FetchAllSensors(&left, &front, &right);
/* if the front is clear and sides are blocked */
if( (front <= 600) && (right > 100) && (left > 100) ){
/* Just keep tracking forward */
totalerror+=track(20);
correct(&totalerror);
}
/* If at a dead end */
if( (front > 400) && (right > 100) && (left > 100) )
{
/* Turn Around */
track(40);
turnright(150);
}
/* If there is an opening on the left */
if( (front < 700) && (right > 90) && (left < 90) )
{
int i;
Delay10KTCYx(100);
right = FetchSensor(RIGHTSENSOR);
/* Track off right wall for a bit */
for(i = 0; i < 100 ; i ++){
totalerror=trackleft(1, right-25);
correct(&totalerror);
}
/* NO Tracking */
while(front < 700)
{
front = FetchSensor(FRONTSENSOR);
forward(1,1,1);
}
turnleft(80);
}
/* If there is an opening on the right */
if( (front < 700) && (right < 150) && (left > 150) )
{
int i;
left = FetchSensor(LEFTSENSOR);
for(i = 0; i< 50 ; i++){
totalerror=trackleft(1, left);
correct(&totalerror);
}
while(front < 700)
{
front = FetchSensor(FRONTSENSOR);
forward(1,1,1);
}
turnright(80);
}
FetchAllSensors(&left, &front, &right);
}
}
