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