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

Updated for simpler picewise function.

Browse Source
This commit is contained in:
Aidan Ferguson 2016-02-19 15:47:52 -05:00
parent 4392dd348e
commit 5b47a61a78
1 changed files with 8 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/Robot.cpp
View File

@ -1,6 +1,5 @@
#include "WPILib.h"
#include "Shooter.h"
#include <cmath>
#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT
@ -42,8 +41,7 @@ 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;
float oldXY;
LiveWindow *lw = LiveWindow::GetInstance();
SendableChooser *chooser;
@ -165,62 +163,20 @@ private:
void UpdateDrive()
{
//regenerating theta
float x = driver_stick.GetX();
float y = driver_stick.GetY();
if (x < 0)
if ((x + y) != oldXY)
{
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)
oldXY = x + y;
if (x > 0)
{
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());
left_drive.Set(y * driver_stick.GetThrottle());
right_drive.Set((1-x)*y * 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());
right_drive.Set(y * driver_stick.GetThrottle());
left_drive.Set((1+x)*y * driver_stick.GetThrottle());
}
}
}