MiniR4Motion.h

Go to the documentation of this file.

#ifndef MINIR4MOTION_H
#define MINIR4MOTION_H
 
#include "MMLower.h"
#include <EEPROM.h>

class MiniR4Motion
{
public:
    MiniR4Motion() {}
 
    enum class AxisType
    {
        X,
        Y,
        Z,
        Roll,
        Pitch,
        Yaw
    };
    
    bool begin(void)
    {
        bool result = applyIMUCalData();
        return (result);
    }   

    double getGyro(AxisType axis)
    {
        double x = 0, y = 0, z = 0;
        mmL.GetIMUGyro(x, y, z);
 
        if (axis == AxisType::X)
            return x;
        else if (axis == AxisType::Y)
            return y;
        else if (axis == AxisType::Z)
            return z;
        else
            return 0;
    }

    double getAccel(AxisType axis)
    {
        double x = 0, y = 0, z = 0;
        mmL.GetIMUAcc(x, y, z);
 
        if (axis == AxisType::X)
            return x;
        else if (axis == AxisType::Y)
            return y;
        else if (axis == AxisType::Z)
            return z;
        else
            return 0;
    }
    
    double getAccelRaw(AxisType axis)
    {
        float accdataCX[3];
        bool result = getIMU_acc_real(accdataCX);
        
        if (!result)
            return 0;
        
        if (axis == AxisType::X)
            return accdataCX[0];
        else if (axis == AxisType::Y)
            return accdataCX[1];
        else if (axis == AxisType::Z)
            return accdataCX[2];
        else
            return 0;
    }
    
    double getEuler(AxisType axis)
    {
        double roll = 0, pitch = 0, yaw = 0;
        mmL.GetIMUEuler(roll, pitch, yaw);
 
        if (axis == AxisType::Roll)
            return roll;
        else if (axis == AxisType::Pitch)
            return pitch;
        else if (axis == AxisType::Yaw)
            return yaw;
        else
            return -999.0f;
    }   
    
    uint8_t getAccel_All(double * dataX)
    {
        double x = 0, y = 0, z = 0;
        mmL.GetIMUAcc(x, y, z);
 
        dataX[0] = x;
        dataX[1] = y;
        dataX[2] = z;
        
        return 0;
    }   
    
    uint8_t getGyro_All(double * dataX)
    {
        double x = 0, y = 0, z = 0;
        mmL.GetIMUGyro(x, y, z);
        dataX[0] = x;
        dataX[1] = y;
        dataX[2] = z;
        
        return 0;
    }

    uint8_t getEuler_All(double * dataX)
    {
        double roll = 0, pitch = 0, yaw = 0;
        mmL.GetIMUEuler(roll, pitch, yaw);
 
        dataX[0] = roll;
        dataX[1] = pitch;
        dataX[2] = yaw;
        
        return 0;
    }
    
    bool saveIMUCalData(float face1, float face2, float face3, float face4, float face5, float face6)
    {
        float accdata[6] = {face1, face2, face3, face4, face5, face6};
        
        int Address_F = 50;
        EEPROM.put(Address_F, accdata[0]);
        Address_F += sizeof(float);
        EEPROM.put(Address_F, accdata[1]);
        Address_F += sizeof(float);
        EEPROM.put(Address_F, accdata[2]);
        Address_F += sizeof(float);
        EEPROM.put(Address_F, accdata[3]);
        Address_F += sizeof(float);
        EEPROM.put(Address_F, accdata[4]);
        Address_F += sizeof(float);
        EEPROM.put(Address_F, accdata[5]);
        
        MMLower::RESULT result = mmL.SetIMU_Calib_data(accdata);
        return (result == MMLower::RESULT::OK);
    }

    bool applyIMUCalData(void)
    {       
        float accdata[6];
        
        int Address_F = 50;     
        EEPROM.get(Address_F, accdata[0]);
        Address_F += sizeof(float);
        EEPROM.get(Address_F, accdata[1]);
        Address_F += sizeof(float);
        EEPROM.get(Address_F, accdata[2]);  
        Address_F += sizeof(float);
        EEPROM.get(Address_F, accdata[3]);  
        Address_F += sizeof(float);
        EEPROM.get(Address_F, accdata[4]);  
        Address_F += sizeof(float);
        EEPROM.get(Address_F, accdata[5]);  
        
        MMLower::RESULT result = mmL.SetIMU_Calib_data(accdata);
        return (result == MMLower::RESULT::OK);
    }
    
    bool getAllSpeed(int32_t * Speed_value)
    {
        int32_t         Speed_En[4];
        MMLower::RESULT result  = mmL.GetALLEncoderSpeed(Speed_En);
        delay(1);
        result  = mmL.GetALLEncoderSpeed(Speed_En);
        Speed_value[0] = Speed_En[0];
        Speed_value[1] = Speed_En[1];
        Speed_value[2] = Speed_En[2];
        Speed_value[3] = Speed_En[3];
        
        return (result == MMLower::RESULT::OK);
    }   
    
    bool getIMU_acc_real(float * accdataCX)
    {
        float         Speed_En[4];
        MMLower::RESULT result  = mmL.Get_IMU_nancalib_acc(Speed_En);
        delay(2);
        result  = mmL.Get_IMU_nancalib_acc(Speed_En);
        
        accdataCX[0] = Speed_En[0];
        accdataCX[1] = Speed_En[1];
        accdataCX[2] = Speed_En[2];
        
        return (result == MMLower::RESULT::OK);
    }   

    bool resetIMUValues(void) { return (mmL.SetIMUToZero() == MMLower::RESULT::OK); }
 
private:
};
 
#endif   // MINIR4MOTION_H