RMD-X Motor Library - Last Update V0.5 - On going...... Current available Function - read velocity - read all data - send velocity - send position
RMD.cpp@11:9ba7b854016f, 2020-11-04 (annotated)
- Committer:
- sornpraram
- Date:
- Wed Nov 04 11:31:36 2020 +0000
- Revision:
- 11:9ba7b854016f
- Parent:
- 10:5221a88e2240
- Child:
- 13:09e4446b41cc
s
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 | 7:2f2c3f244208 | 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 | 9:defd39d3fdf1 | 29 | int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[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 | 8:82b39d6566c2 | 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 | 10:5221a88e2240 | 62 | void RMD::int2byte2(int pos, int speed){ |
sornpraram | 10:5221a88e2240 | 63 | |
sornpraram | 11:9ba7b854016f | 64 | CANdata[0] = speed & 0xFF; |
sornpraram | 11:9ba7b854016f | 65 | CANdata[1] = (speed >> 8) & 0xFF; |
sornpraram | 11:9ba7b854016f | 66 | CANdata[2] = pos & 0xFF; |
sornpraram | 11:9ba7b854016f | 67 | CANdata[3] = (pos >> 8) & 0xFF; |
sornpraram | 11:9ba7b854016f | 68 | CANdata[4] = (pos >> 16) & 0xFF; |
sornpraram | 11:9ba7b854016f | 69 | CANdata[5] = (pos >> 24) & 0xFF; |
sornpraram | 0:433e4ec5de88 | 70 | } |
sornpraram | 0:433e4ec5de88 | 71 | |
sornpraram | 10:5221a88e2240 | 72 | |
sornpraram | 0:433e4ec5de88 | 73 | // Speed Unit is Degree per seconds |
sornpraram | 0:433e4ec5de88 | 74 | // Speed Sensitivity is 0.01 dps per LSB (Least Significant Bit) |
sornpraram | 0:433e4ec5de88 | 75 | // 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 | 76 | // Speed Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) |
sornpraram | 0:433e4ec5de88 | 77 | // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX) |
sornpraram | 0:433e4ec5de88 | 78 | bool RMD::send_speed(int id, int speed){ |
sornpraram | 0:433e4ec5de88 | 79 | //printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 80 | //printf(" Speed: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 81 | //printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 82 | int2byte(6*speed); |
sornpraram | 0:433e4ec5de88 | 83 | |
sornpraram | 0:433e4ec5de88 | 84 | CANsent[0] = 0xA2; |
sornpraram | 0:433e4ec5de88 | 85 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 86 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 87 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 88 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 89 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 90 | CANsent[6] = CANdata[1]; |
sornpraram | 0:433e4ec5de88 | 91 | CANsent[7] = CANdata[0]; |
sornpraram | 0:433e4ec5de88 | 92 | //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; |
sornpraram | 0:433e4ec5de88 | 93 | |
sornpraram | 0:433e4ec5de88 | 94 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 95 | |
sornpraram | 0:433e4ec5de88 | 96 | return true; |
sornpraram | 0:433e4ec5de88 | 97 | } |
sornpraram | 0:433e4ec5de88 | 98 | else { |
sornpraram | 0:433e4ec5de88 | 99 | return false; |
sornpraram | 0:433e4ec5de88 | 100 | } |
sornpraram | 0:433e4ec5de88 | 101 | } |
sornpraram | 0:433e4ec5de88 | 102 | |
sornpraram | 10:5221a88e2240 | 103 | bool RMD::send_position(int id, int position, int speed){ |
sornpraram | 10:5221a88e2240 | 104 | |
sornpraram | 10:5221a88e2240 | 105 | int2byte2(position, 6*speed); |
sornpraram | 10:5221a88e2240 | 106 | |
sornpraram | 11:9ba7b854016f | 107 | CANsent[0] = 0xA4; |
sornpraram | 10:5221a88e2240 | 108 | CANsent[1] = 0x00; |
sornpraram | 11:9ba7b854016f | 109 | CANsent[2] = CANdata[0]; |
sornpraram | 11:9ba7b854016f | 110 | CANsent[3] = CANdata[1]; |
sornpraram | 11:9ba7b854016f | 111 | CANsent[4] = CANdata[2]; |
sornpraram | 11:9ba7b854016f | 112 | CANsent[5] = CANdata[3]; |
sornpraram | 11:9ba7b854016f | 113 | CANsent[6] = CANdata[4]; |
sornpraram | 11:9ba7b854016f | 114 | CANsent[7] = CANdata[5]; |
sornpraram | 10:5221a88e2240 | 115 | //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; |
sornpraram | 10:5221a88e2240 | 116 | |
sornpraram | 10:5221a88e2240 | 117 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 10:5221a88e2240 | 118 | |
sornpraram | 10:5221a88e2240 | 119 | return true; |
sornpraram | 10:5221a88e2240 | 120 | } |
sornpraram | 10:5221a88e2240 | 121 | else { |
sornpraram | 10:5221a88e2240 | 122 | return false; |
sornpraram | 10:5221a88e2240 | 123 | } |
sornpraram | 10:5221a88e2240 | 124 | } |
sornpraram | 0:433e4ec5de88 | 125 | /* |
sornpraram | 0:433e4ec5de88 | 126 | |
sornpraram | 0:433e4ec5de88 | 127 | // Torque Unit is Nm |
sornpraram | 0:433e4ec5de88 | 128 | // Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0) [16bits is (−32768, +32767)] |
sornpraram | 0:433e4ec5de88 | 129 | // Input current mapping to (-32A, 32A) |
sornpraram | 0:433e4ec5de88 | 130 | // Torque Output is Nm vary from () |
sornpraram | 0:433e4ec5de88 | 131 | bool RMD::send_torque(int id, int torque){ |
sornpraram | 0:433e4ec5de88 | 132 | printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 133 | printf(" Torque: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 134 | printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 135 | int2byte(speed); |
sornpraram | 0:433e4ec5de88 | 136 | |
sornpraram | 0:433e4ec5de88 | 137 | CANsent[0] = 0xA1; |
sornpraram | 0:433e4ec5de88 | 138 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 139 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 140 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 141 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 142 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 143 | CANsent[6] = 0x00; |
sornpraram | 0:433e4ec5de88 | 144 | CANsent[7] = 0x00; |
sornpraram | 0:433e4ec5de88 | 145 | //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00}; |
sornpraram | 0:433e4ec5de88 | 146 | |
sornpraram | 0:433e4ec5de88 | 147 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 148 | return true; |
sornpraram | 0:433e4ec5de88 | 149 | } |
sornpraram | 0:433e4ec5de88 | 150 | else { |
sornpraram | 0:433e4ec5de88 | 151 | return false; |
sornpraram | 0:433e4ec5de88 | 152 | } |
sornpraram | 0:433e4ec5de88 | 153 | } |
sornpraram | 0:433e4ec5de88 | 154 | |
sornpraram | 0:433e4ec5de88 | 155 | bool RMD::send_position(int id, int position){ |
sornpraram | 0:433e4ec5de88 | 156 | printf(" ID: %d\n", id); |
sornpraram | 0:433e4ec5de88 | 157 | printf(" Speed: %d\n", speed); |
sornpraram | 0:433e4ec5de88 | 158 | printf(" ----------------------- \n"); |
sornpraram | 0:433e4ec5de88 | 159 | int2byte(speed*6); |
sornpraram | 0:433e4ec5de88 | 160 | |
sornpraram | 0:433e4ec5de88 | 161 | CANsent[0] = 0xA3; |
sornpraram | 0:433e4ec5de88 | 162 | CANsent[1] = 0x00; |
sornpraram | 0:433e4ec5de88 | 163 | CANsent[2] = 0x00; |
sornpraram | 0:433e4ec5de88 | 164 | CANsent[3] = 0x00; |
sornpraram | 0:433e4ec5de88 | 165 | CANsent[4] = CANdata[3]; |
sornpraram | 0:433e4ec5de88 | 166 | CANsent[5] = CANdata[2]; |
sornpraram | 0:433e4ec5de88 | 167 | CANsent[6] = CANdata[1]; |
sornpraram | 0:433e4ec5de88 | 168 | CANsent[7] = CANdata[0]; |
sornpraram | 0:433e4ec5de88 | 169 | //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; |
sornpraram | 0:433e4ec5de88 | 170 | |
sornpraram | 0:433e4ec5de88 | 171 | if (write(CANMessage(id, CANsent)) == true) { |
sornpraram | 0:433e4ec5de88 | 172 | return true; |
sornpraram | 0:433e4ec5de88 | 173 | } |
sornpraram | 0:433e4ec5de88 | 174 | else { |
sornpraram | 0:433e4ec5de88 | 175 | return false; |
sornpraram | 0:433e4ec5de88 | 176 | } |
sornpraram | 0:433e4ec5de88 | 177 | } |
sornpraram | 0:433e4ec5de88 | 178 | |
sornpraram | 0:433e4ec5de88 | 179 | */ |