RMD-X Motor Library - Last Update V0.5 - On going...... Current available Function - read velocity - read all data - send velocity - send position
RMD.cpp@5:07e41b6c6692, 2020-11-04 (annotated)
- Committer:
- sornpraram
- Date:
- Wed Nov 04 10:01:58 2020 +0000
- Revision:
- 5:07e41b6c6692
- Parent:
- 4:083894adb121
- Child:
- 7:2f2c3f244208
2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sornpraram | 0:433e4ec5de88 | 1 | #include "RMD.h" |
sornpraram | 0:433e4ec5de88 | 2 | #include "mbed.h" |
sornpraram | 0:433e4ec5de88 | 3 | #include <iostream> |
sornpraram | 0:433e4ec5de88 | 4 | #include <bitset> |
sornpraram | 0:433e4ec5de88 | 5 | |
sornpraram | 0:433e4ec5de88 | 6 | #define DEVICE_CAN_ID_1 0x141 |
sornpraram | 0:433e4ec5de88 | 7 | |
sornpraram | 0:433e4ec5de88 | 8 | RMD::RMD(PinName rx, PinName tx) : CAN(rx,tx,1000000) { |
sornpraram | 0:433e4ec5de88 | 9 | frequency(1000000); |
sornpraram | 0:433e4ec5de88 | 10 | |
sornpraram | 0:433e4ec5de88 | 11 | } |
sornpraram | 0:433e4ec5de88 | 12 | |
sornpraram | 0:433e4ec5de88 | 13 | void RMD::status_velocity(){ |
sornpraram | 0:433e4ec5de88 | 14 | if (read(msg)){ |
sornpraram | 1:5c31dcbaa375 | 15 | |
sornpraram | 2:d1f33fc9383b | 16 | int fv = (((uint16_t)msg.data[5] << 8) + ((uint16_t)msg.data[4])) / ((uint16_t)6); |
sornpraram | 0:433e4ec5de88 | 17 | |
sornpraram | 0:433e4ec5de88 | 18 | printf(" Speed : %d\n", fv); |
sornpraram | 0:433e4ec5de88 | 19 | printf(" Speed high byte: %x\n", msg.data[5]); |
sornpraram | 0:433e4ec5de88 | 20 | printf(" Speed low byte: %x\n", msg.data[4]); |
sornpraram | 4:083894adb121 | 21 | printf(" -------------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 22 | } |
sornpraram | 0:433e4ec5de88 | 23 | |
sornpraram | 0:433e4ec5de88 | 24 | } |
sornpraram | 0:433e4ec5de88 | 25 | |
sornpraram | 5:07e41b6c6692 | 26 | void RMD::status_position(){ |
sornpraram | 5:07e41b6c6692 | 27 | if (read(msg)){ |
sornpraram | 5:07e41b6c6692 | 28 | |
sornpraram | 5:07e41b6c6692 | 29 | int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[6])) / ((uint16_t)6); |
sornpraram | 5:07e41b6c6692 | 30 | |
sornpraram | 5:07e41b6c6692 | 31 | printf(" Position : %d\n", fv); |
sornpraram | 5:07e41b6c6692 | 32 | printf(" Position high byte: %x\n", msg.data[7]); |
sornpraram | 5:07e41b6c6692 | 33 | printf(" Position low byte: %x\n", msg.data[6]); |
sornpraram | 5:07e41b6c6692 | 34 | printf(" -------------------------- \n"); |
sornpraram | 5:07e41b6c6692 | 35 | } |
sornpraram | 5:07e41b6c6692 | 36 | |
sornpraram | 5:07e41b6c6692 | 37 | } |
sornpraram | 5:07e41b6c6692 | 38 | |
sornpraram | 0:433e4ec5de88 | 39 | void RMD::status(){ |
sornpraram | 0:433e4ec5de88 | 40 | if (read(msg)){ |
sornpraram | 0:433e4ec5de88 | 41 | printf("Message received: %d\n", msg.id); |
sornpraram | 0:433e4ec5de88 | 42 | printf(" Length: %d\n", msg.len); |
sornpraram | 0:433e4ec5de88 | 43 | printf(" Command: %2x\n", msg.data[0]); |
sornpraram | 0:433e4ec5de88 | 44 | printf(" Motor temp: %2x\n", msg.data[1]); // Null |
sornpraram | 0:433e4ec5de88 | 45 | printf(" Torque current low byte: %2x\n", msg.data[2]); |
sornpraram | 0:433e4ec5de88 | 46 | printf(" Torque current high byte: %2x\n", msg.data[3]); |
sornpraram | 0:433e4ec5de88 | 47 | printf(" Speed low byte: %2x\n", msg.data[4]); |
sornpraram | 0:433e4ec5de88 | 48 | printf(" Speed high byte: %2x\n", msg.data[5]); |
sornpraram | 0:433e4ec5de88 | 49 | printf(" Position low byte: %2x\n", msg.data[6]); |
sornpraram | 0:433e4ec5de88 | 50 | printf(" Position high byte: %2x\n", msg.data[7]); |
sornpraram | 0:433e4ec5de88 | 51 | } |
sornpraram | 0:433e4ec5de88 | 52 | } |
sornpraram | 0:433e4ec5de88 | 53 | |
sornpraram | 0:433e4ec5de88 | 54 | void RMD::int2byte(int num){ |
sornpraram | 0:433e4ec5de88 | 55 | |
sornpraram | 0:433e4ec5de88 | 56 | CANdata[0] = (num >> 24) & 0xFF; |
sornpraram | 0:433e4ec5de88 | 57 | CANdata[1] = (num >> 16) & 0xFF; |
sornpraram | 0:433e4ec5de88 | 58 | CANdata[2] = (num >> 8) & 0xFF; |
sornpraram | 0:433e4ec5de88 | 59 | CANdata[3] = num & 0xFF; |
sornpraram | 0:433e4ec5de88 | 60 | } |
sornpraram | 0:433e4ec5de88 | 61 | |
sornpraram | 0:433e4ec5de88 | 62 | void RMD::read_pid(){ |
sornpraram | 0:433e4ec5de88 | 63 | CANsent[0] = 0x30; |
sornpraram | 0:433e4ec5de88 | 64 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 65 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 66 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 67 | CANsent[4] = 0x00; |
sornpraram | 0:433e4ec5de88 | 68 | CANsent[5] = 0x00; |
sornpraram | 0:433e4ec5de88 | 69 | CANsent[6] = 0x00; |
sornpraram | 0:433e4ec5de88 | 70 | CANsent[7] = 0x00; |
sornpraram | 0:433e4ec5de88 | 71 | //CANsent = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; |
sornpraram | 0:433e4ec5de88 | 72 | write(CANMessage(id, CANsent)); |
sornpraram | 0:433e4ec5de88 | 73 | //_data_back = _can.read(msg); |
sornpraram | 0:433e4ec5de88 | 74 | //return _data_back; |
sornpraram | 0:433e4ec5de88 | 75 | } |
sornpraram | 0:433e4ec5de88 | 76 | |
sornpraram | 0:433e4ec5de88 | 77 | // Speed Unit is Degree per seconds |
sornpraram | 0:433e4ec5de88 | 78 | // Speed Sensitivity is 0.01 dps per LSB (Least Significant Bit) |
sornpraram | 0:433e4ec5de88 | 79 | // Speed Input is dps vary from 32bits[-888,000 (0.01)dps, +888,000 (0.01)dps] (0xFFF27340 , 0x000D8CC0) [32bits is (−2,147,483,648, +2,147,483,647)] |
sornpraram | 0:433e4ec5de88 | 80 | // Speed Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) |
sornpraram | 0:433e4ec5de88 | 81 | // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX) |
sornpraram | 0:433e4ec5de88 | 82 | bool RMD::send_speed(int id, int speed){ |
sornpraram | 0:433e4ec5de88 | 83 | //printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 84 | //printf(" Speed: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 85 | //printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 86 | int2byte(6*speed); |
sornpraram | 0:433e4ec5de88 | 87 | |
sornpraram | 0:433e4ec5de88 | 88 | CANsent[0] = 0xA2; |
sornpraram | 0:433e4ec5de88 | 89 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 90 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 91 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 92 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 93 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 94 | CANsent[6] = CANdata[1]; |
sornpraram | 0:433e4ec5de88 | 95 | CANsent[7] = CANdata[0]; |
sornpraram | 0:433e4ec5de88 | 96 | //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; |
sornpraram | 0:433e4ec5de88 | 97 | |
sornpraram | 0:433e4ec5de88 | 98 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 99 | |
sornpraram | 0:433e4ec5de88 | 100 | return true; |
sornpraram | 0:433e4ec5de88 | 101 | } |
sornpraram | 0:433e4ec5de88 | 102 | else { |
sornpraram | 0:433e4ec5de88 | 103 | return false; |
sornpraram | 0:433e4ec5de88 | 104 | } |
sornpraram | 0:433e4ec5de88 | 105 | |
sornpraram | 0:433e4ec5de88 | 106 | } |
sornpraram | 0:433e4ec5de88 | 107 | |
sornpraram | 0:433e4ec5de88 | 108 | /* |
sornpraram | 0:433e4ec5de88 | 109 | |
sornpraram | 0:433e4ec5de88 | 110 | // Torque Unit is Nm |
sornpraram | 0:433e4ec5de88 | 111 | // Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0) [16bits is (−32768, +32767)] |
sornpraram | 0:433e4ec5de88 | 112 | // Input current mapping to (-32A, 32A) |
sornpraram | 0:433e4ec5de88 | 113 | // Torque Output is Nm vary from () |
sornpraram | 0:433e4ec5de88 | 114 | bool RMD::send_torque(int id, int torque){ |
sornpraram | 0:433e4ec5de88 | 115 | printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 116 | printf(" Torque: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 117 | printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 118 | int2byte(speed); |
sornpraram | 0:433e4ec5de88 | 119 | |
sornpraram | 0:433e4ec5de88 | 120 | CANsent[0] = 0xA1; |
sornpraram | 0:433e4ec5de88 | 121 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 122 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 123 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 124 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 125 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 126 | CANsent[6] = 0x00; |
sornpraram | 0:433e4ec5de88 | 127 | CANsent[7] = 0x00; |
sornpraram | 0:433e4ec5de88 | 128 | //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00}; |
sornpraram | 0:433e4ec5de88 | 129 | |
sornpraram | 0:433e4ec5de88 | 130 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 131 | return true; |
sornpraram | 0:433e4ec5de88 | 132 | } |
sornpraram | 0:433e4ec5de88 | 133 | else { |
sornpraram | 0:433e4ec5de88 | 134 | return false; |
sornpraram | 0:433e4ec5de88 | 135 | } |
sornpraram | 0:433e4ec5de88 | 136 | } |
sornpraram | 0:433e4ec5de88 | 137 | |
sornpraram | 0:433e4ec5de88 | 138 | bool RMD::send_position(int id, int position){ |
sornpraram | 0:433e4ec5de88 | 139 | printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 140 | printf(" Speed: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 141 | printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 142 | int2byte(speed*6); |
sornpraram | 0:433e4ec5de88 | 143 | |
sornpraram | 0:433e4ec5de88 | 144 | CANsent[0] = 0xA3; |
sornpraram | 0:433e4ec5de88 | 145 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 146 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 147 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 148 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 149 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 150 | CANsent[6] = CANdata[1]; |
sornpraram | 0:433e4ec5de88 | 151 | CANsent[7] = CANdata[0]; |
sornpraram | 0:433e4ec5de88 | 152 | //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; |
sornpraram | 0:433e4ec5de88 | 153 | |
sornpraram | 0:433e4ec5de88 | 154 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 155 | return true; |
sornpraram | 0:433e4ec5de88 | 156 | } |
sornpraram | 0:433e4ec5de88 | 157 | else { |
sornpraram | 0:433e4ec5de88 | 158 | return false; |
sornpraram | 0:433e4ec5de88 | 159 | } |
sornpraram | 0:433e4ec5de88 | 160 | } |
sornpraram | 0:433e4ec5de88 | 161 | |
sornpraram | 0:433e4ec5de88 | 162 | */ |