TEST(Components, RobotDriveStraightTest)
{
RedBotSpeedController lMotor(0);
RedBotSpeedController rMotor(1);
frc::DifferentialDrive drive(lMotor, rMotor);
MotorDrivePacket* mDrivePacket1;
MotorDrivePacket* mDrivePacket2;
drive.ArcadeDrive(1.0, 0.0);
myPackets.push_back(lMotor.getNextPacket());
mDrivePacket1 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
myPackets.push_back(rMotor.getNextPacket());
mDrivePacket2 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
CHECK(NULL != mDrivePacket1);
CHECK(NULL != mDrivePacket2);
CHECK_EQUAL(255, mDrivePacket1->getSpeed());
CHECK_EQUAL(255, mDrivePacket2->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket1->getDirection());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket2->getDirection());
drive.ArcadeDrive(-1.0, 0.0);
myPackets.push_back(lMotor.getNextPacket());
mDrivePacket1 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
myPackets.push_back(rMotor.getNextPacket());
mDrivePacket2 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
CHECK(NULL != mDrivePacket1);
CHECK(NULL != mDrivePacket2);
CHECK_EQUAL(255, mDrivePacket1->getSpeed());
CHECK_EQUAL(255, mDrivePacket2->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_BACKWARD, mDrivePacket1->getDirection());
CHECK_EQUAL(MotorDrivePacket::DIR_BACKWARD, mDrivePacket2->getDirection());
drive.ArcadeDrive(0.5, 0.0);
myPackets.push_back(lMotor.getNextPacket());
mDrivePacket1 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
myPackets.push_back(rMotor.getNextPacket());
mDrivePacket2 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
CHECK(NULL != mDrivePacket1);
CHECK(NULL != mDrivePacket2);
CHECK_EQUAL(63, mDrivePacket1->getSpeed());
CHECK_EQUAL(63, mDrivePacket2->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket1->getDirection());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket2->getDirection());
drive.ArcadeDrive(1.5, 0.0);
myPackets.push_back(lMotor.getNextPacket());
mDrivePacket1 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
myPackets.push_back(rMotor.getNextPacket());
mDrivePacket2 = dynamic_cast<MotorDrivePacket*>(myPackets.back());
CHECK_EQUAL((MotorDrivePacket*)NULL, mDrivePacket1);
CHECK_EQUAL((MotorDrivePacket*)NULL, mDrivePacket2);
}
Bool BFFLoaderData::Identify(const Filename& name, UChar* probe, Int32 size)
{
if (!probe || size < 4)
return false;
UInt32* p = (UInt32*)probe, v1 = p[0];
lMotor(&v1);
return v1 == 0x42464600;
}
TEST(Components, RobotDriveSimpleTest)
{
RedBotSpeedController lMotor(0);
RedBotSpeedController rMotor(1);
frc::DifferentialDrive drive(lMotor, rMotor);
myPackets.push_back(lMotor.getNextPacket());
CHECK_EQUAL((Packet*)NULL, myPackets.back());
drive.ArcadeDrive(0.0, 0.0);
myPackets.push_back(lMotor.getNextPacket());
CHECK(NULL != myPackets.back());
CHECK(NULL != dynamic_cast<MotorDrivePacket*>(myPackets.back()));
MotorDrivePacket* mDrivePacket1 = static_cast<MotorDrivePacket*>(myPackets.back());
CHECK_EQUAL(0, mDrivePacket1->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket1->getDirection());
myPackets.push_back(rMotor.getNextPacket());
CHECK(NULL != myPackets.back());
CHECK(NULL != dynamic_cast<MotorDrivePacket*>(myPackets.back()));
MotorDrivePacket* mDrivePacket2 = static_cast<MotorDrivePacket*>(myPackets.back());
CHECK_EQUAL(0, mDrivePacket2->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket2->getDirection());
if (mDrivePacket1->getMotor() == MotorDrivePacket::MOTOR_RIGHT)
{
CHECK_EQUAL(MotorDrivePacket::MOTOR_LEFT, mDrivePacket2->getMotor())
}
else
{
CHECK_EQUAL(MotorDrivePacket::MOTOR_RIGHT, mDrivePacket2->getMotor())
}
myPackets.push_back(lMotor.getNextPacket());
CHECK_EQUAL((Packet*)NULL, myPackets.back());
}
TEST(Components, SpeedControllerTest)
{
RedBotSpeedController lMotor(0);
myPackets.push_back(lMotor.getNextPacket());
CHECK_EQUAL((Packet*)NULL, myPackets.back());
lMotor.Set(1.0);
myPackets.push_back(lMotor.getNextPacket());
CHECK(NULL != myPackets.back());
CHECK(NULL != dynamic_cast<MotorDrivePacket*>(myPackets.back()));
MotorDrivePacket* mDrivePacket1 = static_cast<MotorDrivePacket*>(myPackets.back());
CHECK_EQUAL(255, mDrivePacket1->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_FORWARD, mDrivePacket1->getDirection());
CHECK_EQUAL(MotorDrivePacket::MOTOR_LEFT, mDrivePacket1->getMotor());
RedBotSpeedController rMotor(1);
myPackets.push_back(rMotor.getNextPacket());
CHECK_EQUAL((Packet*)NULL, myPackets.back());
rMotor.Set(-0.5);
myPackets.push_back(rMotor.getNextPacket());
CHECK(NULL != myPackets.back());
CHECK(NULL != dynamic_cast<MotorDrivePacket*>(myPackets.back()));
MotorDrivePacket* mDrivePacket2 = static_cast<MotorDrivePacket*>(myPackets.back());
CHECK_EQUAL(127, mDrivePacket2->getSpeed());
CHECK_EQUAL(MotorDrivePacket::DIR_BACKWARD, mDrivePacket2->getDirection());
CHECK_EQUAL(MotorDrivePacket::MOTOR_RIGHT, mDrivePacket2->getMotor());
}
TEST(Components, CurveDriveTestLinear)
{
RedBotSpeedController lMotor(0);
RedBotSpeedController rMotor(1);
frc::DifferentialDrive drive(lMotor, rMotor);
// Stationary
CheckDrive(
drive,
myPackets,
0.0,
0.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
0,
MotorDrivePacket::DIR_FORWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
0,
MotorDrivePacket::DIR_FORWARD
),
false
);
// Drive forward
CheckDrive(
drive,
myPackets,
1.0,
0.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
255,
MotorDrivePacket::DIR_FORWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
255,
MotorDrivePacket::DIR_FORWARD
),
false
);
// Drive forward at half speed
CheckDrive(
drive,
myPackets,
0.5,
0.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
127,
MotorDrivePacket::DIR_FORWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
127,
MotorDrivePacket::DIR_FORWARD
),
false
);
// Drive backward
CheckDrive(
drive,
myPackets,
-1.0,
0.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
255,
MotorDrivePacket::DIR_BACKWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
255,
MotorDrivePacket::DIR_BACKWARD
),
false
);
// Drive backward at half speed
CheckDrive(
drive,
myPackets,
-0.5,
0.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
127,
MotorDrivePacket::DIR_BACKWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
127,
MotorDrivePacket::DIR_BACKWARD
),
false
);
// Pivot right
CheckDrive(
drive,
myPackets,
0.0,
1.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
255,
MotorDrivePacket::DIR_BACKWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
255,
MotorDrivePacket::DIR_FORWARD
),
false
);
// Pivot right at half speed
CheckDrive(
drive,
myPackets,
0.0,
0.5,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
127,
MotorDrivePacket::DIR_BACKWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
127,
MotorDrivePacket::DIR_FORWARD
),
false
);
// Pivot left
CheckDrive(
drive,
myPackets,
0.0,
-1.0,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
255,
MotorDrivePacket::DIR_FORWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
255,
MotorDrivePacket::DIR_BACKWARD
),
false
);
// Pivot left at half speed
CheckDrive(
drive,
myPackets,
0.0,
-0.5,
MotorDrivePacket(
MotorDrivePacket::MOTOR_RIGHT,
127,
MotorDrivePacket::DIR_FORWARD
),
MotorDrivePacket(
MotorDrivePacket::MOTOR_LEFT,
127,
MotorDrivePacket::DIR_BACKWARD
),
false
);
}
