void DriveControl(Controller& controller)
{
// power - hold L1 to go fast, hold L2 to go slow
if ( ControllerButtonIsPressed(controller, ControllerButtonR1) )
{
DrivePowerMultiplier = kDrivePowerHigh;
}
else if ( ControllerButtonIsPressed(controller, ControllerButtonL1) )
{
DrivePowerMultiplier = kDrivePowerLow;
}
else
{
DrivePowerMultiplier = kDrivePowerNormal;
}
// hold L2 or R2 to stomp - let go and they go back up
if ( ControllerButtonIsPressed(controller, ControllerButtonR2) ||
ControllerButtonIsPressed(controller, ControllerButtonL2) )
{
servo[RightStomper] = kRightStomperDown;
servo[LeftStomper] = kLeftStomperDown;
}
else
{
servo[RightStomper] = kRightStomperUp;
servo[LeftStomper] = kLeftStomperUp;
}
// tank drive
SetMotorPower(Left, controller.leftJoystick.y);
SetMotorPower(Right, controller.rightJoystick.y);
}
void DriveLeftMotors(int power)
{
power *= (MotorsReversed() ? -1: 1);
for(int i=(int)PairCount1; i<(int)PairCountEnd; i++)
{
SetMotorPower(LEFT_MOTORS[i], power);
}
}
enum follow_result follow_wall(enum dir dir) {
if (switched && has_hit_wall()) {
switched = false;
// We hit a wall. First, reverse.
SetMotorPower(-BASE_SPEED * 2, -BASE_SPEED * 2);
Msleep(100);
// Continue in direction `dir`.
if (dir == DIR_Left)
SetMotorPower(-BASE_SPEED * 3 / 2, BASE_SPEED * 3 / 2);
else if (dir == DIR_Right)
SetMotorPower(BASE_SPEED * 3 / 2, -BASE_SPEED * 3 / 2);
Msleep(180);
return HIT_WALL;
}
if (dir == DIR_Left)
SetMotorPower(BASE_SPEED * 4 / 3, BASE_SPEED);
else if (dir == DIR_Right)
SetMotorPower(BASE_SPEED, BASE_SPEED * 3 / 2);
return NO_WALL;
}
void SetArmsMotorPower(int power)
{
SetMotorPower(Arm, power);
}
void runLondonEye(bool run) {
SetMotorPower(LONDONI_POSITION,-64);
}
