diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp index 1ec2bad..d620e6b 100644 --- a/Robot2016/src/Robot.cpp +++ b/Robot2016/src/Robot.cpp @@ -11,10 +11,15 @@ #define RAMP_LOWER 3 // Button 4 to lower ramp. #define UNJAM 11 -#define DEADZONE_RADIUS 0.01 // Deadzone Radius prevents tiny twitches in the joystick's value from +#define DEADZONE_RADIUS 0.05 // Deadzone Radius prevents tiny twitches in the joystick's value from // affecting the robot. Use this for cleaning up drive train and shooter. // Also used for detecting changes in an axis' value. +#define TURN_FACTOR 1.5 // Left(x,y) = y*(1 + TF*x) : x < 0 + // = y : x >= 0 + // Right(x,y) = y : x < 0 + // = y*(1 - TF*x) : x >= 0 + #endif // BUTTON_LAYOUT class Robot: public IterativeRobot @@ -51,6 +56,7 @@ private: bool shooting; bool unjamming; bool arming; + bool arcade; float shooter_power; LiveWindow *lw = LiveWindow::GetInstance(); @@ -77,6 +83,7 @@ private: unjamming = false; arming = false; shooter_power = 0; + arcade = false; } @@ -118,7 +125,31 @@ private: std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl; std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl; drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75); - + if(driver_stick.GetRawButton(7)) + { + arcade = true; + } + if(driver_stick.GetRawButton(8)) + { + arcade = false; + } + if (arcade) + { + drive.ArcadeDrive(driver_stick); + } + else + { + if (driver_stick.GetRawButton(THUMB)) + { + float left = driver_stick.GetTwist(); + float right = -driver_stick.GetTwist(); + drive.TankDrive(left, right); + } + else + { + UpdateDrive(); + } + } //bool rampDoing = false; // This is shit code for testing. Replace it with real code. if(!ramping && operator_stick.GetRawButton(RAMP_RAISE)) @@ -189,21 +220,21 @@ private: * This is important because the TeleopPeriodic method executes something like once every 10ms. * Thus, this if-else if pair make the button a toggle. */ - if(driver_stick.GetRawButton(THUMB) && !inverting) + if(driver_stick.GetRawButton(TRIGGER) && !inverting) { std::cout << "Inverting Drive Train."; left_drive.SetInverted(!left_drive.GetInverted()); right_drive.SetInverted(!right_drive.GetInverted()); inverting = true; } - else if(!driver_stick.GetRawButton(THUMB)) + else if(!driver_stick.GetRawButton(TRIGGER)) { inverting = false; } /* - * Unlike the previous actions, this method does need to be called every itteration of + * Unlike the previous actions, this method does need to be called every iteration of * TeleopPeriodic. This is because the logic running this operation needs to be checked * Every time the method is called. This cannot be a loop in the Shoot method because * that would lock the robot every time the trigger is hit. @@ -257,6 +288,45 @@ private: { lw->Run(); } + + void SimpleDrive() + { + float x = -driver_stick.GetX(); + float y = -driver_stick.GetY(); + float left = 0; + float right = 0; + + if (x > 0) + { + right = y; + left = (1- x*TURN_FACTOR)*y ; + } + else + { + left = y; + right = (1+x*TURN_FACTOR)*y; + } + + drive.TankDrive(left, right); + } + + void UpdateDrive() + { + float x = -driver_stick.GetX(); + float y = -driver_stick.GetY(); + if (x > 0) + { + float right = y * SaneThrottle(driver_stick.GetThrottle()); + float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle()); + drive.TankDrive(left, right); + } + else + { + float left = y * SaneThrottle(driver_stick.GetThrottle()); + float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle()); + drive.TankDrive(left, right); + } + } }; START_ROBOT_CLASS(Robot)