adam/FRC2016-old
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.

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

43
Robot2016/src/Robot.cpp
View File

@ -111,8 +111,8 @@ private:
{ {
if (driver_stick.GetRawButton(THUMB)) if (driver_stick.GetRawButton(THUMB))
{ {
left_drive.Set(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); right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist());
} }
else else
{ {
@ -184,8 +184,43 @@ private:
old_theta = theta; old_theta = theta;
if (theta < 0.125) if (theta < 0.125)
{ {
left_drive.Set(1); left_drive.Set(1 * driver_stick.GetThrottle());
right_drive.Set() 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());
} }
} }
} }