Beispiel #1
0
void Motor_setLeft(int percentage){
SetSpeedLeft(percentage);
}
Beispiel #2
0
void maze(void)
{
	long i;
//long j;
	InitMotors();
	InitProx();
	LedClear();
	SetLed(4,1);
	SetStepsRight(1);
	SetStepsLeft(1);
	int left = 0;
	int right = 0;
//	int waitlonger = 0;
    int curious = 0;
	int distance;
	while(1){
		while (curious == 0) {
			int j;
			for(i=0;i<100000;i++) {asm("nop");}

			left = 300;
			right = 300;

			/*wait*/for(i=0;i<40000;i++) {asm("nop");}
			SetSpeedRight(right);
			SetSpeedLeft(left);

			if (GetProx(0) >= 600 || GetProx(7) >= 600){
				left= -300;
				right= 300;
				SetSpeedRight(right);
				SetSpeedLeft(left);
				for(i=0;i<20000;i++) {asm("nop");}
				curious = 1;
			}
			
			if( GetProx(2) >= 500 ){
				for(j=0; j<= 10000; j++){
					curious = 1;
				}
			}
		}

		while(curious==1){
			//too far from wall
			if(GetProx(2) < 400){
				//turn right, towards wall
				left = 300; 
				right = 100; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");}
			}
			//wall in front and on the right
			while (GetProx(0) >= 500 || GetProx(7) >= 600 || GetProx(1) >= 350){
				//sharp turn left
				left = -300; 
				right = 300; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 
			//too close to wall
			if (GetProx(2) > 600){ 
				//turn left
				left = 0; 
				right = 300; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 	
			//enough for it to go straight forwards
			if ( GetProx(2) >= 400 && GetProx(0) < 500 && GetProx(7) < 600 && GetProx(1) < 350 && GetProx(6) <= 800 && GetProx(2) <= 600){ 
				//go forwards
				left = 300; 
				right = 300; 
				left = left * 0.9; 
				right = right * 0.9; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 
			//
			if (GetProx(5) > 600 || GetProx(6) > 600 ){ 
				left = 300; 
				right = 0; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			}
			//goal detection
			if (GetAmbientLight(0)<4000 || GetAmbientLight(7)<4000) {
				SetSpeedLeft(0);
				SetSpeedRight(0);
				int j;
				for (j=0;j<5;j++) {
					for (i=0;i<7;i++)
						SetLed(i,1);
					/*wait*/for(i=0;i<80000;i++) {asm("nop");}
					LedClear();
					/*wait*/for(i=0;i<80000;i++) {asm("nop");}
				}
				
				while (1);
			}
		}
	}		
}
Beispiel #3
0
void Motor_stop(){
	SetSpeedRight(1);
	SetSpeedLeft(1);
}
Beispiel #4
0
void aggressive(void)
{
	long i = 0;			//loop counter, mainly for waiting
	int left = 0;		//speed of left wheel
	int right = 0;		//speed of right wheel
	int waitlonger = 0;	//if set to 1, will wait a bit longer in between each loop
	
	//setup
	InitMotors();
	InitProx();
	LedClear();
	//start with body light on and moving forwards
	SetBodyLed(1);
	SetStepsRight(1);
	SetStepsLeft(1);
	SetSpeedRight(100);
	SetSpeedLeft(100);
	
	while (1) {
		//smooth
		left = left * 0.9;
		right = right * 0.9;
		left+=38;
		right+=38;

		/*light up led if something is near it in that direction*/
		/*and also adjusts wheel speed depending on which ones are lit up*/
		for (i=0;i<7;i++) {
			//~1000 is about to touch
			//any less than 500 and it starts having false positives
			if (GetProx(i)>=500 || GetAmbientLight(i)<4000) {	
				switch (i) {
					//left side sensors
					case 0:left+=150;right+=100;break;
					case 1:left+=100;right-=100;break;
					case 2:left+=300;right-=400;break;
					//back sensors
					case 3:left+=400;right-=300;break;
					//case 4:left-=300;right+=400;break;
					case 4:left+=400;right-=300;break;//testing left turn on both
					//right side sensors
					case 5:left-=400;right+=300;break;
					case 6:left-=100;right+=100;break;
					case 7:left+=150;right+=100;break;
				}
				SetLed(i,1);
			}
			else SetLed(i,0);
		}

		/*
		//back sensors
		if (GetProx(3)>800 || GetProx(4)>800) {
			//spin around in panic
			left = 700;
			right = -800;
			waitlonger = 1;
			
		*/
		
		//front sensors
		//todo: blindly charges straight forwards, is that what we want?
		//		if not, then adjust slightly based on sensors
		if (GetProx(0)>800 || GetProx(7)>800) {
			//light up
			for (i=0;i<7;i++) {
				SetLed(i,1);
			}
			SetFrontLed(1);
			//push for a while
			SetSpeedRight(1000);
			SetSpeedLeft(1000);
			/*wait*/for(i=0;i<800000;i++) {asm("nop");}
			
			LedClear();
			SetFrontLed(0);
			//reverse after pushing
			left = -900;
			right = -900;
			waitlonger = 1;
		}
		
		//max speed
		if (left>1000) {
			left = 1000;
		}
		if (right>1000) {
			right = 1000;
		}
		if (left<-1000) {
			left = -1000;
		}
		if (right<-1000) {
			right = -1000;
		}
		
		/*wait*/for(i=0;i<40000;i++) {asm("nop");}
		SetSpeedRight(right);
		SetSpeedLeft(left);
		
		if (waitlonger == 1) {
			/*wait*/for(i=0;i<100000;i++) {asm("nop");}
			waitlonger = 0;
		}
	}
}