Jonathan Cacace / IONMcMotor

xx

Dependencies:   RoboClaw

IONMcMotors.cpp@2:fc1908afe4f4, 2021-09-14 (annotated)

Committer:
anfontanelli
Date:
Tue Sep 14 11:15:51 2021 +0000
Revision:
2:fc1908afe4f4
Parent:
0:0493ecc839e6 
A

Who changed what in which revision?

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