Jonathan Cacace / IONMcMotor

xx

Dependencies:   RoboClaw

IONMcMotors.cpp@3:7912ab1400c4, 2021-09-14 (annotated)

Committer:
anfontanelli
Date:
Tue Sep 14 12:09:47 2021 +0000
Revision:
3:7912ab1400c4
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 3:7912ab1400c4 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 3:7912ab1400c4 29 bool IONMcMotors::getState(int motor, double& curr, double& speed, double& pos){
anfontanelli 3:7912ab1400c4 30 //printf("get state \r\n");
anfontanelli 0:0493ecc839e6 31 int32_t currM1_int = 0;
anfontanelli 0:0493ecc839e6 32 int32_t currM2_int = 0;
anfontanelli 3:7912ab1400c4 33 int32_t speed_temp;
anfontanelli 0:0493ecc839e6 34 int32_t encPulse;
anfontanelli 0:0493ecc839e6 35 double currM1_double = 0.0;
anfontanelli 0:0493ecc839e6 36 double currM2_double = 0.0;
anfontanelli 0:0493ecc839e6 37
anfontanelli 0:0493ecc839e6 38 if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
anfontanelli 3:7912ab1400c4 39 return false;
anfontanelli 0:0493ecc839e6 40 }
anfontanelli 0:0493ecc839e6 41
anfontanelli 0:0493ecc839e6 42 if(motor == 1){
anfontanelli 0:0493ecc839e6 43 currM1_double = (double)currM1_int / 100;
anfontanelli 0:0493ecc839e6 44 if(!roboclaw->ReadEncM1(encPulse)){
anfontanelli 3:7912ab1400c4 45 return false;
anfontanelli 0:0493ecc839e6 46 }
anfontanelli 3:7912ab1400c4 47 pos = encPulse/(rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 48
anfontanelli 3:7912ab1400c4 49 if(!roboclaw->ReadSpeedM1(speed_temp)){
anfontanelli 3:7912ab1400c4 50 return false;
anfontanelli 0:0493ecc839e6 51 }
anfontanelli 3:7912ab1400c4 52 speed=speed_temp/(rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 53
anfontanelli 3:7912ab1400c4 54 curr = currM1_double*Kt_M1*GearBoxRatio;
anfontanelli 0:0493ecc839e6 55 }else if(motor == 2){
anfontanelli 0:0493ecc839e6 56 currM2_double = (double)currM2_int / 100;
anfontanelli 0:0493ecc839e6 57 if(!roboclaw->ReadEncM2(encPulse)){
anfontanelli 3:7912ab1400c4 58 return false;
anfontanelli 0:0493ecc839e6 59 }
anfontanelli 3:7912ab1400c4 60 pos = encPulse/(rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 61
anfontanelli 3:7912ab1400c4 62 if(!roboclaw->ReadSpeedM2(speed_temp)){
anfontanelli 3:7912ab1400c4 63 return false;
anfontanelli 0:0493ecc839e6 64 }
anfontanelli 3:7912ab1400c4 65 speed=speed_temp/(rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 66
anfontanelli 3:7912ab1400c4 67 curr = currM2_double*Kt_M2*GearBoxRatio;
anfontanelli 3:7912ab1400c4 68 }
anfontanelli 3:7912ab1400c4 69 return true;
anfontanelli 3:7912ab1400c4 70
anfontanelli 0:0493ecc839e6 71
anfontanelli 0:0493ecc839e6 72
anfontanelli 0:0493ecc839e6 73 }
anfontanelli 0:0493ecc839e6 74
anfontanelli 0:0493ecc839e6 75
anfontanelli 0:0493ecc839e6 76 double IONMcMotors::getMotorTorque(int motor){
anfontanelli 0:0493ecc839e6 77 int32_t currM1_int = 0;
anfontanelli 0:0493ecc839e6 78 int32_t currM2_int = 0;
anfontanelli 0:0493ecc839e6 79 double tor_M1_double = 0.0;
anfontanelli 0:0493ecc839e6 80 double tor_M2_double = 0.0;
anfontanelli 0:0493ecc839e6 81
anfontanelli 0:0493ecc839e6 82 if(!roboclaw->ReadCurrentM1M2(currM1_int,currM2_int)){
anfontanelli 0:0493ecc839e6 83 //printf("errorCurrent\r\n");
anfontanelli 0:0493ecc839e6 84 }
anfontanelli 0:0493ecc839e6 85
anfontanelli 0:0493ecc839e6 86
anfontanelli 0:0493ecc839e6 87 if(motor == 1){
anfontanelli 0:0493ecc839e6 88 tor_M1_double = (double)(currM1_int)*Kt_M1*GearBoxRatio / 100;
anfontanelli 0:0493ecc839e6 89
anfontanelli 0:0493ecc839e6 90 return tor_M1_double;
anfontanelli 0:0493ecc839e6 91 }else if(motor == 2){
anfontanelli 0:0493ecc839e6 92 tor_M2_double = (double)(currM2_int)*Kt_M1*GearBoxRatio / 100;
anfontanelli 0:0493ecc839e6 93
anfontanelli 0:0493ecc839e6 94 return tor_M2_double;
anfontanelli 0:0493ecc839e6 95 }else{
anfontanelli 0:0493ecc839e6 96 return 0.0;
anfontanelli 0:0493ecc839e6 97 }
anfontanelli 0:0493ecc839e6 98
anfontanelli 0:0493ecc839e6 99
anfontanelli 0:0493ecc839e6 100 }
anfontanelli 0:0493ecc839e6 101
anfontanelli 0:0493ecc839e6 102
anfontanelli 0:0493ecc839e6 103 double IONMcMotors::getMotorSpeed(int motor){
anfontanelli 0:0493ecc839e6 104 int32_t speed;
anfontanelli 0:0493ecc839e6 105
anfontanelli 0:0493ecc839e6 106 if(motor == 1){
anfontanelli 0:0493ecc839e6 107
anfontanelli 0:0493ecc839e6 108 if(!roboclaw->ReadSpeedM1(speed)){
anfontanelli 0:0493ecc839e6 109 //printf("errorSpeed\r\n");
anfontanelli 0:0493ecc839e6 110 }
anfontanelli 0:0493ecc839e6 111 return (double)speed / (rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 112 }else if(motor == 2){
anfontanelli 0:0493ecc839e6 113
anfontanelli 0:0493ecc839e6 114
anfontanelli 0:0493ecc839e6 115 if(!roboclaw->ReadSpeedM2(speed)){
anfontanelli 0:0493ecc839e6 116 //printf("errorSpeed\r\n");
anfontanelli 0:0493ecc839e6 117 }
anfontanelli 0:0493ecc839e6 118 return (double)speed/(rads2rps*speed2Pulse);
anfontanelli 0:0493ecc839e6 119
anfontanelli 0:0493ecc839e6 120 }else{
anfontanelli 0:0493ecc839e6 121 return 0.0;
anfontanelli 0:0493ecc839e6 122 }
anfontanelli 0:0493ecc839e6 123
anfontanelli 0:0493ecc839e6 124
anfontanelli 0:0493ecc839e6 125 }