void motor_control()
{
if ( runFlag == false ){
if ( p2_0 == 0 ){
Motor(FORWARD, 55, 55);
runFlag = true ;
}
}
else if ( p2_0 == 0 ){
Motor(STOP,0,0);
runFlag = false ;
}
else if ( ad_data[1]<ad_data[2]-50 ){
// Speed down ;
RightTurn(20, 10);
}
else if ( ad_data[1]>ad_data[2]+50 ){
// Speed down ;
LeftTurn(10, 20);
}
/* else if (ad_data[0]!=0x01FF & ad_data[1]!=0x01FF & ad_data[2]!=0x01FF & ad_data[3]!=0x01FF){
Motor(FORWARD, 0, 0);
} */
else {
// Speed Up;
Motor(FORWARD, 55,55);
}
}
void first(){
switch(condition) {
case 0:
lineTracking();
switch(sensor.middle_sensor) {
case 0b00001111:
case 0b00011111:
case 0b00111111:
case 0b01111111:
case 0b00011110:
case 0b00111110:
condition = 1;
break;
}
break;
case 1:
wheels.stop();
RightTurn();
break;
case 2: CheckStop(); break;
case 3: Forward(); break;
case 4: BackToLine(); break;
case 5: LeftTurn(); break;
case 6:
lineTracking();
if(sensor.front_sensor == 0b00111100 || sensor.front_sensor == 0b00011100 || sensor.front_sensor == 0b00111000)
{
wheels.forward(0,0);
condition = 6;
}
break;
}
}
//Move next line on right
int NextLineRight(int direction)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
total++;
ForwardSQ();
RightTurn();
motor[motorB]=20;
motor[motorC]=20;
wait1Msec(2200);
RightTurn();
direction=1;
return(direction);
}//end NextLineRight ()
/*
This task makes the robot follow the line,
turn around corners,
handle crossings
and detect an obstacle.
*/
task Follow(){
int i = 0;
startTask(Sound);
while (true){
status = "active";
/*
Objects are detected below here.
When an object is detected the user can either choose to avoid it or charge.
*/
if(SonarSensor()){
while(speed > 0){
speed = Brake(i,speed);
i--;
Straight(speed);
}
status = "engage";
StopSound();
s = "";
while((s != "FIRE")&&(s != "B")){
}
if (s == "FIRE"){
startTask(Sound);
startTask(AvoidObject);
}
}
else if(speed < max_Speed){
speed = Accelerate(i,speed);
i++;
}
Straight(speed);
/*
Crossings are handled below here.
Once both lightsensors detect the line at the same time the robot will brake unless it was told to go straight.
It will then drive backward to find the line again after which it will continue untill it has crossed it.
The robot will then look at the last given command to determine which way it will go.
*/
if((Convert(SensorValue[S3],minlight,maxlight) < 70) && (Convert(SensorValue[S4],mincolor,maxcolor) < 70)){
if (command != "straight"){
speed = EmergencyBrake(speed);
wait1Msec(1000);
while(Convert(SensorValue[S3],minlight,maxlight) > 30 || Convert(SensorValue[S4],mincolor,maxcolor) > 30){
Straight((-10));
}
if (Convert(SensorValue[S3],minlight,maxlight) < 70 || Convert(SensorValue[S4],mincolor,maxcolor) < 70){
nMotorEncoderTarget[motorB]=(nMotorEncoderTarget[motorC]=105);
motor[motorB]=(motor[motorC]=-10);
while((nMotorRunState[motorB] != runStateIdle) && (nMotorRunState[motorC] != runStateIdle)) wait1Msec(1);
}
if ((s == "FIRE")||(s == "")){
StopSound();
while ((s == "FIRE")||(s == "")){
Straight(0);
}
startTask(Sound);
}
if (command == "left"){
LeftTurn();
}
else if (command == "right"){
RightTurn();
}
}
else{
wait1Msec(200);
}
}
//The code below here makes sure the robot stays on the line.
if(Convert(SensorValue[S3],minlight,maxlight) < 80){
Left(Convert(SensorValue[S3],minlight,maxlight),speed);
}
if(Convert(SensorValue[S4],mincolor,maxcolor) < 80){
Right(Convert(SensorValue[S4],mincolor,maxcolor),speed);
}
wait1Msec(1.5);
}
}
task main()
{
int count=0;
int direction=1;
int end=0;
int doubleline=0;
int pause=0;
//turn right go forward until hit double line go back half a square then turn left (You are at bottom left sq now
RightTurn();
while(pause==0)
{
motor(motorB)=SPEED;
motor(motorC)=SPEED;
wait1Msec(1);
if(SensorValue(S3)<45&&doubleline!=1)
{
if(time1[T1]<200)
{
pause=1;
}
doubleline=1;
}
if(SensorValue(S3)>45&&doubleline==1)
{
doubleline=0;
clearTimer(T1);
}
}
motor[motorB]=-20;
motor[motorC]=-20;
wait1Msec(700);
LeftTurn();
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=6)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos2=0;
pos1++;
}
}
if(direction==2)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos2=0;
pos1++;
}
}//end if()
//go back 7 suares and take away distance from bottom col from start from 7 and go to start
}//end while()
}//end main()
task main()
{
int count=0;
int direction=1;
int end=0;
int doubleline=0;
int pause=0;
//open the file to write
fileHandle=fileOpenWrite(filename);
//turn right go forward until hit double line go back half a square then turn left You are at bottom left sq now
RightTurn();
while(pause==0)
{
motor(motorB)=SPEED;
motor(motorC)=SPEED;
wait1Msec(1);
if(SensorValue(S3)<45&&doubleline!=1)
{
if(time1[T1]<200)
{
pause=1;
}
doubleline=1;
}
if(SensorValue(S3)>45&&doubleline==1)
{
doubleline=0;
clearTimer(T1);
}
}
motor[motorB]=-20;
motor[motorC]=-20;
wait1Msec(700);
LeftTurn();
//start traversing grid
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]='1';
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}
if(direction==2)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]='1';
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2--;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}//end if()
}//end while()
checkarray();
//Goes back to the start function
GoStart();
//THIS IS THE SECOND PART OF THE PROGRAM TO MAP THE LOCATION OF THE OBJECT
while(1==1)
{
if(getTouchValue(S2)==1)
{
//reset all values
pos1=0;
pos2=0;
count=0;
direction=1;
end=0;
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
//check to see if there is an object 140mm away
if(getUSDistance(S1)<14)
{
grid[pos1][pos2]='J';
motor[motorB]=0;
motor[motorC]=0;
wait1Msec(1000000);
}
//increment counters
count++;
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}
if(direction==2)
{
//check to see if there is an object 140mm away
end++;
while(count<8)
{
if(getUSDistance(S1)<14)
{
grid[pos1][pos2]='J';
motor[motorB]=0;
motor[motorC]=0;
wait1Msec(1000000);
}
//increment counters
count++;
//move forward one square
Forward();
pos2--;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}//end if()
}//end while()
} }//end if()
}//end main()
