SmartDrive.h

Go to the documentation of this file.

 
#pragma once
 
#include "ART/Vec2.h"
#include "ART/PID.h"
#include "ART/Units.h"
#include "ART/TankDrive.h"
 
#include <memory>
#include <cmath>
 
namespace art
{
 
    class SmartDrive : public TankDrive
    {
    public:
        SmartDrive(TankDrive drive, vex::inertial inert);
 
        SmartDrive &withWheelSize(Length size);
 
        SmartDrive &withGearRatio(double ratio);
 
        SmartDrive &withHorizontalTracker(vex::rotation rotation, Length wheelSize, double gearRatio);
 
        SmartDrive &withHorizontalTracker(vex::rotation rotation, Length wheelSize, double gearRatio, Length wheelOffset);
 
        int track();
 
        void driveFor(Length target, double speed);
 
        void driveForPID(Length target);
 
        PID m_driveForPID;
 
        SmartDrive &withDriveForPID(PID pid);
 
        void turnFor(Angle target, double speed);
 
        void turnForPID(Angle target);
 
        PID m_turnForPID;
 
        SmartDrive &withTurnForPID(PID pid);
 
        void turnTo(Angle target, double speed);
 
        void turnToPID(Angle target);
 
        PID m_turnToPID;
 
        SmartDrive &withTurnToPID(PID pid);
 
        Length getWheelTravel();
 
    private:
        struct HorizontalTracker
        {
            HorizontalTracker();
 
            HorizontalTracker(vex::rotation rotation, Length wheelSize, double gearRatio);
            HorizontalTracker(vex::rotation rotation, Length wheelSize, double gearRatio, Length wheelOffset);
 
            Length getTravel();
 
            std::shared_ptr<vex::rotation> m_rotation{nullptr};
            Length m_wheelSize{Inches(2.75)};
            double m_gearRatio{1.0};
 
            Angle m_lastAngle = Angle(0);
            Angle m_travelAngle = Angle(0);
            Length m_travelDistance = Length(0);
 
            Length m_offset = Length(0);
        };
 
        HorizontalTracker m_tracker;
 
        Vec2 m_pos;
        Vec2 m_centerPos;
        Angle m_dir;
 
        vex::inertial m_inert;
 
        Length m_wheelSize{Inches(3.25)};
        double m_gearRatio{1.0};
 
        Angle getLeftTravel();
 
        Angle m_LastLeftPos;
 
        Angle getRightTravel();
 
        Angle m_LastRightPos;
 
    };
 
} // namespace art