IONMcMotors.cpp

00001 #include "IONMcMotors.h"
00002 
00003 IONMcMotors::IONMcMotors(uint8_t adr, int baudrate, PinName rx, PinName tx, uint16_t _GearBoxRatio, uint16_t _EncoderPulse, double _Kt_M1, double _Kt_M2){
00004     roboclaw = new RoboClaw(adr,baudrate, rx, tx, 1.0);
00005     GearBoxRatio = _GearBoxRatio;
00006     EncoderPulse = _EncoderPulse;
00007     speed2Pulse = GearBoxRatio*EncoderPulse;
00008     rads2rps = 1/(2*M_PI);
00009     Kt_M1 = _Kt_M1;
00010     Kt_M2 = _Kt_M2;
00011 }
00012 
00013 
00014 bool IONMcMotors::setSpeed(int motor, double shaftSpeed, double shaftAcc){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
00015     if(motor == 1){
00016         roboclaw->SpeedAccelM1(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);
00017         return true;
00018     }else if(motor == 2){
00019         roboclaw->SpeedAccelM2(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);  
00020         return true;
00021     }else{
00022         return false;
00023     }
00024 }
00025 
00026 void IONMcMotors::resetEncoder(){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
00027     roboclaw->ResetEnc();
00028 }
00029 
00030 double* IONMcMotors::getState(int motor){
00031     printf("get state \r\n");
00032     int32_t currM1_int = 0;
00033     int32_t currM2_int = 0;
00034     int32_t speed;
00035     int32_t encPulse;
00036     double currM1_double = 0.0;
00037     double currM2_double = 0.0;
00038     
00039     if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
00040         printf("errorCurrent\r\n");
00041     }
00042 
00043     if(motor == 1){  
00044         currM1_double = (double)currM1_int / 100;   
00045         if(!roboclaw->ReadEncM1(encPulse)){
00046            printf("errorPos\r\n");
00047         }         
00048         stateM1[0] = encPulse/(rads2rps*speed2Pulse); 
00049         
00050         if(!roboclaw->ReadSpeedM1(speed)){
00051            printf("errorSpeed\r\n");
00052         }        
00053         stateM1[1]=speed/(rads2rps*speed2Pulse);
00054 
00055         stateM1[2] = currM1_double*Kt_M1*GearBoxRatio;
00056         return stateM1;
00057     }else if(motor == 2){
00058         currM2_double = (double)currM2_int / 100;
00059         if(!roboclaw->ReadEncM2(encPulse)){
00060            printf("errorPos\r\n");
00061         }        
00062         stateM2[0] = encPulse/(rads2rps*speed2Pulse); 
00063         
00064         if(!roboclaw->ReadSpeedM2(speed)){
00065            printf("errorSpeed\r\n");
00066         }        
00067         stateM2[1]=speed/(rads2rps*speed2Pulse);
00068 
00069         stateM2[2] = currM2_double*Kt_M2*GearBoxRatio;
00070         return stateM2;
00071     }else{
00072         return stateM1;
00073         }
00074 
00075     
00076 }
00077 
00078 
00079 double IONMcMotors::getMotorTorque(int motor){
00080     int32_t currM1_int = 0;
00081     int32_t currM2_int = 0;    
00082     double tor_M1_double = 0.0;
00083     double tor_M2_double = 0.0;
00084 
00085     if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
00086         //printf("errorCurrent\r\n");
00087     }
00088 
00089 
00090     if(motor == 1){  
00091         tor_M1_double = (double)(currM1_int)*Kt_M1*GearBoxRatio / 100;  
00092        
00093         return tor_M1_double;
00094     }else if(motor == 2){
00095         tor_M2_double = (double)(currM2_int)*Kt_M1*GearBoxRatio / 100;
00096         
00097         return tor_M2_double;
00098     }else{
00099         return 0.0;
00100         }
00101 
00102     
00103 }
00104 
00105 
00106 double IONMcMotors::getMotorSpeed(int motor){
00107     int32_t speed;
00108 
00109     if(motor == 1){  
00110         
00111         if(!roboclaw->ReadSpeedM1(speed)){
00112            //printf("errorSpeed\r\n");
00113         }        
00114         return (double)speed / (rads2rps*speed2Pulse);
00115     }else if(motor == 2){
00116         
00117         
00118         if(!roboclaw->ReadSpeedM2(speed)){
00119            //printf("errorSpeed\r\n");
00120         }        
00121         return (double)speed/(rads2rps*speed2Pulse);
00122 
00123     }else{
00124         return 0.0;
00125         }
00126 
00127     
00128 }