DriveMotorRPM.h

//
// Created by Tucker Haydon on 4/27/17.
//

#pragma once
#include "../hardwareinterface/MotorAdapter.h"
#include "../sensors/EncoderSensor.h"
#include "../Communicator.h"
#include "Command.h"
#include "../DriveConstants.h"
#include <stdlib.h>
#include <math.h>

class DriveMotorRPM: public Command {

public:

    DriveMotorRPM(Communicator* comm, Hardware motorHardware, EncoderSensor* encoder, int desiredRPM, int direction);

    ~DriveMotorRPM();

    bool execute() override;

    bool cleanup(bool canceled) override;

private:
    int desiredRPM;
    int direction;
    Hardware motorHardware;
    EncoderSensor* encoder;
    Communicator* comm;

    double k_p = 1.0;
    double k_d = 0.0;

    double lastDiff = 0;
    double setpoint = 0;

};