void printLayar(long red, long green, long blue){
writeDebugStreamLine("R : %ld",red);
writeDebugStreamLine("G : %ld",green);
writeDebugStreamLine("B : %ld",blue);
getColorRGB(colorSensor,red,green,blue);
displayTextLine(2,"R : %ld",red);
displayTextLine(3,"G : %ld",green);
displayTextLine(4,"B : %ld",blue);
}
static unsigned long random_color(unsigned char *r, unsigned char *g,
unsigned char *b)
{
double d1, d2;
d1 = (double)rand() / (double)RAND_MAX;
d2 = (double)rand() / (double)RAND_MAX;
return getColorRGB(d1, d2 + 0.5, r, g, b);
}
task main()
{
long red,green,blue;
float jarakSementara = 0;
// sensor sees light:
resetTimer(timer1);
int costWarna;
while(true)
{
printLayar(red,green,blue);
getColorRGB(colorSensor,red,green,blue);
if(red < 20 && green < 20 && blue < 20)
{
// counter-steer left:
motor[leftMotor] = 25;
motor[rightMotor] = 15;
}
// sensor sees dark:
else if (red > 70 && green > 70 && blue > 70)
{
//counter right
motor[leftMotor] = 5;
motor[rightMotor] = 25;
}
//sensor sees light
else
{
//go straight
motor[leftMotor] = 20;
motor[rightMotor] = 20;
sleep(200);
costWarna = checkCost(red,green,blue);
if (costWarna == 100) //intersection green
{
motor[leftMotor] = 0;
motor[rightMotor] = 0;
sleep(1000);
displayTextLine(7,"detected intersection");
} else if (costWarna == 101) //intersection green
{
turnLeft();
turnLeft();
sleep(1000);
displayTextLine(7,"detected stop turn around");
}
displayTextLine(6,"costWarna = %d",costWarna);
}
jarakSementara = getTimer(timer1, seconds);
printJarak(jarakSementara);
}
}
static unsigned long random_color(unsigned char *r, unsigned char *g, unsigned char *b)
{
double d1, d2;
/* To avoid warning messages from about "cast does not match function type"
we allow implicit casts */
d1 = rand();
d1 = d1 / (double) RAND_MAX;
d2 = rand();
d2 = d2 / (double)RAND_MAX;
return getColorRGB(d1, d2 + 0.5, r, g, b);
}
int moveToColor() {
int r,g,b,colorSeen;
while(true)
{
if (isSensorSeesLight()){
steerLeft();
}
else {
steerRight();
}
getColorRGB(colorSensor,r,g,b);
colorSeen = detectColor(r,g,b);
displayTextLine(0,"color seen : %d -> %d %d %d", colorSeen,r,g,b);
if ( colorSeen != bw ) return colorSeen;
}
}
int check_color(){
int res=-1; //if no color.
int i=0;
long red=0;
long grn=0;
long blu=0;
getColorRGB(colorSensor,red,grn,blu);//Baca warna
displayTextLine(1,"R: %d, G: %d, B: %d",red,grn,blu);
while(res==-1 && i<color_number){
if(red>color_lib[i][0]-color_tolerance && red<color_lib[i][0]+color_tolerance &&
grn>color_lib[i][1]-color_tolerance && grn<color_lib[i][1]+color_tolerance &&
blu>color_lib[i][2]-color_tolerance && blu<color_lib[i][2]+color_tolerance){
res=i;
break;
}
i++;
}
return res;
}
