void moveStraight(int magnitude, int time) //we need to calculate speed in units as "tiles/second"
// time is in seconds
{
motor[LeftBWheel] = motor[LeftMWheel] = motor[LeftFWheel] = motor[RightBWheel] = motor[RightMWheel] = motor[RightFWheel] = magnitude;
wait1Msec(1000*time); // wait1Msec only accepts milliseconds
if(magnitude > 0)
preciseDriveStop(FORWARD);
else if(magnitude < 0)
preciseDriveStop(BACKWARD);
else
/* Pseudocode -
while(SensorValue[RightEncoder]
*/
resetValues();
}
// direction is binary -1 or 1, positive is right
// time is milliseconds
// degrees is always positive
void spin(int time, int direction, int degrees) // we need to calculate degrees per tick
{
nMotorEncoder[LeftMWheel] = 0;
// Must test
while(nMotorEncoder[LeftBWheel] < direction * degrees)
{
motor[LeftBWheel] = motor[LeftMWheel] = motor[LeftFWheel] = direction * magnitude;
motor[RightBWheel] = motor[RightMWheel] = motor[RightFWheel] = -direction * magnitude;
}
if(direction > 0)
preciseDriveStop(RIGHT);
else
preciseDriveStop(LEFT);
}
void softSpin(int direction, int time, int degrees)
{
nMotorEncoder[LeftMWheel] = 0;
// Must test
while(abs(nMotorEncoder[LeftMWheel]) < degrees)
{
motor[LeftBWheel] = motor[LeftMWheel] = motor[LeftFWheel] = direction * TWENTY_FIVE;
motor[RightBWheel] = motor[RightMWheel] = motor[RightFWheel] = -direction * TWENTY_FIVE;
}
if(direction > 0)
preciseDriveStop(RIGHT);
else
preciseDriveStop(LEFT);
}
void noRamp(int direction, int distance)
{
nMotorEncoder[LeftMWheel] = 0;
while(abs(nMotorEncoder[LeftMWheel]) < distance)
{
setLeft(direction*80); setRight(direction*80);
}
if(direction>0)
{
preciseDriveStop(FORWARD);
}
else
{
preciseDriveStop(BACKWARD);
}
}
// direction is binary -1 or 1, positive is right
// time is milliseconds
// degrees is always positive
void spin(int direction, int time, int degrees) // 90 degrees is 200 both sides.
{
nMotorEncoder[LeftMWheel] = 0;
// Must test
while(abs(nMotorEncoder[LeftMWheel]) < degrees)
{
motor[RightBWheel] = motor[RightMWheel] = motor[RightFWheel] = -direction * magnitude;
motor[LeftBWheel] = motor[LeftMWheel] = motor[LeftFWheel] = direction * magnitude;
}
if(direction > 0)
preciseDriveStop(RIGHT);
else
preciseDriveStop(LEFT);
}
// direction is binary -1 or 1, positive is right
// time is milliseconds
// degrees is always positive
void spin(int direction, int time, int degrees) // we need to calculate degrees per tick
{
nMotorEncoder[LeftMWheel] = 0;
// Must test - works
while(abs(nMotorEncoder[LeftMWheel]) < degrees)
{
motor[LeftBWheel] = motor[LeftMWheel] = motor[LeftFWheel] = direction * FIFTY;
motor[RightBWheel] = motor[RightMWheel] = motor[RightFWheel] = -direction * FIFTY;
}
if(direction > 0)
preciseDriveStop(RIGHT);
else
preciseDriveStop(LEFT);
}
void backRed()
{
//move straight into the balls in the corner
MoveLinear(64); //half-speed forward motion
wait1Msec(100); //wait to start the intake rolling
motor[LeftIntake] = 127; //intake inwards to pick up balls
motor[RightIntake] = 127;
wait1Msec(500);
preciseDriveStop(FORWARD);
wait1Msec(1000);
MoveLinear(-64);//reversing at half speed
wait1Msec(600);
preciseDriveStop(BACKWARD);
wait1Msec(1000);
Rotate(-64); //go for a ~165 degree turn
wait1Msec(1000);
Rotate(0); // end rotation
wait1Msec(1000);
motor[LeftArm] = 64;
motor[RightArm] = 64;
MoveLinear(64); //moves up to barrier
wait1Msec(300);
preciseDriveStop(FORWARD);
wait1Msec(1000);
motor[LeftArm] = 0;
motor[RightArm] = 0;
motor[LeftIntake] = -127; //output balls!
motor[RightIntake] = -127;
wait1Msec(900);
wait1Msec(1000);
Rotate(64); //rotate to the left to pickup big ball
wait1Msec(400);
Rotate(0);
wait1Msec(1000);
MoveLinear(64);
motor[LeftIntake] = 127; //intake inwards to pick up balls
motor[RightIntake] = 127;
wait1Msec(200);
motor[LeftIntake] = 0;
motor[RightIntake] = 0;
wait1Msec(700);
preciseDriveStop(FORWARD);
wait1Msec(1000);
Rotate(-64); //point towards scoring zones
wait1Msec(200);
Rotate(0);
wait1Msec(1000);
motor[LeftIntake] = -127; //output balls!
motor[RightIntake] = -127;
wait1Msec(500);
}
