task main()
{
motor[intakeLeft] = -127;
motor[intakeRight] = -127;
zeroEncoders();
while(rightEncoder() + leftEncoder() < (1500)*2)
{
motor[driveLeftBack] = 100;
motor[driveLeftFront] = 100;
motor[driveRightBack] = 100;
motor[driveRightFront] = 100;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
motor[intakeLeft] = 0; // Stop Arm
motor[intakeRight] = 0;
//zeroEncoders();
//driveForwardInches(1,100);
////wait10Msec(10);
//pivotTurnLeft(100);
////wait10Msec(10);
//pivotTurnRight(100);
////wait10Msec(10);
//driveBackwardInches(12,100);
}
void driveForwardInches(float inches, int speed)
{
zeroEncoders();
while(rightEncoder() + leftEncoder() < (inches * ticksPerInch * 2))
{
motor[driveLeftBack] = speed;
motor[driveLeftFront] = speed;
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
void pivotTurnLeft(int speed)
{
zeroEncoders();
while(rightEncoder() - leftEncoder() < (inchesFor90Turn * ticksPerInch))
{ // Turn left while encoders are less than target value
motor[driveLeftBack] = -speed;
motor[driveLeftFront] = -speed;
motor[driveRightBack] = speed;
motor[driveRightFront] = speed;
}
motor[driveLeftBack] = 0; // Stop
motor[driveLeftFront] = 0;
motor[driveRightBack] = 0;
motor[driveRightFront] = 0;
}
TEST(Components, EncoderTest)
{
RedBotEncoder leftEncoder(false);
RedBotEncoder rightEncoder(true);
Packet* packet0 = leftEncoder.getNextPacket();
myPackets.push_back(packet0);
CHECK(packet0 != NULL);
CHECK(NULL != dynamic_cast<EncoderInputPacket*>(packet0));
EncoderInputPacket* leftInputPacket = static_cast<EncoderInputPacket*>(packet0);
CHECK_FALSE(leftInputPacket->isRight());
Packet* packet1 = leftEncoder.getNextPacket();
myPackets.push_back(packet1);
CHECK_EQUAL((Packet*)NULL, packet1);
Packet* packet2 = rightEncoder.getNextPacket();
myPackets.push_back(packet2);
CHECK(packet2 != NULL);
CHECK(NULL != dynamic_cast<EncoderInputPacket*>(packet2));
EncoderInputPacket* rightInputPacket = static_cast<EncoderInputPacket*>(packet2);
CHECK(rightInputPacket->isRight());
CHECK(leftEncoder.processPacket(EncoderCountPacket(false, 345)));
CHECK_EQUAL(345, leftEncoder.Get());
CHECK(leftEncoder.processPacket(EncoderCountPacket(false, 367)));
CHECK_EQUAL(367, leftEncoder.Get());
CHECK_FALSE(leftEncoder.processPacket(EncoderCountPacket(true, 203)));
CHECK_EQUAL(367, leftEncoder.Get());
CHECK_EQUAL(0, rightEncoder.Get());
CHECK(rightEncoder.processPacket(EncoderCountPacket(true, -20)));
CHECK_EQUAL(-20, rightEncoder.Get());
CHECK_FALSE(rightEncoder.processPacket(AcknowledgePacket()));
leftEncoder.Reset();
Packet* packet3 = leftEncoder.getNextPacket();
myPackets.push_back(packet3);
CHECK(packet3 != NULL);
CHECK(NULL != dynamic_cast<EncoderClearPacket*>(packet3));
EncoderClearPacket* leftClearPacket = dynamic_cast<EncoderClearPacket*>(packet3);
CHECK_FALSE(leftClearPacket->isRight());
Packet* packet4 = leftEncoder.getNextPacket();
myPackets.push_back(packet4);
CHECK(packet4 != NULL);
CHECK(NULL != dynamic_cast<EncoderInputPacket*>(packet4));
rightEncoder.Reset();
Packet* packet5 = rightEncoder.getNextPacket();
myPackets.push_back(packet5);
CHECK(packet5 != NULL);
CHECK(NULL != dynamic_cast<EncoderClearPacket*>(packet5));
EncoderClearPacket* rightClearPacket = dynamic_cast<EncoderClearPacket*>(packet5);
CHECK(rightClearPacket->isRight());
}
