void XboxTankDriveProfile::OperatorControl(void){
m_drive->SetSafetyEnabled(true);
while (this->IsOperatorControl())
{
float leftY = Cutoff(xbox->GetAxis(xbox->LeftY));
float rightY = Cutoff(xbox->GetAxis(xbox->RightY));
m_drive->TankDrive(leftY, rightY);
if (xbox->GetLeftBumperButton()) {
m_collector->PistonPull();
} else if (xbox->GetRightBumperButton()) {
m_collector->PistonPush();
}
if(xbox->GetAButton()){
m_collector->SpinInwards();
}
if(xbox->GetBButton()){
m_collector->SpinOutwards();
}
if(xbox->GetYButton()){
m_collector->SpinStop();
}
/*
float bumper = Cutoff(xbox->GetAxis(xbox->Bumper));
if(bumper >= 0.4){
m_shooter->shootWithArm();
}*/
Wait(0.005); // wait for a motor update time
}
}
void
OGL_Light::BindIntoBuffer(GLuint _buffer, unsigned int _index)
{
glBindBuffer(GL_UNIFORM_BUFFER, _buffer);
GLint baseOffset = m_ComputeLightOffset(_index);
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, 3 * sizeof(GLfloat), m_Ia.ToArray().get());
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + (4 * sizeof(GLfloat)), 3 * sizeof(GLfloat), m_Id.ToArray().get());
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + 2 * (4 * sizeof(GLfloat)), 3 * sizeof(GLfloat), m_Is.ToArray().get());
baseOffset += 3 * 4 * sizeof(GLfloat);
switch (m_Type)
{
case DIRECTIONAL:
{
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, 3 * sizeof(GLfloat), m_Direction.ToArray().get());
}
break;
case POINT:
{
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, 3 * sizeof(GLfloat), m_Position.ToArray().get());
baseOffset += 4 * sizeof(GLfloat);
GLfloat constant = Constant(), linear = Linear(), quadratic = Quadratic();
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, sizeof(GLfloat), &constant);
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + sizeof(GLfloat), sizeof(GLfloat), &linear);
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + 2 * sizeof(GLfloat), sizeof(GLfloat), &quadratic);
}
break;
case SPOT:
{
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, 3 * sizeof(GLfloat), m_Position.ToArray().get());
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + (4 * sizeof(GLfloat)), 3 * sizeof(GLfloat), m_Direction.ToArray().get());
baseOffset += 2 * (4 * sizeof(GLfloat));
GLfloat innerCutoff = InnerCutoff(), cutoff = Cutoff();
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset, sizeof(GLfloat), &innerCutoff);
glBufferSubData(GL_UNIFORM_BUFFER, baseOffset + sizeof(GLfloat), sizeof(GLfloat), &cutoff);
}
break;
default:
break;
}
glBindBuffer(GL_UNIFORM_BUFFER, 0);
}
void
OGL_Light::BindIntoShader(OGL_Shader* _shader, unsigned int _index)
{
std::string typeName = "";
std::string index = std::to_string(_index);
switch (m_Type)
{
case DIRECTIONAL:
typeName = "Directional";
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Direction"), 1, m_Direction.ToStdVec().data());
break;
case POINT:
typeName = "Point";
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Position"), 1, m_Position.ToStdVec().data());
// These values describe a light with a range of 20 units
glUniform1f(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Constant"), Constant());
glUniform1f(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Linear"), Linear());
glUniform1f(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Quadratic"), Quadratic());
break;
case SPOT:
typeName = "Spot";
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Position"), 1, m_Position.ToStdVec().data());
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Direction"), 1, m_Direction.ToStdVec().data());
glUniform1f(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].OuterCutoff"), Cutoff());
glUniform1f(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].InnerCutoff"), InnerCutoff());
break;
default:
return;
}
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Ia"), 1, m_Ia.ToStdVec().data());
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Id"), 1, m_Id.ToStdVec().data());
glUniform3fv(_shader->GetUniform("u_" + typeName + "Lights[" + index + "].Is"), 1, m_Is.ToStdVec().data());
}
void MainRobot::OperatorControl()
{
int operatorControlLifetime = 0;
while (IsOperatorControl()) {
SmartDashboard::PutNumber("Operator Lifetime", ++operatorControlLifetime);
if (CONTROLLER == XBOX) {
// DRIVING
// ----------------------------------------------------------------------
if (DRIVING == TANK) {
// (for the two code lines below) The minus in front makes the robot drive in the
// direction of the collector (when joysticks are forward)
float leftY = -Cutoff(driveController->GetAxis(driveController->LeftY));
float rightY = -Cutoff(driveController->GetAxis(driveController->RightY));
// Drive reversal when right trigger down (go opposite direction)
if (driveController->GetTriggerAxis() <= -0.4) {
leftY = -leftY;
rightY = -rightY;
}
m_drive->TankDrive(leftY, rightY);
} else if (DRIVING == ARCADE) {
float arcadeY = -Cutoff(driveController->GetLeftYAxis());
float arcadeX = -Cutoff(driveController->GetLeftXAxis());
if (arcadeY == 0 && arcadeX == 0) {
arcadeY = -Cutoff(driveController->GetRightYAxis());
arcadeX = -Cutoff(driveController->GetRightXAxis());
}
// Drive reversal when right trigger down (go opposite direction)
if (driveController->GetTriggerAxis() <= -0.4) {
arcadeY = -arcadeY;
//arcadeX = -arcadeX;
}
m_drive->ArcadeDrive(arcadeY, arcadeX);
} else if (DRIVING == ARCADE2) {
float arcadeY = -Cutoff(driveController->GetLeftYAxis());
float arcadeX = -Cutoff(driveController->GetRightXAxis());
// Drive reversal when right trigger down (go opposite direction)
if (driveController->GetTriggerAxis() <= -0.4) {
arcadeY = -arcadeY;
//arcadeX = -arcadeX; // May need to be uncommented?
}
m_drive->ArcadeDrive(arcadeY, arcadeX);
}
// PISTON BUTTONS (FOR COLLECTOR)
// ----------------------------------------------------------------------
if (shootController->GetLeftBumperButton()) {
SmartDashboard::PutBoolean("ShootController LB", true);
m_collector->PistonPull();
} else if (shootController->GetRightBumperButton()) {
SmartDashboard::PutBoolean("ShootController RB", true);
m_collector->BringArmDown();
if (m_shooter->GetLimitSwitch()) {
m_collector->BringArmDown();
}
} else {
SmartDashboard::PutBoolean("ShootController LB", false);
SmartDashboard::PutBoolean("ShootController RB", false);
}
// SPIN BUTTONS (FOR COLLECTOR)
// ----------------------------------------------------------------------
if (shootController->GetXButton()){
m_collector->SpinInwards();
} else if (shootController->GetYButton()){
m_collector->SpinOutwards();
} else {
m_collector->SpinStop();
}
// PASSING
// ----------------------------------------------------------------------
if(driveController->GetRightBumperButton()) {
m_shooter->ShooterPass();
}
// SHOOTER ARM LIFT / LOWER SLOWLY
// ----------------------------------------------------------------------
// The Set() values below (-.15 & .15) indicate the power
// to the motor (15%) and are very important to understand
// before changing. It controls the speed of the shooter
// arm while the button is pressed. If the values are too high,
// you run the risk of wrapping the arm into the robot
// (or around the robot).
if (shootController->GetAButton()) {
m_shooter->Set(-.15);
} else if (shootController->GetBButton()) {
m_shooter->Set(.15);
} else {
m_shooter->Set(0);
}
// SHOOTING
// ----------------------------------------------------------------------
float trigger = shootController->GetTriggerAxis();
if (trigger <= -0.4){ // High shot
if (!isShooting) {
m_compressor->Stop();
isShooting = true;
m_shooter->ShootWithArm(false, 1);
m_compressor->Start();
}
} else if (trigger >= 0.4){ // Low shot
if (!isShooting) {
m_compressor->Stop();
isShooting = true;
m_shooter->ShootWithArm(true, 0.9);
m_compressor->Start();
}
} else {
isShooting = false;
}
} else if (CONTROLLER == JOYSTICKS) { // Xbox is a higher priority, do this later/never
if (DRIVING == TANK) {
float leftY = -Cutoff(m_leftStick->GetY());
float rightY = -Cutoff(m_leftStick->GetX());
m_drive->TankDrive(leftY, rightY);
} else if (DRIVING == ARCADE) {
float arcadeY = -Cutoff(m_leftStick->GetY());
float arcadeX = -Cutoff(m_leftStick->GetX());
if (arcadeY == 0 && arcadeX == 0) {
arcadeY = -Cutoff(m_rightStick->GetY());
arcadeX = -Cutoff(m_rightStick->GetX());
}
m_drive->ArcadeDrive(arcadeY, arcadeX);
}
}
SmartDashboard::PutBoolean("Pistons extended", m_collector->IsExtended ());
SmartDashboard::PutBoolean("shooter limit switch", m_shooter->GetLimitSwitch());
Wait(0.005); // wait for a motor update time
}
}
int pickParent(int prevChosen, int nInDeme )
{
int i, j, n, count, dadi, momi, *ipt;
int aliveOnes[nInDeme], nAlive=0;
//double fitnessArray[TOTAL_N];
short int *offspringGenotyes, *sipt;
int *offspringDemeLocations;
offspringGenotyes = (short int *) malloc( (sizeof(short int) * 2 * totalSitesInGenome * TOTAL_N) );
offspringDemeLocations = (int *) malloc( (sizeof(int) * TOTAL_N) );
// momi = *(ipt + momi); // extract the right index from the overall index array; "momi" and "dadi" are "local" indexes in the consecutive fitness array
// dadi = *(ipt + dadi);
sipt = offspringGenotyes;
int luckyOne;
if ( MODEL_TYPE == MODEL_TYPE_NEUTRAL_ONLY ) {
do {
luckyOne = randI() % nInDeme;
} while ( luckyOne == prevChosen );
}
else if ( MODEL_TYPE == MODEL_TYPE_SELECTION ) {
//fprintf(stderr, "\nError in pickParent()! Procedures not in place for MODEL_TYPE_SELECTION\n");
//exit(-1);
int s_total=0;
int s_sum;
int individual=0;
int i=0, j=0;
double viabilityArray[nInDeme];
for (i; i<nSITES_PER_WINDOW; i++){
s_sum += selectionCoefficients[i];
}
for(individual = 0; individual < nInDeme; individual++){
s_total=0, i=0;
for (j; j<nSITES_PER_WINDOW; j++)
{
for (i=0;i<2;i++){
if (*sipt = 1){
s_total += selectionCoefficients[j];
}
sipt++;
}
} //builds individual selection total for comparison to overall total
viabilityArray[individual]=s_total;
}
cutoff = Cutoff(viabilityArray);
i=0;
int deadTracker;
for (i; i<nInDeme; i++){
if (viabilityArray[i]>cutoff){
//aliveTracker[deadTracker] = i;
deadTracker++;
} //Scans through to find values less than cutoff; make 0 or 1 in living ones array instead?
}
luckyOne = randI() % deadTracker; //not nInDeme! Choose from array of live/dead
//aliveTracker currently has nInDeme elements; is the current modulus operation OK??
}
return aliveOnes[luckyOne];
}
double ALNAPI CutoffEvalMinMax(const ALNNODE* pNode, const ALN* pALN,
const double* adblX, CEvalCutoff cutoff,
ALNNODE** ppActiveLFN)
{
ASSERT(NODE_ISMINMAX(pNode));
// set first child
const ALNNODE* pChild0;
if (MINMAX_EVAL(pNode))
pChild0 = MINMAX_EVAL(pNode);
else
pChild0 = MINMAX_LEFT(pNode);
// set next child
const ALNNODE* pChild1;
if (pChild0 == MINMAX_LEFT(pNode))
pChild1 = MINMAX_RIGHT(pNode);
else
pChild1 = MINMAX_LEFT(pNode);
// get reference to region for this node
const ALNREGION& region = pALN->aRegions[NODE_REGION(pNode)];
double dblDist, dblRespActive;
if (region.dbl4SE > 0.0) // smoothing is used
{
// I think this step is wrong. Smoothing makes a max function greater and a min function smaller.
// So I am commenting it out to see what happens. WWA
/*
// loosen cutoff constraint for children
if (MINMAX_ISMAX(pNode) && cutoff.bMax)
cutoff.dblMax -= region.dbl4SE;
else if (MINMAX_ISMIN(pNode) && cutoff.bMin)
cutoff.dblMin += region.dbl4SE;
*/
// eval first child
ALNNODE* pActiveLFN0;
double dbl0 = CutoffEval(pChild0, pALN, adblX, cutoff, &pActiveLFN0);
// see if we can cutoff...
if (Cutoff(dbl0, pNode, cutoff, region.dbl4SE))
{
*ppActiveLFN = pActiveLFN0;
return dbl0;
}
// eval second child
ALNNODE* pActiveLFN1;
double dbl1 = CutoffEval(pChild1, pALN, adblX, cutoff, &pActiveLFN1);
// calc active child, active child response, and distance
int nActive = CalcActiveChild(dblRespActive,
dblDist,
dbl0, dbl1, pNode,
region.dblSmoothEpsilon,
region.dbl4SE, region.dblOV16SE);
if (nActive == 0)
{
*ppActiveLFN = pActiveLFN0;
}
else
{
*ppActiveLFN = pActiveLFN1;
}
}
else // dbl4SE == 0, i.e. there is zero smoothing
{
// eval first child
ALNNODE* pActiveLFN0;
double dbl0 = CutoffEval(pChild0, pALN, adblX, cutoff, &pActiveLFN0);
// see if we can cutoff...
if (Cutoff(dbl0, pNode, cutoff, 0.0))
{
*ppActiveLFN = pActiveLFN0;
return dbl0;
}
// eval second child
ALNNODE* pActiveLFN1;
double dbl1 = CutoffEval(pChild1, pALN, adblX, cutoff, &pActiveLFN1);
// calc active child and distance without using CalcActiveChild()
if((MINMAX_ISMAX(pNode) > 0) == (dbl1 > dbl0)) // int MINMAX_ISMAX is used as a bit-vector!
{
*ppActiveLFN = pActiveLFN1;
dblDist = dbl1;
}
else
{
*ppActiveLFN = pActiveLFN0;
dblDist = dbl0;
}
}
return dblDist;
}
