Marco De Silva / IONMcMotor

a

IONMcMotor/IONMcMotors.cpp@0:6b67f1bb9c76, 2021-09-28 (annotated)

Committer:
marcodesilva
Date:
Tue Sep 28 10:42:56 2021 +0000
Revision:
0:6b67f1bb9c76
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 0:6b67f1bb9c76 1 #include "IONMcMotors.h"
marcodesilva 0:6b67f1bb9c76 2
marcodesilva 0:6b67f1bb9c76 3 IONMcMotors::IONMcMotors(uint8_t adr, int baudrate, PinName rx, PinName tx, uint16_t _GearBoxRatio, uint16_t _EncoderPulse, double _Kt_M1, double _Kt_M2){
marcodesilva 0:6b67f1bb9c76 4 roboclaw = new RoboClaw(adr,baudrate, rx, tx, 1.0);
marcodesilva 0:6b67f1bb9c76 5 GearBoxRatio = _GearBoxRatio;
marcodesilva 0:6b67f1bb9c76 6 EncoderPulse = _EncoderPulse;
marcodesilva 0:6b67f1bb9c76 7 speed2Pulse = GearBoxRatio*EncoderPulse;
marcodesilva 0:6b67f1bb9c76 8 rads2rps = 1/(2*M_PI);
marcodesilva 0:6b67f1bb9c76 9 Kt_M1 = _Kt_M1;
marcodesilva 0:6b67f1bb9c76 10 Kt_M2 = _Kt_M2;
marcodesilva 0:6b67f1bb9c76 11 }
marcodesilva 0:6b67f1bb9c76 12
marcodesilva 0:6b67f1bb9c76 13
marcodesilva 0:6b67f1bb9c76 14 bool IONMcMotors::setSpeed(int motor, double shaftSpeed, double shaftAcc){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
marcodesilva 0:6b67f1bb9c76 15 if(motor == 1){
marcodesilva 0:6b67f1bb9c76 16 roboclaw->SpeedAccelM1(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 17 return true;
marcodesilva 0:6b67f1bb9c76 18 }else if(motor == 2){
marcodesilva 0:6b67f1bb9c76 19 roboclaw->SpeedAccelM2(shaftAcc*rads2rps*speed2Pulse,shaftSpeed*rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 20 return true;
marcodesilva 0:6b67f1bb9c76 21 }else{
marcodesilva 0:6b67f1bb9c76 22 return false;
marcodesilva 0:6b67f1bb9c76 23 }
marcodesilva 0:6b67f1bb9c76 24 }
marcodesilva 0:6b67f1bb9c76 25
marcodesilva 0:6b67f1bb9c76 26 void IONMcMotors::resetEncoder(){ // shaftSpeed in rad/s // shaftAcc in rad/s^2
marcodesilva 0:6b67f1bb9c76 27 roboclaw->ResetEnc();
marcodesilva 0:6b67f1bb9c76 28 }
marcodesilva 0:6b67f1bb9c76 29
marcodesilva 0:6b67f1bb9c76 30 double* IONMcMotors::getState(int motor){
marcodesilva 0:6b67f1bb9c76 31 printf("get state \r\n");
marcodesilva 0:6b67f1bb9c76 32 int32_t currM1_int = 0;
marcodesilva 0:6b67f1bb9c76 33 int32_t currM2_int = 0;
marcodesilva 0:6b67f1bb9c76 34 int32_t speed;
marcodesilva 0:6b67f1bb9c76 35 int32_t encPulse;
marcodesilva 0:6b67f1bb9c76 36 double currM1_double = 0.0;
marcodesilva 0:6b67f1bb9c76 37 double currM2_double = 0.0;
marcodesilva 0:6b67f1bb9c76 38
marcodesilva 0:6b67f1bb9c76 39 if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
marcodesilva 0:6b67f1bb9c76 40 printf("errorCurrent\r\n");
marcodesilva 0:6b67f1bb9c76 41 }
marcodesilva 0:6b67f1bb9c76 42
marcodesilva 0:6b67f1bb9c76 43 if(motor == 1){
marcodesilva 0:6b67f1bb9c76 44 currM1_double = (double)currM1_int / 100;
marcodesilva 0:6b67f1bb9c76 45 if(!roboclaw->ReadEncM1(encPulse)){
marcodesilva 0:6b67f1bb9c76 46 printf("errorPos\r\n");
marcodesilva 0:6b67f1bb9c76 47 }
marcodesilva 0:6b67f1bb9c76 48 stateM1[0] = encPulse/(rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 49
marcodesilva 0:6b67f1bb9c76 50 if(!roboclaw->ReadSpeedM1(speed)){
marcodesilva 0:6b67f1bb9c76 51 printf("errorSpeed\r\n");
marcodesilva 0:6b67f1bb9c76 52 }
marcodesilva 0:6b67f1bb9c76 53 stateM1[1]=speed/(rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 54
marcodesilva 0:6b67f1bb9c76 55 stateM1[2] = currM1_double*Kt_M1*GearBoxRatio;
marcodesilva 0:6b67f1bb9c76 56 return stateM1;
marcodesilva 0:6b67f1bb9c76 57 }else if(motor == 2){
marcodesilva 0:6b67f1bb9c76 58 currM2_double = (double)currM2_int / 100;
marcodesilva 0:6b67f1bb9c76 59 if(!roboclaw->ReadEncM2(encPulse)){
marcodesilva 0:6b67f1bb9c76 60 printf("errorPos\r\n");
marcodesilva 0:6b67f1bb9c76 61 }
marcodesilva 0:6b67f1bb9c76 62 stateM2[0] = encPulse/(rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 63
marcodesilva 0:6b67f1bb9c76 64 if(!roboclaw->ReadSpeedM2(speed)){
marcodesilva 0:6b67f1bb9c76 65 printf("errorSpeed\r\n");
marcodesilva 0:6b67f1bb9c76 66 }
marcodesilva 0:6b67f1bb9c76 67 stateM2[1]=speed/(rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 68
marcodesilva 0:6b67f1bb9c76 69 stateM2[2] = currM2_double*Kt_M2*GearBoxRatio;
marcodesilva 0:6b67f1bb9c76 70 return stateM2;
marcodesilva 0:6b67f1bb9c76 71 }else{
marcodesilva 0:6b67f1bb9c76 72 return stateM1;
marcodesilva 0:6b67f1bb9c76 73 }
marcodesilva 0:6b67f1bb9c76 74
marcodesilva 0:6b67f1bb9c76 75
marcodesilva 0:6b67f1bb9c76 76 }
marcodesilva 0:6b67f1bb9c76 77
marcodesilva 0:6b67f1bb9c76 78
marcodesilva 0:6b67f1bb9c76 79 double IONMcMotors::getMotorTorque(int motor){
marcodesilva 0:6b67f1bb9c76 80 int32_t currM1_int = 0;
marcodesilva 0:6b67f1bb9c76 81 int32_t currM2_int = 0;
marcodesilva 0:6b67f1bb9c76 82 double tor_M1_double = 0.0;
marcodesilva 0:6b67f1bb9c76 83 double tor_M2_double = 0.0;
marcodesilva 0:6b67f1bb9c76 84
marcodesilva 0:6b67f1bb9c76 85 if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
marcodesilva 0:6b67f1bb9c76 86 //printf("errorCurrent\r\n");
marcodesilva 0:6b67f1bb9c76 87 }
marcodesilva 0:6b67f1bb9c76 88
marcodesilva 0:6b67f1bb9c76 89
marcodesilva 0:6b67f1bb9c76 90 if(motor == 1){
marcodesilva 0:6b67f1bb9c76 91 tor_M1_double = (double)(currM1_int)*Kt_M1*GearBoxRatio / 100;
marcodesilva 0:6b67f1bb9c76 92
marcodesilva 0:6b67f1bb9c76 93 return tor_M1_double;
marcodesilva 0:6b67f1bb9c76 94 }else if(motor == 2){
marcodesilva 0:6b67f1bb9c76 95 tor_M2_double = (double)(currM2_int)*Kt_M1*GearBoxRatio / 100;
marcodesilva 0:6b67f1bb9c76 96
marcodesilva 0:6b67f1bb9c76 97 return tor_M2_double;
marcodesilva 0:6b67f1bb9c76 98 }else{
marcodesilva 0:6b67f1bb9c76 99 return 0.0;
marcodesilva 0:6b67f1bb9c76 100 }
marcodesilva 0:6b67f1bb9c76 101
marcodesilva 0:6b67f1bb9c76 102
marcodesilva 0:6b67f1bb9c76 103 }
marcodesilva 0:6b67f1bb9c76 104
marcodesilva 0:6b67f1bb9c76 105
marcodesilva 0:6b67f1bb9c76 106 double IONMcMotors::getMotorSpeed(int motor){
marcodesilva 0:6b67f1bb9c76 107 int32_t speed;
marcodesilva 0:6b67f1bb9c76 108
marcodesilva 0:6b67f1bb9c76 109 if(motor == 1){
marcodesilva 0:6b67f1bb9c76 110
marcodesilva 0:6b67f1bb9c76 111 if(!roboclaw->ReadSpeedM1(speed)){
marcodesilva 0:6b67f1bb9c76 112 //printf("errorSpeed\r\n");
marcodesilva 0:6b67f1bb9c76 113 }
marcodesilva 0:6b67f1bb9c76 114 return (double)speed / (rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 115 }else if(motor == 2){
marcodesilva 0:6b67f1bb9c76 116
marcodesilva 0:6b67f1bb9c76 117
marcodesilva 0:6b67f1bb9c76 118 if(!roboclaw->ReadSpeedM2(speed)){
marcodesilva 0:6b67f1bb9c76 119 //printf("errorSpeed\r\n");
marcodesilva 0:6b67f1bb9c76 120 }
marcodesilva 0:6b67f1bb9c76 121 return (double)speed/(rads2rps*speed2Pulse);
marcodesilva 0:6b67f1bb9c76 122
marcodesilva 0:6b67f1bb9c76 123 }else{
marcodesilva 0:6b67f1bb9c76 124 return 0.0;
marcodesilva 0:6b67f1bb9c76 125 }
marcodesilva 0:6b67f1bb9c76 126
marcodesilva 0:6b67f1bb9c76 127
marcodesilva 0:6b67f1bb9c76 128 }