TEST_F(TestMemFnPointerSample, All) {
Counter counter;
CountLater countLater;
int expected = 0;
EXPECT_EQ(expected, counter.Get());
counter.Increment();
++expected;
EXPECT_EQ(expected, counter.Get());
counter.Decrement();
counter.Decrement();
expected -= 2;
EXPECT_EQ(expected, counter.Get());
countLater.Execute();
EXPECT_EQ(expected, counter.Get());
countLater.ExecuteLater(&counter, &Counter::Decrement);
EXPECT_EQ(expected, counter.Get());
--expected;
for(int i=0; i<2; ++i) {
countLater.Execute();
EXPECT_EQ(expected, counter.Get());
}
countLater.ExecuteLater(&counter, &Counter::Increment);
EXPECT_EQ(expected, counter.Get());
countLater.ExecuteLater(&counter, &Counter::Increment);
++expected;
EXPECT_EQ(expected, counter.Get());
}
/**
* Read the current counter value.
* Read the value at this instant. It may still be running, so it reflects the current value. Next
* time it is read, it might have a different value.
* @param channel The channel of the digital input used with this counter
*/
INT32 GetCounter(UINT32 channel)
{
Counter *counter = AllocateCounter(channel);
if (counter != NULL)
return counter->Get();
else
return 0;
}
/**
* Read the current counter value.
* Read the value at this instant. It may still be running, so it reflects the current value. Next
* time it is read, it might have a different value.
* @param slot The slot the digital module is plugged into
* @param channel The channel of the digital input used with this counter
*/
INT32 GetCounter(UINT8 moduleNumber, UINT32 channel)
{
Counter *counter = AllocateCounter(moduleNumber, channel);
if (counter != NULL)
return counter->Get();
else
return 0;
}
void UpdateGearCount ()
//this function turns the relative gear tooth counter into an absolute counter
{
if (curForkSetSpeed < 0)
forkDirection = inwards;
else //curForkSetSpeed > 0; this function should not be called when curForkSpeed = 0, should on be called when the forks are moving;
forkDirection = outwards;
curGearToothCount = gearToothCounter->Get();
absGearToothCount += forkDirection*(curGearToothCount-lastGearToothCount);
lastGearToothCount = curGearToothCount;
}
void TeleopPeriodic()
{
char myString [STAT_STR_LEN];
if (initialZeroing) // moving to the inner limit
{
sprintf(myString, "initZero ip\n"); //in progress
SmartDashboard::PutString("DB/String 0", myString);
if (GetForkLimitSwitchMin())
{
SetForkMotor(MOTOR_SPEED_STOP);
gearToothCounter->Reset();
initialZeroing = false;
sprintf(myString, "initZero comp\n"); //complete
SmartDashboard::PutString("DB/String 0", myString);
}
}
else //manual control
{
//motor control
if (joystick->GetRawButton(BUT_JS_OUT) && !GetForkLimitSwitchMax()) // move outwards
SetForkMotor(MOTOR_SPEED_GO);
else if(joystick->GetRawButton(BUT_JS_IN) && !GetForkLimitSwitchMin()) // move inwards
SetForkMotor(-MOTOR_SPEED_GO);
else
SetForkMotor(MOTOR_SPEED_STOP); //stop
//counter control
if (joystick->GetRawButton(BUT_JS_RES_GTC)) // reset the gear tooth counter
gearToothCounter->Reset();
}
//status
sprintf(myString, "jsOut %d\n", joystick->GetRawButton(BUT_JS_OUT));
SmartDashboard::PutString("DB/String 1", myString);
sprintf(myString, "jsIn %d\n", joystick->GetRawButton(BUT_JS_IN));
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "jsResGtc %d\n", joystick->GetRawButton(BUT_JS_RES_GTC));
SmartDashboard::PutString("DB/String 3", myString);
sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
SmartDashboard::PutString("DB/String 4", myString);
sprintf(myString, "gtc #: %d\n", gearToothCounter->Get()); //gtc count
SmartDashboard::PutString("DB/String 5", myString);
}
void TeleopPeriodic()
{
char myString[64];
float y;
// get the joystick position and filter the deadband
y = -joystick->GetY();
if (y > -0.1 && y < 0.1)
y = 0;
sprintf(myString, "joystick setting: %f\n", y);
SmartDashboard::PutString("DB/String 1", myString);
motor->Set(y, 0); // set the speed of the motor
sprintf(myString, "gear count: %d\n", gearToothCounter->Get());
SmartDashboard::PutString("DB/String 2", myString);
sprintf(myString, "gear stopped: %d\n", gearToothCounter->GetStopped());
SmartDashboard::PutString("DB/String 3", myString);
// sprintf(myString, "In val: %d\n", toothInput->GetValue());
// SmartDashboard::PutString("DB/String 2", myString);
}
