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)); }