bool GLCgShader::SetConstant(const str_type::string& name, const float x, const float y)
{
return SetConstant(name, math::Vector2(x, y));
}
bool GLCgShader::SetConstant(const str_type::string& name, const Color& dw)
{
math::Vector4 v;
v.SetColor(dw);
return SetConstant(name, v);
}
bool GLCgShader::SetConstant(const str_type::string& name, const float x, const float y, const float z, const float w)
{
return SetConstant(name, math::Vector4(x, y, z, w));
}
bool GLES2Shader::SetConstant(const str_type::string& name, const float x, const float y, const float z)
{
return SetConstant(name, math::Vector3(x,y,z));
}
bool GLES2Shader::SetConstant(const std::size_t nameHash, const str_type::string& name, const Color& dw)
{
math::Vector4 v;
v.SetColor(dw);
return SetConstant(nameHash, name, v);
}
inline void SetConstant(ShaderStage stage, uint32 slot, const ConstantBuffer<Type>& buffer)
{
SetConstant(stage, slot, buffer.base(), buffer.getPtr(), sizeof(Type) / sizeof(__m128));
}
void Shoot(uint16 diff)
{
switch (shooterState)
{
case ssAim: // controls if robot is to shoot or to move back
StartShooterControl();
if (beginBackIR == -1)
beginBackIR = GetRearAveragedSum();
// try to load more balls
if (!IsLoaded())
{
StopMoving();
GuardUp();
if (UpdateTimer(diff, loadTimer, cLoadTimer)) // ran out of time to load new ball - have no balls
shooterState = ssAlignBack;
sDebugPhase = "Loading";
}
else
{
loadTimer = cLoadTimer;
if (GetTargAveragedSum() < cTargOPMinThresh) //need to coarse adj
{
if (doMidAdjust)
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
/*
if (PID::OPDriveAdjustCompleted(sCalibratedBackIRVal * cIRFalloffCenter))
{
StopMoving();
doMidAdjust = false;
midAjustComplete = true;
}
PID::OPDriveUpdate(diff, sCalibratedBackIRVal * cIRFalloffCenter);
*/
}
else
{
if (aimState == asCW)
{
//RotateCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 + cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 - cAimSpeed);
}
else if (aimState == asCCW)
{
//RotateCCW(cAimSpeed);
SetMotorSpeed(tLeftDriveMotor, 100 - cAimSpeed);
SetMotorSpeed(tRightDriveMotor, 100 + cAimSpeed);
}
}
if (coarseMaxThreshPassed) //coarse adj control
{
if (GetRearAveragedSum() < beginBackIR * (aimState == asCW ? cTurnIRFalloffCW : cTurnIRFalloffCCW))
{
//switch
aimState = (aimState == asCW ? asCCW : asCW);
coarseMaxThreshPassed = false;
midAjustComplete = false;
doMidAdjust = false;
}
sDebugPhase = "Coarse 2";
}
else
{
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
{
if (!midAjustComplete)
doMidAdjust = true;
coarseMaxThreshPassed = true;
}
sDebugPhase = "Coarse 1";
}
}
else // fine
{
midAjustComplete = false;
doMidAdjust = false;
if (GetRearAveragedSum() >= beginBackIR * 0.75f)
coarseMaxThreshPassed = true;
if (PID::TargOPAdjustCompleted())
{
StopMoving();
shooterState = ssFire;
//sAimTmr = cMaxAimDurationTimer;
}
PID::OPUpdate(true, diff, PID::OPPGainFront, 0, 0);
if (IsLoaded())
if (UpdateTimer(diff, sAimTmr, cMaxAimDurationTimer))
{
StopMoving();
shooterState = ssFire;
}
sDebugPhase = "Fine Adj";
}
}
break;
case ssFire:
StopMoving();
if (shooterReady)
GuardDown();
if (!IsLoaded())
if (UpdateTimer(diff, sShootTmr, cShootTimer))
{
sAimTmr = cMaxAimDurationTimer;
shooterState = ssAim;
}
sDebugPhase = "Fire";
break;
case ssAlignBack:
StopShooterControl();
if (PID::RearOPAdjustCompleted())
{
StopMoving();
shooterState = ssAim;
robotState = rsNavigateBack;
beginBackIR = -1;
sDebugPhase = "******************";
}
PID::OPUpdate(false, diff, PID::OPPGainBack, 0, 0);
sDebugPhase = "Align Back";
break;
}
if (UpdateTimer(diff, LCDTimer, LCD_UPDATE_TIMER))
{
LCD_PRINTNEW("S:");
LCD.print(sDebugPhase);
LCD.setCursor(0, 1);
switch (shooterState)
{
case ssAim:
LCD.print("A ");
LCD.print(GetAveragedLeftTargOPReading());
LCD.print(" ");
LCD.print(GetAveragedRightTargOPReading());
LCD.print(" ");
LCD.print(beginBackIR * cTurnIRFalloffCW);
LCD.print(" ");
LCD.print(GetRearAveragedSum());
break;
case ssFire:
LCD.print("SPD ");
LCD.print(shooterTopFreq);
LCD.print(" ");
LCD.print(shooterBotFreq);
LCD.print(" ");
break;
case ssAlignBack:
LCD.print("B ALGN ");
LCD.print(GetLeftRearOPReading());
LCD.print(" ");
LCD.print(GetRightRearOPReading());
LCD.print(" ");
break;
}
}
if (stopbutton())
{
StopShooterControl();
shooterTopFreq = SetConstant("Shooter Top Freq", cShooterTopFreq, 1/2.0f);
shooterBotFreq = SetConstant("Shooter Bot Freq", cShooterBotFreq, 1/2.0f);
SetConstant("Right OP Gain", cFrontRightOPGain, 1/20.0f);
StartShooterControl();
}
}
bool Effect::SetConstant(const Shader &i_shader, const ConstantHandle &i_constant, const Color& i_val)
{
return SetConstant(i_shader, i_constant, reinterpret_cast<const float*>(&i_val), 4);
}
