Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Technical Discussion (http://www.chiefdelphi.com/forums/forumdisplay.php?f=22)
-   -   Our experiments with omni drive (field centric) (http://www.chiefdelphi.com/forums/showthread.php?t=137349)

teslalab2 26-05-2015 23:47
Our experiments with omni drive (field centric)
 
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 02-06-2015 22:41
Re: Our experiments with omni drive (field centric)
 
Sorry I forgot, here is the code :D

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 02-06-2015 23:10
Re: Our experiments with omni drive (field centric)
 
Quote:
Originally Posted by teslalab2 (Post 1485531)
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 03-06-2015 13:35
Re: Our experiments with omni drive (field centric)
 
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... ::ouch:: ::ouch:: ::ouch:: ::ouch::

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 03-06-2015 14:44
Re: Our experiments with omni drive (field centric)
 
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 03-06-2015 19:30
Re: Our experiments with omni drive (field centric)
 
Quote:
Originally Posted by Pault (Post 1485620)
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.


All times are GMT -5. The time now is 16:23.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi