int main()
{
set_each_analog_state(0,0,0,1,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1)
create_connect();
enable_servos();
set_create_total_angle(0);
set_create_distance(0);
//AUO=start_process(ArmUpOpen);
ArmUp();
set_servo_position(
sleep(3);
CenterCamera();
create_drive_straight(-200);
while(digital(8) != 1)
{}
create_stop();
sleep(1);
ArmDownClose();
sleep(3);
beep();
create_drive_straight(200);
sleep(2);
create_stop();
}
task main() {
initializeRobot();
//waitForStart();
motorForDistance(autonomousWheelPower, 6.4);
wait10Msec(autonomous10Mdelay);
encodedTurn(autonomousWheelPower, 0.5);
wait10Msec(autonomous10Mdelay);
encodedDiagonal(autonomousWheelPower, 4.3);
wait10Msec(autonomous10Mdelay);
encodedTurn(autonomousWheelPower, 0.1);
wait10Msec(autonomous10Mdelay);
motorForDistance(-autonomousWheelPower, 0.10);
motorStrafeForDistance(autonomousWheelPower, 0.25);
wait10Msec(autonomous10Mdelay);
/*
while (currentLightForwardIterations < lightForwardIterations) {
currentLight = SensorValue(lightSensor);
writeDebugStreamLine("Sensor: %d", currentLight);
if (currentLight < lightSensorThreshold) { //-5) || (currentLight > lightSensorThreshold+5)) {
if (lightStrafeCount % 2 == 1) {
motorStrafeForDistance(autonomousWheelPower, lightFineAdjust);
//lightStrafeCount++;
wait10Msec(lightDelay);
}
else if (lightStrafeCount % 2 == 0) {
motorStrafeForDistance(-autonomousWheelPower, lightFineAdjust);
//lightStrafeCount++;
wait10Msec(lightDelay);
}
}
else {
motorForDistance(autonomousWheelPower, lightFineAdjust);
//lightStrafeCount = 1;
lightForwardIterations++;
wait10Msec(lightDelay);
}
}
*/
ArmUpDistance(7.2);
wait10Msec(autonomous10Mdelay);
while (SensorValue(IRseeker) != irTarget) {
writeDebugStreamLine("Sensor: %d", SensorValue(IRseeker));
if (SensorValue(IRseeker) == 0) {
if (irStrafeCount % 2 == 1) {
motorStrafeForDistance(-autonomousWheelPower, 0.2*irStrafeCount);
irStrafeCount++;
wait10Msec(irSeekingDelay);
}
else if (irStrafeCount % 2 == 0) {
//motorStrafeForDistance(autonomousWheelPower, 0.2*irStrafeCount);
irStrafeCount++;
wait10Msec(irSeekingDelay);
}
else
wait10Msec(irSeekingDelay);
}
else if (SensorValue(IRseeker) < irTarget) {
motorStrafeForDistance(-autonomousWheelPower, 0.2);
irLeftRightCount--;
wait10Msec(irSeekingDelay);
}
else if (SensorValue(IRseeker) > irTarget) {
motorStrafeForDistance(autonomousWheelPower, 0.2);
irLeftRightCount++;
wait10Msec(irSeekingDelay);
}
else if (SensorValue(IRseeker) == irTarget) {
break;
}
}
if (irLeftRightCount >= 0)
motorStrafeForDistance(autonomousWheelPower, 0.25);
else if (irLeftRightCount < 0)
motorStrafeForDistance(-autonomousWheelPower, 0.35);
ArmUp();
wait10Msec(autonomous10Mdelay);
motorForDistance(autonomousWheelPower,1.5);
wait10Msec(autonomous10Mdelay);
ArmDownDistance(2);
wait10Msec(autonomous10Mdelay);
motorForDistance(-autonomousWheelPower,3);
wait10Msec(autonomous10Mdelay);
ArmDown();
}
