diff --git a/src/Robot.cpp b/src/Robot.cpp index 70fb409..ed81880 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -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()); } } }