Our experiments with omni drive (field centric)

[teslalab2]

Just a quick video, https://www.youtube.com/watch?v=gG1j...ature=youtu.be

The Stabilization PID needs some help... but we seem to like omni drive, with the field centric it really takes all of the thinking out of driving... I'll cough up the code tomorrow, not too bad, just some simple trig and a PID. Funny enough I did this to break in our new programmer next and give him some experience, we worked out all the drive algorithms and the trig for the gyro, Good job Correy!

[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);

[faust1706]

Originally Posted by teslalab2

I reject the null hypothesis, there is significant evidence to support the claim "routers do not like reverse polarity".

Recent speculation has caused peer review journals to forbid even the mention of p-values.

Back to the purpose of this tread: nice work! I love how concise and neat the code is.

[teslalab2]

off topic: I think anything to do with stats should be outlawed in public places I just posted that because in my complacency I have killed 3 routers this year from reverse polarity...

On topic: If anyone uses this make sure that you have the IsEnabled() in the main while loop because if the robot is disabled the integration will loose it mind causing it to go into an uncontrollable death spin when re-enabled.

[Pault]

Very clean and simple. I like it.

My only critique is that you did not use the teleopPeriodic() method properly. It will automatically loop every 20ms, so there is no need to include your own while loop. All of the initialization code (such as zeroing the gyro) can go into teleopInit(), which will be called once at the beginning of teleop.

[teslalab2]

Originally Posted by Pault

Very clean and simple. I like it.

My only critique is that you did not use the teleopPeriodic() method properly. It will automatically loop every 20ms, so there is no need to include your own while loop. All of the initialization code (such as zeroing the gyro) can go into teleopInit(), which will be called once at the beginning of teleop.

Thanks for the advice, I never new that, learn something everyday I guess.