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