diff --git a/src/Robot.cpp b/src/Robot.cpp index 6303a61..bcd86dd 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -435,7 +435,147 @@ public: void TestPeriodic() { - lw->Run(); + const int driveTime = 5; //Drive for 5 seconds + const float drivePower = 0.5, armPower=.35, rampPower=.4; //Arbitrary powers to test at + static Timer t; + enum test_stages { + INIT, + ARMS_UP, + ARMS_DOWN, + RAMP_UP, + RAMP_DOWN, + DRIVE_FORWARD, + DRIVE_BACKWARD, + TURN_CW, + TURN_CCW, + END}; + + static enum test_stages test_stage, old_test_stage; + + if (old_test_stage != test_stage) + { + //Reset timer between stages + t.Stop(); + t.Reset(); + + //Wait for a button press between stages + if (operator_stick.GetRawButton(TRIGGER)) + old_test_stage = test_stage; + } + else + { + if(!t.Get()) t.Start(); + + switch (test_stage) + { + case INIT: + { + break; + } + + case ARMS_UP: + { + if (arms.GetForwardLimitOK()) + arms.Set(1); + else + { + arms.Set(0); + test_stage = ARMS_DOWN; + } + break; + } + + case ARMS_DOWN: + { + if (arms.GetReverseLimitOK()) + arms.Set(-1); + else + { + arms.Set(0); + test_stage = RAMP_UP; + } + break; + } + + case RAMP_UP: + { + if (arms.GetForwardLimitOK()) + ramp.Set(1); + else + { + ramp.Set(0); + test_stage = RAMP_DOWN; + } + break; + } + + case RAMP_DOWN: + { + if (arms.GetReverseLimitOK()) + ramp.Set(-1); + else + { + ramp.Set(0); + test_stage = DRIVE_FORWARD; + } + break; + } + + case DRIVE_FORWARD: + { + if (t.Get() < driveTime) + drive.TankDrive(drivePower, drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = DRIVE_BACKWARD; + } + break; + } + + case DRIVE_BACKWARD: + { + if (t.Get() < driveTime) + drive.TankDrive(-drivePower, -drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = TURN_CW; + } + break; + } + + case TURN_CW: + { + if (t.Get() < driveTime) + drive.TankDrive(drivePower, -drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = TURN_CCW; + } + break; + } + + case TURN_CCW: + { + if (t.Get() < driveTime) + drive.TankDrive(-drivePower, drivePower); + else + { + drive.TankDrive(0.0, 0.0); + test_stage = END; + } + break; + } + + case END: + { + break; + } + } + } + LogCSVData(); } };