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

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
1 changed files with 39 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/Robot.cpp
View File

@ -1,5 +1,6 @@
#include "WPILib.h"
#include "Shooter.h"
#include <cmath>
#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT
@ -41,6 +42,8 @@ private:
// 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 inverting;
float theta;
float old_theta;
LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser;
@ -106,9 +109,16 @@ private:
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))
{
ramp.Set(1);
@ -152,6 +162,33 @@ private:
{
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)