ok, so i have a robotc that is to navigate a course, find a colored block and deliver it to the correct corner. there is red, blue, and red.
this is the program i have so far and it is a failure... any pointers on how to get my robot to do this would be greatly appreciated!!
Code:
#pragma config(Sensor, S4,lightSensor, sensorLightActive)
#pragma config(Sensor, S3, sonarSensor1, sensorSONAR)//side
#pragma config(Sensor, S1, colorPort, sensorCOLORFULL)
#pragma config(Sensor, S2, sonarSensor2, sensorSONAR)
//Function
// robot goes back and forth across the area
void BackandForth()
{
while(sensorValue[sonarSensor1 || sonarSensor2] > 12)
{
motor(motorB)=20;
motor(motorC)=20;
}
//Left Turn
motor(motorB)=16.665;
motor(motorC)=-16.665;
wait1Msec(1150);
//forward for 1 sec
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(100);
//Left Turn
motor(motorB)=16.665;
motor(motorC)=-16.665;
wait1Msec(1150);
//forward for 5 sec
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(500);
while(sensorValue[sonarSensor1 || sonarSensor2] > 8)
{
motor(motorB)=20;
motor(motorC)=20;
}
//Right Turn
motor(motorB)=-16.665;
motor(motorC)=16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(100);
//Right turn
motor(motorB)=-16.665;
motor(motorC)=16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(500);
while(sensorValue[sonarSensor1 || sonarSensor2] > 8)
{
motor(motorB)=20;
motor(motorC)=20;
}
//Left Turn
motor(motorB)=16.665;
motor(motorC)=-16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(100);
//Left Turn
motor(motorB)=16.665;
motor(motorC)=-16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(500);
while(sensorValue[sonarSensor1 || sonarSensor2] > 8)
{
motor(motorB)=20;
motor(motorC)=20;
}
//Right Turn
motor(motorB)=-16.665;
motor(motorC)=16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(100);
//Right turn
motor(motorB)=-16.665;
motor(motorC)=16.665;
wait1Msec(1150);
motor(motorB)=20;
motor(motorC)=20;
wait1Msec(500);
}
void stayNearWall()
{
while(SensorValue[sonarSensor1 || sonarSensor2] => 10)
{
motor(motorB)=20;
motor(motorC)=0;
wait1Msec(100);
motor(motorB)=20;
motor(motorC)=20;
}
while(SensorValue[sonarSensor1 || sonarSensor2]=< 12)
{
motor(motorB)=20;
motor(motorC)=0;
wait1Msec(100);
motor(motorB)=20;
motor(motorC)=20;
}
}
void ClampClaw()
{
motor(motorA)=25;
wait1Msec(100);
}
void ReleaseClaw()
{
motor(motorA)=-25;
wait1Msec(100);
}
task main()
{
do
motor(motorB)=25;
motor(motorC)=25;
while(SensorValue[sonarSensor1 || sonarSensor2] >5)
{
if(SensorValue[colorPort]== BLUECOLOR)
{
ClampClaw();
stayNearWall();
if (SensorValue[lightSensor] >8 || <11)
{
ReleaseClaw();
motor(motorB)=-50;
motor(motorC)=-50;
wait1Msec(500);
BackandForth();
}
else
{ stayNearWall();
}
if else(SensorValue[colorPort]== REDCOLOR)
{
ClampClaw();
stayNearWall();
if (SensorValue[lightSensor] >12 || >15)
{
ReleaseClaw();
motor(motorB)=-50;
motor(motorC)=-50;
wait1Msec(500);
BackandForth();
}
else
{ stayNearWall();
}
if else(SensorValue[colorPort] == GREENCOLOR)
{
ClampClaw();
stayNearWall();
if (SensorValue[lightSensor] >15 || <13)
{
ReleaseClaw();
motor(motorB)=-50;
motor(motorC)=-50;
wait1Msec(500);
BackandForth();
}
}
else
{
motor(motorB)=50;
motor(motorC)=50;
}
}