adam/FRC2016-old
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add real TestPeriodic

Browse Source
This commit is contained in:
Adam Goldsmith 2016-03-05 14:28:32 -05:00
parent 10d9a20d51
commit 45bda434a9
1 changed files with 141 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
Robot2016/src/Robot.cpp
View File

@ -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();
}
};