void finalMethod(){
int masterPower = 30;
int slavePower = 30;
int error = 0;
//float error = 0;
//float kp = 0.2; -- float didnt work to good
int kp = 6;//initial value was 5, adjusted to get a better straight line
//changed to flaoting point numbers as ev3 does have floating point (original code was for a different system that only supported integers
resetMotorEncoder(motorB); //sets both motor 'encoders' (distance recorders) to zero
resetMotorEncoder(motorC);
//Repeat ten times a second.
while(getUSDistance(S4)> 10)// changed to 10 is probably safer
//while(getTouchValue (S1) || getTouchValue (S2))
{
//Set the motor powers to their respective variables.
setMotorSpeed(motorB, masterPower);
setMotorSpeed(motorC, slavePower);
error = getMotorEncoder(motorB)- getMotorEncoder(motorC);
slavePower += error / kp;
//slavePower = error * kp;
//Reset the encoders every loop so we have a fresh value to use to calculate the error.
resetMotorEncoder(motorB);
resetMotorEncoder(motorC);
}
while(!getTouchValue (S1) || !getTouchValue (S2) ){
sleep(50);
}
setMotorSpeed(motorB, 100);
setMotorSpeed(motorC, 100);
sleep(3000);
setMotorSpeed(motorB, 0);
setMotorSpeed(motorC, 0);
}
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()
