#pragma config(Sensor, in1, firesensor, sensorPotentiometer)
#pragma config(Sensor, in5, Ultrasonic, sensorSONAR, int1)
#pragma config(Sensor, in3, leftEncoder, sensorRotation)
#pragma config(Sensor, in2, rightEncoder, sensorRotation)
#pragma config(Motor, port2, rightMotor, tmotorNormal, openLoop,reversed)
#pragma config(Motor, port3, leftMotor, tmotorNormal, openLoop)
#pragma config(Motor, port7, rightarm, tmotorNormal, openLoop)
#pragma config(Motor, port8, leftarm, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//turn left
//Look for the fire
//If you find the fire gofoward
//If you don't find the fire turn left more (repeat)
//to get out of the room use ultrasonic to find the empty wall
void goforward(int distance)
{
SensorValue(leftEncoder)=0;
SensorValue(rightEncoder)=0;
while(SensorValue[leftEncoder]<distance && SensorValue(rightEncoder)<distance)
{
if (SensorValue(Ultrasonic)>2)
{
motor[port3]=60;
motor[port2]=60;
}
}
motor[port3]=0;
motor[port2]=0;
}
void raisearm(int time10)
{
motor[port7]=-34;
motor[port8]=34;
wait10Msec(15);
}
void findflame2(int time10)
{
SensorValue[leftEncoder] = 0;
bool flamenotfound2 = true;
while(flamenotfound2) //Play with number 180
{
if (SensorValue(firesensor)<100 && SensorValue[leftEncoder]<520)//search
{
motor[port2]=-60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>100 && SensorValue(firesensor)<200)// first go find it
{
motor[port2]=60;
motor[port3]=60;
wait10Msec(10);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>200 && SensorValue(firesensor)<450)
{
motor[port2]=-60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>450 && SensorValue(Ultrasonic)>6)
{
motor[port2]=60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>850 && SensorValue(Ultrasonic)<6)
{
SensorValue(leftEncoder)=0;
SensorValue(rightEncoder)=0;
while(SensorValue(leftEncoder)<200 && SensorValue(rightEncoder)<200)
{
motor[port2]=0; //Stop
motor[port3]=0;
wait10Msec(5);
motor[port7]=127; //commence wacking
motor[port8]=-127;
wait10Msec(50);
motor[port7]=-127; //return wacker
motor[port8]=127;
wait10Msec(50);
motor[port2]=-60; // Spin by time !Not Good!
motor[port3]=60;
wait10Msec(7);
motor[port2]=0; //Stop
motor[port3]=0;
wait10Msec(5);
motor[port7]=127; //commence wacking
motor[port8]=-127;
wait10Msec(50);
motor[port7]=-127; //return wacker
motor[port8]=127;
wait10Msec(50);
motor[port2]=60; // Good!
motor[port3]=-60;
wait10Msec(7);
}
}
// Signal that the flame is found
if (SensorValue(firesensor)<=60 && SensorValue(leftEncoder)>=520)
{
flamenotfound2 = false;
}
}
}
void goleft(int distance)
{
SensorValue(leftEncoder)=0;
SensorValue(rightEncoder)=0;
while(SensorValue[leftEncoder]<distance && SensorValue(rightEncoder)<distance)
{
motor[port3]=-60;
motor[port2]=60;
}
motor[port3]=0;
motor[port2]=0;
}
void goright(int distance)
{
SensorValue(leftEncoder)=0;
SensorValue(rightEncoder)=0;
while(SensorValue[leftEncoder]<distance && SensorValue(rightEncoder)<distance)
{
motor[port3]=60;
motor[port2]=-60;
}
motor[port3]=0;
motor[port2]=0;
}
void findflame1(int time10)
{
SensorValue[leftEncoder] = 0;
bool flamenotfound1 = true;
while(flamenotfound1) //Play with number 180
{
if (SensorValue(firesensor)<100 && SensorValue[leftEncoder]<507)//search
{
motor[port2]=-60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>100 && SensorValue(firesensor)<200)// first go find it
{
motor[port2]=60;
motor[port3]=60;
wait10Msec(10);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>200 && SensorValue(firesensor)<450)
{
motor[port2]=-60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>450 && SensorValue(Ultrasonic)>6)
{
motor[port2]=60;
motor[port3]=60;
wait10Msec(5);
motor[port2]=0;
motor[port3]=0;
wait10Msec(1);
}
if (SensorValue(firesensor)>850 && SensorValue(Ultrasonic)<5)
{
SensorValue(leftEncoder)=0;
SensorValue(rightEncoder)=0;
while(SensorValue(leftEncoder)<200 && SensorValue(rightEncoder)<200)
{
motor[port2]=0; //Stop
motor[port3]=0;
wait10Msec(5);
motor[port7]=127; //commence wacking
motor[port8]=-127;
wait10Msec(50);
motor[port7]=-127; //return wacker
motor[port8]=127;
wait10Msec(50);
motor[port2]=-60; // Spin by time !Not Good!
motor[port3]=60;
wait10Msec(10);
motor[port2]=0; //Stop
motor[port3]=0;
wait10Msec(5);
motor[port7]=127; //commence wacking
motor[port8]=-127;
wait10Msec(50);
motor[port7]=-127; //return wacker
motor[port8]=127;
wait10Msec(50);
motor[port2]=60; // Good!
motor[port3]=-60;
wait10Msec(10);
}
}
// Signal that the flame is found
if (SensorValue(firesensor)<=60 && SensorValue(leftEncoder)>=507)
{
flamenotfound1 = false;
}
}
}
task main()
{
while(true)
{
wait10Msec(200);//searches first room
raisearm(10);
wait10Msec(100);
goforward(373);
wait10Msec(10);
goright(97);
wait10Msec(10);
goforward(320);
wait10Msec(10);
goright(96);
wait10Msec(10);
goforward(135);
wait10Msec(10);
findflame1(150);//looks for flame ans if found puts it out
wait10Msec(10);
goforward(135);//gets out of the room
wait10Msec(10);
goleft(93);
wait10Msec(10);//goes to the next room
goforward(301);
wait10Msec(10);
goright(100);
wait10Msec(10);
goforward(415);
wait10Msec(10);
goright(95);
wait10Msec(10);
goforward(220);//enters room in front
wait10Msec(10);
findflame2(150);
wait10Msec(10);
goforward(215);
wait10Msec(10);
goleft(91);
wait10Msec(10);
goforward(170);
wait10Msec(10);
goright(90);
wait10Msec(10);
goforward(240);
wait10Msec(10);
findflame1(150);
wait1Msec(10);
goforward(120);
wait10Msec(10);
goright(90);
wait10Msec(10);
goforward(200);
wait10Msec(10);
goright(90);
wait10Msec(10);
goforward(200);
wait10Msec(10);
goleft(90);
wait10Msec(10);
goforward(120);
wait10Msec(10);
findflame1(150);
wait10Msec(10);
}
}
No comments:
Post a Comment