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

Well, ... it's a rough draft, but it SHOULD match the behavior requested.

This commit is contained in:
Aidan Ferguson 2016-02-17 16:39:08 -05:00
parent 5710edfadd
commit 4392dd348e
1 changed files with 39 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/Robot.cpp
View File

@ -111,8 +111,8 @@ private:
{
if (driver_stick.GetRawButton(THUMB))
{
left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist);
right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist);
left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist());
right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist());
}
else
{
@ -184,8 +184,43 @@ private:
old_theta = theta;
if (theta < 0.125)
{
left_drive.Set(1);
right_drive.Set()
left_drive.Set(1 * driver_stick.GetThrottle());
right_drive.Set((0.25 + 0.25 * 8 * theta) * driver_stick.GetThrottle());
}
else if (theta < 0.25)
{
left_drive.Set(1 * driver_stick.GetThrottle());
right_drive.Set((0.5 + 0.5 * 8 * (theta - 0.125)) * driver_stick.GetThrottle());
}
else if (theta < .375)
{
left_drive.Set((1 - 0.5 * 8 * (theta - 0.25)) * driver_stick.GetThrottle());
right_drive.Set(1 * driver_stick.GetThrottle());
}
else if (theta < 0.5)
{
left_drive.Set((0.5 - 0.25 * 8 * (theta - 0.375)) * driver_stick.GetThrottle());
right_drive.Set(1 * driver_stick.GetThrottle());
}
else if (theta < 0.625)
{
left_drive.Set((0.25 - 0.25 * 8 * (theta - 0.5)) * driver_stick.GetThrottle());
right_drive.Set((1 - 8 * (theta - 0.5)) * driver_stick.GetThrottle());
}
else if (theta < 0.75)
{
left_drive.Set(-8 * (theta - 0.625) * driver_stick.GetThrottle());
right_drive.Set(-8 * (theta - 0.625) * driver_stick.GetThrottle());
}
else if (theta < 0.875)
{
left_drive.Set((8 * (theta - 0.75) - 1) * driver_stick.GetThrottle());
right_drive.Set((8 * (theta - 0.75) - 1) * driver_stick.GetThrottle());
}
else
{
left_drive.Set(8 * (theta - 0.75) * driver_stick.GetThrottle());
right_drive.Set(0.25 * 8 * (theta - 0.75) * driver_stick.GetThrottle());
}
}
}