Our experiments with omni drive (field centric)

[teslalab2]

Sorry I forgot, here is the code

#include "WPILib.h"
#include "Commands/Command.h"
#include "Commands/ExampleCommand.h"
#include "CommandBase.h"
#include "math.h"
float x,y,z;
float angle;
float target;
double PI = 3.141592654;

class myPID{
private:
double Kp,Ki,Kd;
double error, error1 = 0;
double integral;
float value;
public:
double Pc,Pi,Pd;
double target,acutal;
void Update(double,double);
float GetValue(void);
void SetConstants(double,double,double);
};

void myPID::Update(double target,double actual){
	error = target - actual;
	integral += error*.01; //todo add actualy eleapsed time
	value = (error*Kp) + (integral*Ki) + ((error-error1)/(.01)*Kd);
	error1 = error;
}
float myPID::GetValue(){
	return value;
}

void myPID::SetConstants(double Pc,double Pi,double Pd){
	Kp = Pc;
	Ki = Pi;
	Kd = Pd;
}

myPID gyroPID;

class Robot: public IterativeRobot
{

	Talon *fR = new Talon (0);
	Talon *fL = new Talon (1);
	Talon *bR = new Talon (2);
	Talon *bL = new Talon (3);
	Joystick *DriveStick = new Joystick(0);
	Joystick *Turnstick = new Joystick(1);
	Gyro *gyro = new Gyro(0);
public:
	//Robot():

private:
	Command *autonomousCommand;
	LiveWindow *lw;

	void RobotInit()
	{
		CommandBase::init();
		autonomousCommand = new ExampleCommand();
		lw = LiveWindow::GetInstance();
	}
	
	void DisabledPeriodic()
	{
		Scheduler::GetInstance()->Run();
	}

	void AutonomousInit()
	{
		if (autonomousCommand != NULL)
			autonomousCommand->Start();
	}

	void AutonomousPeriodic()
	{
		Scheduler::GetInstance()->Run();
	}

	void TeleopInit()
	{
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		if (autonomousCommand != NULL)
			autonomousCommand->Cancel();
	}

	void TeleopPeriodic()
	{
		gyro->Reset();
		angle = gyro->GetAngle();
		target = angle;
		gyroPID.SetConstants(.012,.00001,0);
		while(IsEnabled()){
			angle = gyro->GetAngle();
			gyroPID.Update(target,angle);
			z = (Turnstick->GetZ()/-4)+gyroPID.GetValue();
			x = (cos (angle*PI/180)*-DriveStick->GetX())+(sin (angle*PI/180)*-DriveStick->GetY());
			y = (cos (angle*PI/180)*DriveStick->GetY())+(sin (angle*PI/180)*-DriveStick->GetX());

				fL->Set(-x-y+z);
				fR->Set(x-y+z);
				bL->Set(-x+y+z);
				bR->Set(x+y+z);
		}

		Wait(10);



		Scheduler::GetInstance()->Run();
	}

	void TestPeriodic()
	{
		lw->Run();
	}
};

START_ROBOT_CLASS(Robot);