task main()
{
waitForStart();
initializeRobot();
zeroEncoders();
accelerate(100, 2000, 0, 50);
zeroEncoders();
decelerate(0, 250, 100, 10);
wait10Msec(100);
}
void moveFB(int direction, int ticks, int speed){
zeroEncoders();
float wt = 2;
if(direction == up){
while(getFBEncoderAverage() < ticks){
if (getFBEncoderAverage() == ticks - 100 && speed != 127){
setFBMotors(speed - 20);
}
else {
setFBMotors(speed);
}
}
setFBMotors(-127);
wait10Msec(wt);
}
else if(direction == down){
while(getFBEncoderAverage() < ticks){
if (getFBEncoderAverage() == ticks - 100){
setFBMotors(speed + 20);
}
else {
setFBMotors(-speed);
}
}
setFBMotors(127);
wait10Msec(wt);
}
else {
setFBMotors(0);
}
}
task main()
{
motor[intakeLeft] = -127;
motor[intakeRight] = -127;
zeroEncoders();
while(rightEncoder() + leftEncoder() < (1500)*2)
{
motor[driveLeftBack] = 100;
motor[driveLeftFront] = 100;
motor[driveRightBack] = 100;
motor[driveRightFront] = 100;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
motor[intakeLeft] = 0; // Stop Arm
motor[intakeRight] = 0;
//zeroEncoders();
//driveForwardInches(1,100);
////wait10Msec(10);
//pivotTurnLeft(100);
////wait10Msec(10);
//pivotTurnRight(100);
////wait10Msec(10);
//driveBackwardInches(12,100);
}
task usercontrol()
{
// Flags
// Servo / program initialization:
zeroEncoders();
// Main loop:
waitForStart();
while (true)
{
getJoystickSettings(joystick);
setLeft(100);
setRight(100);
}
return;
}
task main()
{
const int maxSpeed = 100;
zeroEncoders();
waitForStart();
motor[left] = maxSpeed;
motor[right] = maxSpeed;
wait1Msec(3610);
motor[left] = 0;
motor[right] = 0;
liftHeight(60 + amountAboveTubes);
servo[door] = 255;
wait1Msec(3000); // TODO: Correct
liftHeight(zeroLift);
servo[door] = 100;
wait1Msec(500);
}
void driveForwardInches(float inches, int speed)
{
zeroEncoders();
while(rightEncoder() + leftEncoder() < (inches * ticksPerInch * 2))
{
motor[driveLeftBack] = speed;
motor[driveLeftFront] = speed;
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
void pivotTurnLeft(int speed)
{
zeroEncoders();
while(rightEncoder() - leftEncoder() < (inchesFor90Turn * ticksPerInch))
{ // Turn left while encoders are less than target value
motor[driveLeftBack] = -speed;
motor[driveLeftFront] = -speed;
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
void move(int direction, int speed, int distance, int FBbreakspd){
revolutions = 360*(distance/wheelCircumference);
switch (direction) {
case forward:
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(127,127,127, 127);
setFBMotors(FBbreakspd);
}
zeroEncoders();
Break(forward);
break;
case turnLeft: //int FLSpeed, int FRspeed, int BRspeed, int BLspeed
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(-127, 127, 127, -127);
setFBMotors(FBbreakspd);
}
Break(turnLeft);
break;
case turnRight: //int FLSpeed, int FRspeed, int BRspeed, int BLspeed
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(127, -127, -127, 127);
setFBMotors(FBbreakspd);
}
Break(turnRight);
break;
case backwards:
zeroEncoders();
while(getDTEncoderAverage() < revolutions){
setDriveMotors(-127, -127, -127, -127);
setFBMotors(FBbreakspd);
}
Break(backwards);
break;
default:
setDriveMotors(0,0,0,0);
zeroEncoders();
}
zeroEncoders();
}
void move( int steps)
{
zeroEncoders();
nSyncedMotors = synchEG;
nSyncedTurnRatio = +100;
nMotorEncoderTarget[motorE] = steps;
nSyncedMotors = synchDF;
nSyncedTurnRatio = +100;
nMotorEncoderTarget[motorD] = steps;
while(nMotorEncoder[motorD] < steps)
{}
}
void encoderTest(int ticks){
zeroEncoders();
while(getDTEncoderAverage() < ticks){
setDriveMotors(127,127,127, 127);
}
}
