task main(int argc, char** argv)
{
EV3Init();
SetSensorTouch(S1);
SetSensorColour(S2);
SetSensorColour(S3);
SetSensorUS(S4);
//SetSensorNXTUS(S4);
//SetSensorNXTLight(S4);
//SetColourSensorMode(S2, REFLECTED);
while(1)
{
TextNumOut(0, LCD_LINE1, "Tch : ", SENSOR_1, 15);
TextNumOut(0, LCD_LINE2, "LCol: ", SENSOR_2, 15);
TextNumOut(0, LCD_LINE3, "RCol: ", SENSOR_3, 15);
TextNumOut(0, LCD_LINE4, "US : ", SENSOR_4, 15);
//usleep(10000);
}
EV3Exit();
return (EXIT_SUCCESS);
}
task taskControl()
{
Follows(main);
int ack;
char pfData[8];
SetSensorTouch(IN_4);
while(true) {
if(Sensor(IN_4) == 1)
{
StopAllTasks(); // stop the program
}
ack = bluetooth_receiveInt();
if (ack==1){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=25;
}
if (ack==2){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=-25;
}
if (ack==3){
pfData[IR_LEFT]=0;
pfData[IR_RIGHT]=0;
}
if (ack==4){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=50;
}
if (ack==5){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-50;
}
if (ack==6){
pfData[IR_LEFT]=25;
pfData[IR_RIGHT]=50;
}
if (ack==7){
pfData[IR_LEFT]=50;
pfData[IR_RIGHT]=25;
}
if (ack==8){
pfData[IR_LEFT]=-25;
pfData[IR_RIGHT]=-50;
}
if (ack==9){
pfData[IR_LEFT]=-50;
pfData[IR_RIGHT]=-25;
}
motorControlDrive = (pfData[IR_LEFT] + pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
motorControlSteer = (pfData[IR_LEFT] - pfData[IR_RIGHT]) * CONTROL_SPEED / 200.0;
Wait(CONTROL_WAIT);
}
}
task main()
{
EV3Init();
SetSensorTouch(S1);
SetSensorColour(S2);
SetSensorColour(S3);
SetSensorUS(S4);
int LBLACK;
int RBLACK;
int LGREEN;
int RGREEN;
int LYELLOW;
int RYELLOW;
int LBG;
int RBG;
int LGY;
int RGY;
//Read and Display Values
//Black
while(EnterButtonState() == NOTPRESSED);
LBLACK = SENSOR_2;
RBLACK = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LBlck: ", LBLACK, 15);
TextNumOut(0, LCD_LINE2, "RBlck: ", RBLACK, 15);
Wait(500); //Threshold
//Green
while(EnterButtonState() == NOTPRESSED);
LGREEN = SENSOR_2;
RGREEN = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LGrn: ", LGREEN, 15);
TextNumOut(0, LCD_LINE2, "RGrn: ", RGREEN, 15);
Wait(500);
//Yellow
while(EnterButtonState() == NOTPRESSED);
LYELLOW = SENSOR_2;
RYELLOW = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LYlw: ", LYELLOW, 15);
TextNumOut(0, LCD_LINE2, "RYlw: ", RYELLOW, 15);
Wait(500);
//Calc. calibration values
while(EnterButtonState() == NOTPRESSED);
LBG = (LBLACK + LGREEN) / 2;
RBG = (RBLACK + RGREEN) / 2;
LGY = (LGREEN + LYELLOW) / 2;
RGY = (RGREEN + RYELLOW) / 2;
TextNumOut(0, LCD_LINE1, "LBG: ", LBG, 15);
TextNumOut(0, LCD_LINE2, "RBG: ", RBG, 15);
TextNumOut(0, LCD_LINE3, "LGY: ", LGY, 15);
TextNumOut(0, LCD_LINE4, "RGY: ", RGY, 15);
Wait(500);
//LBG = 10;
//RBG = 10;
//LGY = 100;
//RGY = 100;
//Line Follower
while(EnterButtonState() == NOTPRESSED);
while(SENSOR_2 < LGY || SENSOR_3 < RGY)
while(true)
{
TextNumOut(0, LCD_LINE1, "Tch : ", SENSOR_1, 15);
TextNumOut(0, LCD_LINE2, "LCol: ", SENSOR_2, 15);
TextNumOut(0, LCD_LINE3, "RCol: ", SENSOR_3, 15);
TextNumOut(0, LCD_LINE4, "US : ", SENSOR_4, 15);
if(SENSOR_2 > LGY)
{
OnFwd(OUT_A, -50);
OnFwd(OUT_D, 90);
}
else if(SENSOR_3 > RGY)
{
OnFwd(OUT_A, 90);
OnFwd(OUT_D, -50);
}
else if(SENSOR_2 < LBG)
{
OnFwd(OUT_A, -50);
OnFwd(OUT_D, 90);
}
else if (SENSOR_3 < RBG)
{
OnFwd(OUT_A, 90);
OnFwd(OUT_D, -50);
}
else OnFwd(OUT_AD, 80);
Wait(1);
}
//Vind het blikje
OnFwd(OUT_D, 100);
OnFwd(OUT_A, -100);
Wait(300);
OnFwd(OUT_A, 80);
OnFwd(OUT_C, -80);
while(SENSOR_4 > 25);
Wait(75);
OnFwd(OUT_AC, 100);
Wait(2000);
Off(OUT_AC);
EV3Exit();
return (EXIT_SUCCESS);
}
