void OurRobot::DS_PrintOut() {
struct timeval rawTime;
uint32_t currentTime;
gettimeofday( &rawTime , NULL );
currentTime = rawTime.tv_usec / 1000 + rawTime.tv_sec * 1000;
if ( currentTime - lastTime > 5 ) {
//pidGraph.graphData( currentTime - startTime , 5000.f , "PID0" );
pidGraph.graphData( currentTime - startTime , shooter.getRPM() , "PID0" );
pidGraph.graphData( currentTime - startTime , shooter.getTargetRPM() , "PID1" );
lastTime = currentTime;
}
/* ===== Print to Driver Station LCD =====
* Packs the following variables:
*
* std::string: type of data (either "display" or "autonList")
* unsigned int: drive1 ScaleZ
* unsigned int: drive2 ScaleZ
* unsigned int: turret ScaleZ
* bool: drivetrain is in low gear
* unsigned char: is hammer mechanism deployed
* unsigned int: shooter RPM
* bool: shooter RPM control is manual
* bool: isShooting
* bool: isAutoAiming
* bool: turret is locked on
* unsigned char: Kinect is online
* unsigned int: distance to target
*/
// floats don't work so " * 100000" saves some precision in a UINT
driverStation->clear();
*driverStation << static_cast<std::string>( "display" );
*driverStation << static_cast<unsigned int>(ScaleZ(driveStick1) * 100000.f);
*driverStation << static_cast<unsigned int>(ScaleZ(driveStick2) * 100000.f);
*driverStation << static_cast<unsigned int>(ScaleZ(turretStick) * 100000.f);
*driverStation << static_cast<bool>( false );
*driverStation << static_cast<unsigned char>( bridgeArm.Get() );
*driverStation << static_cast<unsigned int>(shooter.getRPM() * 100000.f);
*driverStation << static_cast<bool>( shooter.getControlMode() == Shooter::Manual );
*driverStation << shooter.isShooting();
*driverStation << isAutoAiming;
*driverStation << static_cast<bool>( fabs( turretKinect.getPixelOffset() ) < TurretKinect::pxlDeadband
&& turretKinect.getOnlineStatus() == sf::Socket::Done );
*driverStation << static_cast<unsigned char>( turretKinect.getOnlineStatus() );
*driverStation << turretKinect.getDistance();
driverStation->sendToDS();
const std::string& command = driverStation->receiveFromDS( &autonMode );
// If the DS just connected, send it a list of available autonomous modes
if ( std::strcmp( command.c_str() , "connect\r\n" ) == 0 ) {
driverStation->clear();
*driverStation << static_cast<std::string>( "autonList" );
for ( unsigned int i = 0 ; i < autonModes.size() ; i++ ) {
*driverStation << autonModes.name( i );
}
driverStation->sendToDS();
}
/* ====================================== */
}
void Entity::Update(const Camera& camera, float dt)
{
if (mSpinning) {Yaw(dt*movementMult);}
if (mUpDown) {GoUpDown(dt);}
if (mFlipping) {Pitch(dt*movementMult);}
if (mRolling) {Roll(dt*movementMult);}
if (mSideToSide) {GoSideToSide(dt);}
if (mBackAndForth) {GoBackAndForth(dt);}
if (mOrbit) { Yaw(dt*movementMult); Walk(dt*movementMult*100); }
XMVECTOR R = XMLoadFloat3(&mRight);
XMVECTOR U = XMLoadFloat3(&mUp);
XMVECTOR L = XMLoadFloat3(&mLook);
XMVECTOR P = XMLoadFloat3(&mPosition);
// Keep axes orthogonal to each other and of unit length.
L = XMVector3Normalize(L);
U = XMVector3Normalize(XMVector3Cross(L, R));
// U, L already ortho-normal, so no need to normalize cross product.
R = XMVector3Cross(U, L);
// Fill in the world matrix entries.
// float x = XMVectorGetX(XMVector3Dot(P, R));
// float y = XMVectorGetX(XMVector3Dot(P, U));
// float z = XMVectorGetX(XMVector3Dot(P, L));
float x = XMVectorGetX(P);
float y = XMVectorGetY(P);
float z = XMVectorGetZ(P);
XMStoreFloat3(&mRight, R);
XMStoreFloat3(&mUp, U);
XMStoreFloat3(&mLook, L);
mWorld(0, 0) = mRight.x;
mWorld(1, 0) = mRight.y;
mWorld(2, 0) = mRight.z;
mWorld(3, 0) = x;
mWorld(0, 1) = mUp.x;
mWorld(1, 1) = mUp.y;
mWorld(2, 1) = mUp.z;
mWorld(3, 1) = y;
if (reverseLook){
mWorld(0, 2) = -mLook.x;
mWorld(1, 2) = -mLook.y;
mWorld(2, 2) = -mLook.z;
mWorld(3, 2) = z;}
else
{ mWorld(0, 2) = mLook.x;
mWorld(1, 2) = mLook.y;
mWorld(2, 2) = mLook.z;
mWorld(3, 2) = z;}
mWorld(0, 3) = 0.0f;
mWorld(1, 3) = 0.0f;
mWorld(2, 3) = 0.0f;
mWorld(3, 3) = 1.0f;
if (hovering)
{
XMMATRIX M = XMLoadFloat4x4(&mWorld);
XMMATRIX scaling = XMMatrixScaling(1.3f, 1.3f, 1.3f);
XMStoreFloat4x4(&mWorld, scaling * M);
}
//GROWING MOVEMENTS
if (mPulse) { Pulse(dt); }
if (mGrowIn){ GrowIn(dt); }
if (mGrowOut){ GrowOut(dt); }
if (mSquishX || mSquishY || mSquishZ){ Squish(dt);
if (mSquishX){ ScaleX(currProgress); }
if (mSquishY){ ScaleY(currProgress); }
if (mSquishZ){ ScaleZ(currProgress); }}
if (progressBar)
{
ScaleX(currProgress);
}
if (billboard)
{
XMMATRIX M = XMLoadFloat4x4(&mWorld);
XMVECTOR L = XMVector3Normalize(XMVectorSubtract(camera.GetPositionXM(), GetPositionXM()));
XMVECTOR Look = XMLoadFloat3(&mLook);
XMVECTOR angle = XMVector3AngleBetweenNormals(L, Look);
float theAngle = XMVectorGetY(angle);
XMMATRIX rotY;
camera.GetPosition().x < mPosition.x ? rotY = XMMatrixRotationY(-theAngle) : rotY = XMMatrixRotationY(theAngle);
XMStoreFloat4x4(&mWorld, rotY * M);
}
if (goToPos)
{
if (mDistanceLeft <= 0){ goToPos = false; }
}
if (mUseAnimation){ mAnim->Update(dt);}
//update sphere collider
mSphereCollider.Center = mPosition;
if (mUseAAB){ UpdateAAB(); }
if (mUseAABOnce){ UpdateAAB(); mUseAABOnce = false; }
}
