void Motor_setRight(int percentage){
SetSpeedRight(percentage);
}
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);
}
}
}
}
void Motor_stop(){
SetSpeedRight(1);
SetSpeedLeft(1);
}
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;
}
}
}
