It now only twists when the thumb button is pressed and I added the beginnings of an UpdateDrive method.

This commit is contained in:
Aidan Ferguson 2016-02-17 15:22:32 -05:00
parent 439cf34a70
commit 5710edfadd

View File

@ -1,5 +1,6 @@
#include "WPILib.h" #include "WPILib.h"
#include "Shooter.h" #include "Shooter.h"
#include <cmath>
#ifndef BUTTON_LAYOUT #ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT #define BUTTON_LAYOUT
@ -41,6 +42,8 @@ private:
// instance variables // instance variables
bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released. bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released.
bool inverting; bool inverting;
float theta;
float old_theta;
LiveWindow *lw = LiveWindow::GetInstance(); LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser; SendableChooser *chooser;
@ -106,9 +109,16 @@ private:
void TeleopPeriodic() void TeleopPeriodic()
{ {
drive.ArcadeDrive(&driver_stick, true); if (driver_stick.GetRawButton(THUMB))
{
left_drive.Set(driver_stick.GetThrottle() * driver_stick.GetTwist);
right_drive.Set(-1 * driver_stick.GetThrottle() * driver_stick.GetTwist);
}
else
{
UpdateDrive();
}
// This is shit code for testing. Replace it with real code.
if(operator_stick.GetRawButton(RAMP_RAISE)) if(operator_stick.GetRawButton(RAMP_RAISE))
{ {
ramp.Set(1); ramp.Set(1);
@ -152,6 +162,33 @@ private:
{ {
lw->Run(); lw->Run();
} }
void UpdateDrive()
{
//regenerating theta
float x = driver_stick.GetX();
float y = driver_stick.GetY();
if (x < 0)
{
theta = atan(y / x) * -1;
}
else
{
theta = atan(y / x);
}
theta = theta / 6.283185307; // TODO: This math is optimized for humans. Fix it after initial debugging.
theta = theta + 1; // I know this is a lot of steps, but I want it to be really readable, because there are at least two stupid mistakes I've yet to find.
if (theta != old_theta)
{
//actually update motors
old_theta = theta;
if (theta < 0.125)
{
left_drive.Set(1);
right_drive.Set()
}
}
}
}; };
START_ROBOT_CLASS(Robot) START_ROBOT_CLASS(Robot)