bool AutoAlignCommand::Run(){
if (timer_->Get() < .25)
return false;
if (resetTimer_->Get() > .5) {
autoTurn_->Reset();
resetTimer_->Reset();
}
autoTurn_->UpdateDriver();
if (TimeoutExpired()){
drive_->SetLinearPower(0,0);
}
return TimeoutExpired();
}
bool BridgeBallsCommand::Run(){
if (runIntake_) {
intake_->SetIntakePower(1);
shooter_->SetLinearConveyorPower(-1);
} else {
intake_->SetIntakePower(0);
shooter_->SetLinearConveyorPower(0);
}
// Float intake eventually
if (timer_->Get() > .9) {
intake_->SetIntakePosition(Intake::INTAKE_FLOATING);
} else if (timer_->Get() > .5) {
intake_->SetIntakePosition(Intake::INTAKE_DOWN);
} else {
intake_->SetIntakePosition(Intake::INTAKE_UP);
}
// Return condition
bool ret = TimeoutExpired();
if (ret) {
return ret;
}
return false;
}
void CAIActionAttackTurret::DeactivateAction( CAI* pAI )
{
// Sanity check.
if( !pAI )
{
return;
}
// Kill the looping turning sound.
PlayTurningSound( pAI, false );
// Timeout expired.
// Clear target and stop firing.
if( TimeoutExpired( pAI ) )
{
HOBJECT hTarget = pAI->GetAIBlackBoard()->GetBBTargetObject();
CAIWMFact factQuery;
factQuery.SetFactType( kFact_Character );
factQuery.SetTargetObject( hTarget );
pAI->GetAIWorkingMemory()->ClearWMFact( factQuery );
pAI->ClearState();
g_pAISoundMgr->RequestAISound( pAI->m_hObject, kAIS_SearchFailed, kAISndCat_Always, NULL, 0.f );
}
}
bool QueueBallCommand::Run() {
shooter_->SetLinearConveyorPower(1);
intake_->SetIntakePower(1);
bool done = shooter_->GetBallRange() > 180 || TimeoutExpired();
if (done) {
shooter_->SetLinearConveyorPower(0);
intake_->SetIntakePower(0);
}
return done;
}
bool OldDriveCommand::Run() {
drive_->SetHighGear(true);
if (TimeoutExpired()) {
drive_->SetLinearPower(0, 0);
return true;
}
drive_->SetHighGear(true);
double currLeftDist = drive_->GetLeftEncoderDistance();
double currRightDist = drive_->GetRightEncoderDistance();
double currTime = driveTimer_->Get();
double lVel = (currLeftDist - prevLeftDist_)/(currTime - prevTime_);
double rVel = (currRightDist - prevRightDist_)/(currTime - prevTime_);
prevTime_ = currTime;
prevLeftDist_ = currLeftDist;
prevRightDist_ = currRightDist;
// Get PID feedback and send back to the motors.
double leftPIDOutput = PwmLimit(leftPid_->Update(distanceGoal_, currLeftDist));
double rightPIDOutput = PwmLimit(rightPid_->Update(distanceGoal_, currRightDist));
double angleDiff = drive_->GetGyroAngle() - (startingAngle_ + angleGoal_);
double straightGain = angleDiff * Constants::GetInstance()->straightDriveGain;
double leftPwr = leftPIDOutput - straightGain;
double rightPwr = rightPIDOutput + straightGain;
leftPwr = (leftPwr < -maxSpeed_) ? -maxSpeed_: (leftPwr > maxSpeed_) ? maxSpeed_ : leftPwr;
rightPwr = (rightPwr < -maxSpeed_) ? -maxSpeed_ : (rightPwr > maxSpeed_) ? maxSpeed_ : rightPwr;
leftPwr -= straightGain;
rightPwr += straightGain;
//PidTuner::PushData(currLeftDist, distanceGoal_, 0.0);
drive_->SetLinearPower(leftPwr, rightPwr);
if (fabs(currLeftDist - distanceGoal_ ) < 2 || fabs(currRightDist- distanceGoal_) < 2) {
if (false) {
drive_->SetLinearPower(0,0);
return true;
}
if (fabs(lVel) < 6 && fabs(rVel) < 6) {
brakeTimer_->Start();
}
}
if (brakeTimer_->Get() > .2) {
drive_->SetPizzaWheelDown(resetPizza_);
drive_->SetLinearPower(0,0);
return true;
}
// Indicate that the goal has not yet been reached.
return false;
}
bool CAIActionAttackTurret::ValidateAction( CAI* pAI )
{
// Sanity check.
if( !pAI )
{
return false;
}
// Do not attack if senses are turned off.
if( !pAI->GetAIBlackBoard()->GetBBSensesOn() )
{
return false;
}
// BlindFire when target goes outside of narrow FOV.
bool bBlindFire = true;
if( pAI->GetAIBlackBoard()->GetBBTargetVisibleFromWeapon() &&
TargetInFOV( pAI ) )
{
bBlindFire = false;
}
if( bBlindFire != pAI->GetAIBlackBoard()->GetBBBlindFire() )
{
return false;
}
// Timeout expired.
if( TimeoutExpired( pAI ) )
{
return false;
}
// Default validation.
return super::ValidateAction( pAI );
}
bool DriveCommand::Run() {
if (distanceGoal_ == 0.0 && angleGoal_ == 0.0)
return true;
if (TimeoutExpired()) {
drive_->SetLinearPower(0, 0);
return true;
}
drive_->SetHighGear(false);
double currLeftDist = drive_->GetLeftEncoderDistance() * 0.0254;
double currRightDist = drive_->GetRightEncoderDistance() * 0.0254;
// Convert from inches to meters and degrees to radians
double distGoal = distanceGoal_ * 0.0254;
double maxAcc = maxAcceleration_ * 0.0254;
double maxVel = maxSpeed_ * 0.0254;
double angGoal = angleGoal_ * 0.0174532925;
double maxAlph = maxAlpha_ * 0.0174532925;
double maxOmeg = maxOmega_ * 0.0174532925;
straightFilter_->CalcSystem(distGoal - curX_, curV_, 0.0, maxAcc, maxVel, 0.02);
curA_ = straightFilter_->GetCurrAcc();
curV_ = straightFilter_->GetCurrVel();
curX_ = straightFilter_->GetCurrPos();
turnFilter_->CalcSystem(angGoal - curThet_, curWubl_, 0.0, maxAlph, maxOmeg, 0.02);
curJeez_ = turnFilter_->GetCurrAcc();
curWubl_ = turnFilter_->GetCurrVel();
curThet_ = turnFilter_->GetCurrPos();
static double robotWidth = .827096;
double wubbleuFactor = curWubl_ * robotWidth / 2;
double thetaFactor = curThet_ * robotWidth / 2;
double theta_measured = (currRightDist - currLeftDist) / robotWidth;
double theta_gyro = -(drive_->GetGyroAngle() - startingAngle_) * 0.0174532925 ;
double kI = 0.5;
double KpTurn = 0.3;
double gyroError = (angGoal + turnOffset_ - theta_measured);
if (fabs(angGoal - curThet_) < 0.0001 && fabs(gyroError) < 18.0 * 0.0174532925) {
double KiTurn = Constants::GetInstance()->driveControllerKiTurn;
sumStoppedError_ += gyroError * KiTurn;
} else {
if (!(fabs(angGoal - curThet_) < 0.0001)) {
sumStoppedError_ *= 0.97;
}
}
double doffset = ((theta_measured - turnOffset_) - theta_gyro) * kI;
if (doffset > 0.05) {
doffset = 0.05;
} else if (doffset < -0.05) {
doffset = -0.05;
}
turnOffset_ += doffset;
double turnCompensationOffset = (turnOffset_ + sumStoppedError_ + gyroError * KpTurn) * robotWidth;
flash_matrix(r_, curX_ - turnCompensationOffset / 2.0 - thetaFactor, curV_ - wubbleuFactor, curX_ + turnCompensationOffset / 2.0 + thetaFactor, curV_ + wubbleuFactor);
flash_matrix(y_, currLeftDist, currRightDist);
ssc_->update(r_, y_);
drive_->SetLinearPower(ssc_->U->data[0] / 12.0, ssc_->U->data[1] / 12.0);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line1, "curX:%f", curX_);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line2, "stoppedError:%f", sumStoppedError_);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line3, "thetaFactor:%f", thetaFactor);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line4, "turnOffset:%f", turnOffset_);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line5, "compensation:", turnCompensationOffset);
DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line6, "%f", 0);
DriverStationLCD::GetInstance()->UpdateLCD();
bool angleDone = fabs(drive_->GetGyroAngle() * 0.01714532925 - angGoal) < 0.5;
bool leftDone = fabs((distGoal - angGoal * robotWidth / 2.0) - currLeftDist) < 0.03;
bool rightDone = fabs((distGoal + angGoal * robotWidth / 2.0) - currRightDist) < 0.03;
return TimeoutExpired() || (angleDone && leftDone && rightDone);
}
bool CollisionModel3DImpl::collision(CollisionModel3D* other,
int AccuracyDepth,
int MaxProcessingTime,
float* other_transform)
{
m_ColType=Models;
CollisionModel3DImpl* o=static_cast<CollisionModel3DImpl*>(other);
if (!m_Final) throw Inconsistency();
if (!o->m_Final) throw Inconsistency();
Matrix3D t=( other_transform==NULL ? o->m_Transform : *((Matrix3D*)other_transform) );
if (m_Static) t *= m_InvTransform;
else t *= m_Transform.Inverse();
RotationState rs(t);
if (AccuracyDepth<0) AccuracyDepth=0xFFFFFF;
if (MaxProcessingTime==0) MaxProcessingTime=0xFFFFFF;
DWORD EndTime,BeginTime = GetTickCount();
int num=Max(m_Triangles.size(),o->m_Triangles.size());
int Allocated=Max(64,(num>>4));
std::vector<Check> checks(Allocated);
int queue_idx=1;
Check& c=checks[0];
c.m_first=&m_Root;
c.depth=0;
c.m_second=&o->m_Root;
while (queue_idx>0)
{
if (queue_idx>(Allocated/2)) // enlarge the queue.
{
Check c;
checks.insert(checks.end(),Allocated,c);
Allocated*=2;
}
EndTime=GetTickCount();
if (EndTime >= (BeginTime+MaxProcessingTime)) throw TimeoutExpired();
// @@@ add depth check
//Check c=checks.back();
Check& c=checks[--queue_idx];
BoxTreeNode* first=c.m_first;
BoxTreeNode* second=c.m_second;
assert(first!=NULL);
assert(second!=NULL);
if (first->intersect(*second,rs))
{
int tnum1=first->getTrianglesNumber();
int tnum2=second->getTrianglesNumber();
if (tnum1>0 && tnum2>0)
{
{
for(int i=0;i<tnum2;i++)
{
BoxedTriangle* bt2=second->getTriangle(i);
Triangle tt(Transform(bt2->v1,rs.t),Transform(bt2->v2,rs.t),Transform(bt2->v3,rs.t));
for(int j=0;j<tnum1;j++)
{
BoxedTriangle* bt1=first->getTriangle(j);
if (tt.intersect(*bt1))
{
m_ColTri1=*bt1;
m_iColTri1=getTriangleIndex(bt1);
m_ColTri2=tt;
m_iColTri2=o->getTriangleIndex(bt2);
return true;
}
}
}
}
}
else
if (first->getSonsNumber()==0)
{
BoxTreeNode* s1=second->getSon(0);
BoxTreeNode* s2=second->getSon(1);
assert(s1!=NULL);
assert(s2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=first;
c1.m_second=s1;
Check& c2=checks[queue_idx++];
c2.m_first=first;
c2.m_second=s2;
}
else
if (second->getSonsNumber()==0)
{
BoxTreeNode* f1=first->getSon(0);
BoxTreeNode* f2=first->getSon(1);
assert(f1!=NULL);
assert(f2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=f1;
c1.m_second=second;
Check& c2=checks[queue_idx++];
c2.m_first=f2;
c2.m_second=second;
}
else
{
float v1=first->getVolume();
float v2=second->getVolume();
if (v1>v2)
{
BoxTreeNode* f1=first->getSon(0);
BoxTreeNode* f2=first->getSon(1);
assert(f1!=NULL);
assert(f2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=f1;
c1.m_second=second;
Check& c2=checks[queue_idx++];
c2.m_first=f2;
c2.m_second=second;
}
else
{
BoxTreeNode* s1=second->getSon(0);
BoxTreeNode* s2=second->getSon(1);
assert(s1!=NULL);
assert(s2!=NULL);
Check& c1=checks[queue_idx++];
c1.m_first=first;
c1.m_second=s1;
Check& c2=checks[queue_idx++];
c2.m_first=first;
c2.m_second=s2;
}
}
}
}
return false;
}
bool DelayCommand::Run(){
return TimeoutExpired();
}
