void turn(bool direction, int degrees, int power)
{
float amount = ROBOT_RADIUS * degreesToRadians(degrees) * 2;
if(direction == LEFT)
driveInches(amount, -1*power, power);
else
driveInches(amount, power, -1*power);
}
task main()
{
/*
while(true)
{
print(SensorValue[sonar], 4);
}
*/
driveInches(16,30,20);
wait1Msec(500);
int sonarValue = SensorValue[sonarFront];
wait1Msec(500);
wallFollowDrive2(84,30,30);
}
/// called when program starts
task main()
{
setMotorEncoderUnits(encoderRotations);
setMotorSpeed(armMotor, 0);// don't let arm move around
// drive to object
driveInches(13.0, 50);
rotate(-90, true);
driveInches(12.0, 50);
rotate(90, true);
driveInches(11.0, 50);
rotate(90, true);
driveInches(12.0, 50);
rotate(-90, true);
driveInches(12.0, 50);
rotate(-90, true);
driveInches(24.0, 50);
rotate(-90, true);
driveInches(24.0, 50);
rotate(90, true);
driveInches(12.0, 50);
rotate(90, true);
// keep going until we hit the object
while(!limitSwitchPressed()) {
setMotorSpeed(leftMotor, DRIVE_SPEED);
setMotorSpeed(rightMotor, DRIVE_SPEED);
}
// back up
driveInches(-4.0, 50);
// pick up object
clawClose(false);
homeArm();
driveInches(1, 50);
clawClose(true);
driveInches(-4, 50);
setArmDegrees(400);
// get to line
while(getColorGrayscale(colorSensor) > LINE_THRESHOLD){
setMotorSpeed(leftMotor, DRIVE_SPEED);
}
while(!limitSwitchPressed()) {
// follow line
displayString(1, "%d", getColorGrayscale(colorSensor));
if(getColorGrayscale(colorSensor) > LINE_THRESHOLD) {
setMotorSpeed(leftMotor, 50);
setMotorSpeed(rightMotor, 0);
}else {
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 50);
}
}
// back up and put down object
driveInches(-3.5, 50);
homeArm();
clawClose(false);
setArmDegrees(400);
}
task main()
{
eraseDisplay();
bool i = inputWaitForStart();//check for wait for start
wait1Msec(500);
if(i)
{
waitForStart();
}
initializeRobot();
/*
nMotorEncoder[left] = 0;
while(abs(nMotorEncoder[left]) < inchesToEncoder(32))
{
motor[right]=100;
motor[left] = 0;
PlaySound(soundBeepBeep);
}
stopMotors();
return;
*/
driveInches(88,35,35); //drive to tube
wait1Msec(500);
servoDown();
wait1Msec(500);
driveInches(-5);
raiseLift();
wait1Msec(500);
dump();
wait1Msec(500);
dumpUp();
wait1Msec(500);
// turn around and drive to ramp
nMotorEncoder[left] = 0;
//start of floor code
//turn around and drive to ramp
nMotorEncoder[right] = 0;
//start of floor code
//go flat against wall
while(abs(nMotorEncoder[right]) < inchesToEncoder(20))
{
motor[left]= 50;
motor[right] = -50;
print(nMotorEncoder[left], 4);
}
//turn towards floor parking zone
motor[left]= 100;
motor[right] = 0;
wait1Msec(200);
motor[left] = 0;
//drive to parking zone
driveInches(120,50,50);
//release thingy
servoUp();
return;
//start of ramp code
while(abs(nMotorEncoder[left]) < inchesToEncoder(64))
{
motor[right]=100;
motor[left] = 0;
}
stopMotors();
//driveInches(4);
//driveInches(40,50,0);
turn(LEFT, 40);
driveInches(257,100,100);
turn(RIGHT, 30);
wait1Msec(5000);
PlaySound(soundFastUpwardTones);
driveInches(30, 50, 50);
wait1Msec(500);
turn(LEFT, 30);
wait1Msec(500);
driveInches(30, 50, 50);
servoUp();
//drive back off of ramp
driveInches(-90);
}
void moveWithDirection(float distance, byte power) {
driveInches(distance, power);
updateCoords(distance * sgn(power));
}
void moveWithDirection(float distance, byte power, int stopSeconds) {
driveInches(distance, power, stopSeconds);
updateCoords(distance * sgn(power));
}
