File content as of revision 3:7912ab1400c4:

#include "IONMcMotors.h"

IONMcMotors::IONMcMotors(uint8_t adr, int baudrate, PinName rx, PinName tx, uint16_t _GearBoxRatio, uint16_t _EncoderPulse, double _Kt_M1, double _Kt_M2){
    roboclaw = new RoboClaw(adr,baudrate, rx, tx, 1.0);
    GearBoxRatio = _GearBoxRatio;
    EncoderPulse = _EncoderPulse;
    speed2Pulse = GearBoxRatio*EncoderPulse;
    rads2rps = 1/(2*M_PI);
    Kt_M1 = _Kt_M1;
    Kt_M2 = _Kt_M2;
}


bool IONMcMotors::setSpeed(int motor, double shaftSpeed, double shaftAcc){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
    if(motor == 1){
        roboclaw->SpeedAccelM1(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);
        return true;
    }else if(motor == 2){
        roboclaw->SpeedAccelM2(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);  
        return true;
    }else{
        return false;
    }
}

void IONMcMotors::resetEncoder(){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
    roboclaw->ResetEnc();
}
bool IONMcMotors::getState(int motor, double& curr, double& speed, double& pos){
    //printf("get state \r\n");
    int32_t currM1_int = 0;
    int32_t currM2_int = 0;
    int32_t speed_temp;
    int32_t encPulse;
    double currM1_double = 0.0;
    double currM2_double = 0.0;
    
    if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
        return false;
    }

    if(motor == 1){  
        currM1_double = (double)currM1_int / 100;   
        if(!roboclaw->ReadEncM1(encPulse)){
           return false;
        }         
        pos = encPulse/(rads2rps*speed2Pulse); 
        
        if(!roboclaw->ReadSpeedM1(speed_temp)){
           return false;
        }        
        speed=speed_temp/(rads2rps*speed2Pulse);

        curr = currM1_double*Kt_M1*GearBoxRatio;
    }else if(motor == 2){
        currM2_double = (double)currM2_int / 100;
        if(!roboclaw->ReadEncM2(encPulse)){
           return false;
        }        
        pos = encPulse/(rads2rps*speed2Pulse); 
        
        if(!roboclaw->ReadSpeedM2(speed_temp)){
           return false;
        }        
        speed=speed_temp/(rads2rps*speed2Pulse);

        curr = currM2_double*Kt_M2*GearBoxRatio;
    }
    return true;
    

    
}


double IONMcMotors::getMotorTorque(int motor){
    int32_t currM1_int = 0;
    int32_t currM2_int = 0;    
    double tor_M1_double = 0.0;
    double tor_M2_double = 0.0;

    if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
        //printf("errorCurrent\r\n");
    }


    if(motor == 1){  
        tor_M1_double = (double)(currM1_int)*Kt_M1*GearBoxRatio / 100;  
       
        return tor_M1_double;
    }else if(motor == 2){
        tor_M2_double = (double)(currM2_int)*Kt_M1*GearBoxRatio / 100;
        
        return tor_M2_double;
    }else{
        return 0.0;
        }

    
}


double IONMcMotors::getMotorSpeed(int motor){
    int32_t speed;

    if(motor == 1){  
        
        if(!roboclaw->ReadSpeedM1(speed)){
           //printf("errorSpeed\r\n");
        }        
        return (double)speed / (rads2rps*speed2Pulse);
    }else if(motor == 2){
        
        
        if(!roboclaw->ReadSpeedM2(speed)){
           //printf("errorSpeed\r\n");
        }        
        return (double)speed/(rads2rps*speed2Pulse);

    }else{
        return 0.0;
        }

    
}