Motor.h

Go to the documentation of this file.

/*
 * Motor.h
 *
 *  Created on: May 31, 2020
 *      Author: hephaestus
 */

#ifndef LIBRARIES_RBE1001LIB_SRC_MOTOR_H_
#define LIBRARIES_RBE1001LIB_SRC_MOTOR_H_
#include <ESP32Servo.h>
#include <ESP32Encoder.h>
#include <Arduino.h>
#define MAX_POSSIBLE_MOTORS 4
#define ENCODER_CPR 12.0f
#define GEAR_BOX_RATIO 120.0f
#define QUADRATUE_MULTIPLYER 1.0f
#define TICKS_TO_DEGREES ((QUADRATUE_MULTIPLYER / (ENCODER_CPR * GEAR_BOX_RATIO / 360.0)) * -1)
#define I_TERM_SIZE 120.0f

const float DELTA_EFFORT = 0.0025;

enum interpolateMode
{
    LINEAR_INTERPOLATION = 1,
    SINUSOIDAL_INTERPOLATION = 2,
    VELOCITY_MODE = 3,
    BEZIER = 4,
    TRAPEZOIDAL = 5
};
class Motor
{
private:
    ESP32PWM *pwm;
    ESP32Encoder *encoder;
    bool isAttached = false;
    int interruptCountForVelocity = 0;
    /*
     * this stores the previous count of the encoder last time the velocity was calculated
     */
    int prevousCount = 0;
    float cachedSpeed = 0;
    float setpoint = 0;
    float kP = 0.02;
    float kI = 0.01;
    float kD = 0;
    float runntingITerm = 0;
    /*
     * effort of the motor
     * @param a value from -1 to 1 representing effort
     *        0 is brake
     *        1 is full speed clockwise
     *        -1 is full speed counter clockwise
     * @note this should only be called from the PID thread
     */
    void setEffortLocal(float effort);
    static void allocateTimer(int PWMgenerationTimer);
    bool closedLoopControl = true;
    float targetEffort = 0;
    float currentEffort = 0;
    float duration = 0;
    float startTime = 0;
    float endSetpoint = 0;
    float startSetpoint = 0;
    float unitDuration = 1;
    interpolateMode mode = LINEAR_INTERPOLATION;

    float milisecondPosIncrementForVelocity;
    float BEZIER_P0 = 0.25;
    float BEZIER_P1 = 0.75;

    float TRAPEZOIDAL_time = 0;

public:
    int MotorPWMPin = -1;
    int directionFlag = -1;
    int MotorEncAPin = -1;
    int MotorEncBPin = -1;
    float getInterpolationUnitIncrement();
    int64_t nowEncoder = 0;
    static bool timersAllocated;
    static Motor *list[MAX_POSSIBLE_MOTORS];



    Motor(int pwmPin, int dirPin, int encAPin, int encBPin);
    virtual ~Motor();

    void attach();
    /*
     *  \brief effort of the motor, proportional to PWM
     *
     * @param effort a value from -1 to 1 representing effort
     *        0 is brake
     *        1 is full speed clockwise
     *        -1 is full speed counter clockwise
     */
    void setEffort(float effort);
    /*
     * effort of the motor
     * @param percent a value from -100 to 100 representing effort
     *        0 is brake
     *        100 is full speed clockwise
     *        -100 is full speed counter clockwise
     */
    void setEffortPercent(float percent)
    {
        if (!isAttached)
            attach();
        setEffort(percent * 0.01);
    }
    /*
     * effort of the motor
     * @return a value from -1 to 1 representing effort
     *        0 is brake
     *        1 is full speed clockwise
     *        -1 is full speed counter clockwise
     */
    float getEffort();
    /*
     * effort of the motor
     * @return a value from -100 to 100 representing effort
     *        0 is brake
     *        100 is full speed clockwise
     *        -100 is full speed counter clockwise
     */
    float getEffortPercent()
    {
        if (!isAttached)
            attach();
        return getEffort() * 100;
    }
    float getDegreesPerSecond();
    float getCurrentDegrees();
    void loop();
    void setSetpointWithTime(float newTargetInDegrees, long miliseconds,
                             interpolateMode mode);
    void setSpeed(float newDegreesPerSecond);
    void setSpeed(float newDegreesPerSecond, long miliseconds);
    void moveTo(float newTargetInDegrees, float speedDegPerSec);
    void moveFor(float deltaTargetInDegrees, float speedDegPerSec);

    void blockUntilMoveIsDone();
    float startMoveFor(float deltaTargetInDegrees, float speedDegPerSec);

    void setSetpoint(float newTargetInDegrees)
    {
        if (!isAttached)
            attach();
        setSetpointWithTime(newTargetInDegrees, 0, LINEAR_INTERPOLATION);
    }

    void setSetpointWithLinearInterpolation(float newTargetInDegrees,
                                            long miliseconds)
    {
        if (!isAttached)
            attach();
        setSetpointWithTime(newTargetInDegrees, miliseconds,
                            LINEAR_INTERPOLATION);
    }

    void setSetpointWithSinusoidalInterpolation(float newTargetInDegrees,
                                                long miliseconds)
    {
        if (!isAttached)
            attach();
        setSetpointWithTime(newTargetInDegrees, miliseconds,
                            SINUSOIDAL_INTERPOLATION);
    }
    void setSetpointWithBezierInterpolation(float newTargetInDegrees,
                                            long miliseconds, float Control_0 = 0.5, float Control_1 = 1.0)
    {
        if (!isAttached)
            attach();
        BEZIER_P0 = Control_0;
        BEZIER_P1 = Control_1;
        setSetpointWithTime(newTargetInDegrees, miliseconds,
                            BEZIER);
    }
    void setSetpointWithTrapezoidalInterpolation(float newTargetInDegrees,
                                                 long miliseconds, float trapazoidalTime)
    {
        if (!isAttached)
            attach();
        if (trapazoidalTime * 2 > miliseconds)
        {
            setSetpointWithSinusoidalInterpolation(newTargetInDegrees,
                                                   miliseconds);
            return;
        }
        TRAPEZOIDAL_time = trapazoidalTime;
        setSetpointWithTime(newTargetInDegrees, miliseconds, TRAPEZOIDAL);
    }
    void setGains(float p, float i, float d);
    void setGainsP(float p);
    void setGainsI(float i);
    void setGainsD(float d);

    float getGainsP()
    {
        if (!isAttached)
            attach();
        return kP;
    }
    float getGainsI()
    {
        if (!isAttached)
            attach();
        return kI;
    }
    float getGainsD()
    {
        if (!isAttached)
            attach();
        return kD;
    }

    bool isMotorDoneWithMove();
};

//extern Motor left_motor;
//extern Motor right_motor;

#endif /* LIBRARIES_RBE1001LIB_SRC_MOTOR_H_ */