JankyAutoSequencer::JankyAutoSequencer(frc::RobotDrive*drive, frc::ADXRS450_Gyro*gyro, SensorCollection*leftEncoder, SensorCollection*rightEncoder, WPI_TalonSRX*leftmotor, WPI_TalonSRX*rightmotor, InAndOut*inAndOut, UpAndDown*upAndDown) {
//JankyAutoSequencer::JankyAutoSequencer(RobotDrive*drive, frc::ADXRS450_Gyro*gyro, Encoder*encoder) {
for(int i = 0; i<MAX_NAMES; i++){
entries[i]=NULL;
}
turnLeft90 = new TurnSegment(gyro, drive, -90.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
turnRight90 = new TurnSegment(gyro, drive, 90.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
turnLeft45 = new TurnSegment(gyro, drive, -45.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
turnRight45 = new TurnSegment(gyro, drive, 45.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
turnLeft30 = new TurnSegment(gyro, drive, -30.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
turnRight30 = new TurnSegment(gyro, drive, 30.0, TURN_SPEED, turn_kP, turn_kI, turn_kD);
drive6Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 38, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive10Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 50, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive40Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 2, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive50Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 62, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive52Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 69, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive60Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 48, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive72Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 84, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive120Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 160, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive144Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 194, FAST_DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive162Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 163, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive210Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 227, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive240Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 268, DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
drive260Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, 260, SCALE_DRIVE_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
cubeUp = new ::CubeUp(inAndOut, upAndDown, 'l');
cubeUpScale = new ::CubeUp(inAndOut, upAndDown, 'h');
releaseCube = new ::ReleaseCube(drive, inAndOut, upAndDown, 'l');
visionSegment = new ::VisionSegment(drive, VISION_DRIVE_SPEED, drive_kP, drive_kI, drive_kD); //might need negative p, i, d since backwards
driveBack10Inches = new DriveSegment(gyro, drive, leftEncoder, rightEncoder, leftmotor, rightmotor, -25, DRIVE_BACK_SPEED, drive_kP, drive_kI, drive_kD, inAndOut);
SetMachineName("JankyAutoSequencer");
JankyStateMachine::SetName(Rest, "Rest");
SetName(TurnLeft90, "Turn Left 90 Degrees", turnLeft90);
SetName(TurnRight90, "Turn Right 90 Degrees", turnRight90);
SetName(TurnLeft45, "Turn Left 45 Degrees", turnLeft45);
SetName(TurnRight45, "Turn Right 45 Degrees", turnRight45);
SetName(TurnLeft30, "Turn Left 30 Degrees", turnLeft30);
SetName(TurnRight30, "Turn Right 30 Degrees", turnRight30);
SetName(Drive6Inches, "Drive straight 6 inches", drive6Inches);
SetName(Drive10Inches, "Drive straight 10 inches", drive10Inches);
SetName(Drive40Inches, "Drive straight 40 inches", drive40Inches);
SetName(Drive50Inches, "Drive straight 50 inches", drive50Inches);
SetName(Drive52Inches, "Drive straight 52 inches", drive50Inches);
SetName(Drive60Inches, "Drive straight 60 inches", drive60Inches);
SetName(Drive72Inches, "Drive straight 72 inches", drive72Inches);
SetName(Drive120Inches, "Drive straight 120 inches", drive120Inches);
SetName(Drive144Inches, "Drive straight 144 inches", drive144Inches);
SetName(Drive162Inches, "Drive straight 162 inches", drive162Inches);
SetName(Drive210Inches, "Drive straight 210 inches", drive210Inches);
SetName(Drive240Inches, "Drive straight 240 inches", drive240Inches);
SetName(Drive260Inches, "Drive straight 260 inches", drive260Inches);
SetName(CubeUp, "Lift cube to switch height", cubeUp);
SetName(CubeUpScale, "Lift cube to scale height", cubeUpScale);
SetName(ReleaseCube, "Release cube onto the switch", releaseCube);
SetName(VisionSegment, "Drive to switch with vision", visionSegment);
SetName(DriveBack10Inches, "Drive back 10 inches", driveBack10Inches);
JankyStateMachine::SetName(Stop, "End of Sequence");
c = 0;
done = false;
aMode = DEFAULT_MODE;
//gyro->Calibrate();
Start(); //most important part!!
/*SetName(DrivingSwitchEdge, "Drive Straight 162 inches");
SetName(TurningToRightSwitchEdge, "Turn left 90 degrees and drive forward 6 inches");
SetName(TurningToLeftSwitchEdge, "Turn right 90 degrees and drive forward 6 inches");
SetName(DrivingToRightFieldEdge, "Turn right 45 degrees and drive forward 60 inches");
SetName(DrivingToLeftFieldEdge, "Turn right 45 degrees and drive forward 60 inches");
SetName(CrossRightAutoLine, "Turn right 45 degrees and drive forward 120 inches");
SetName(CrossLeftAutoLine, "Turn left 45 degrees and drive forward 120 inches");
SetName(DrivingMid, "Drive straight 60 inches");
SetName(DrivingMidToAlignToLeftSwitch, "Turn left 90 degrees and drive forward until encoder distance is 60 inches");
SetName(DrivingMidToAlignToRightSwitch, "Turn right 90 degrees and drive forward until encoder distance is 60 inches");
SetName(DrivingLeftToAlignToRightSwitch, "Turn right 90 degrees and drive forward until encoder distance is 144 inches");
SetName(DrivingRightToAlignToLeftSwitch, "Turn left 90 degrees and drive forward until encoder distance is 144 inches");
SetName(DrivingToRightSwitchFront, "Turn left 90 degrees and drive forward until encoder distance is 72 inches");
SetName(DrivingToLeftSwitchFront, "Turn right 90 degrees and drive forward until encoder distance is 72 inches");
SetName(ReleaseCube, "Release the cube onto the switch");*/
}
int
set_machine_name_main(
int argc,
char* argv[]
)
{
DWORD dwError = 0;
DWORD dwRetError = 0;
PSTR pszMachineName = NULL;
size_t dwErrorBufferSize = 0;
BOOLEAN bPrintOrigError = TRUE;
if (geteuid() != 0) {
fprintf(stderr, "This program requires super-user privileges.\n");
dwError = LW_ERROR_ACCESS_DENIED;
BAIL_ON_LSA_ERROR(dwError);
}
dwError = ParseArgs(argc, argv,
&pszMachineName);
BAIL_ON_LSA_ERROR(dwError);
dwError = ValidateParameters(pszMachineName);
BAIL_ON_LSA_ERROR(dwError);
dwError = SetMachineName(pszMachineName);
BAIL_ON_LSA_ERROR(dwError);
cleanup:
if (pszMachineName)
{
LW_SAFE_FREE_STRING(pszMachineName);
}
return dwError;
error:
dwRetError = MapErrorCode(dwError);
dwErrorBufferSize = LwGetErrorString(dwRetError, NULL, 0);
if (dwErrorBufferSize > 0)
{
PSTR pszErrorBuffer = NULL;
dwError = LwAllocateMemory(
dwErrorBufferSize,
(PVOID*)&pszErrorBuffer);
if (!dwError)
{
DWORD dwLen = 0;
dwLen = LwGetErrorString(dwRetError,
pszErrorBuffer,
dwErrorBufferSize);
if ((dwLen == dwErrorBufferSize) &&
!LW_IS_NULL_OR_EMPTY_STR(pszErrorBuffer))
{
fprintf(stderr, "Failed to modify machine name. %s\n", pszErrorBuffer);
bPrintOrigError = FALSE;
}
}
LW_SAFE_FREE_STRING(pszErrorBuffer);
}
if (bPrintOrigError)
{
fprintf(stderr, "Failed to modify machine name. Error code [%u]\n", dwRetError);
}
dwError = dwRetError;
goto cleanup;
}
