void MedicDrive::autoDrive(double target, double speed)
{
static bool init = true;
static double initTicks = 0;
currentTicks = (leftEncoder->Get() + rightEncoder->Get()) / 2;
if(init)
{
initTicks = currentTicks;
targetTicks = target / INCHES_PER_TICK;
init = false;
}
else
{
deltaTicks = currentTicks - initTicks;
error = targetTicks - deltaTicks;
if(error < 0 - AUTO_DRIVE_DEADZONE / INCHES_PER_TICK)
{
setLinVelocity(-speed);
isAtDriveTarget = false;
}
else if(error > 0 + AUTO_DRIVE_DEADZONE / INCHES_PER_TICK)
{
setLinVelocity(speed);
isAtDriveTarget = false;
}
else
{
setLinVelocity(0);
isAtDriveTarget = true;
init = true;
}
drive();
}
}
void PhysShape::unpack(BitStream* stream)
{
bool enabled = stream->readFlag();
if (isEnabled()!=enabled)
setEnable(enabled);
if (enabled)
{
bool active = stream->readFlag();
bool toInactive = false;
if (!active)
{
toInactive = stream->readFlag();
if (!toInactive)
return;
}
if (!isActive())
setActive(true);
QuatF q;
VectorF vec;
mathRead(*stream, &vec);
setPosition(vec);
mathRead(*stream, &q);
setRotation(q);
mathRead(*stream, &vec);
setForce(vec);
mathRead(*stream, &vec);
setTorque(vec);
mathRead(*stream, &vec);
setLinVelocity(vec);
mathRead(*stream, &vec);
setAngVelocity(vec);
if (toInactive)
setActive(false);
}
}
