sornpraram xu / RMD

RMD-X Motor Library - Last Update V0.5 - On going...... Current available Function - read velocity - read all data - send velocity - send position

RMD.cpp@20:0756a01f7415, 2020-12-19 (annotated)

Committer:
sornpraram
Date:
Sat Dec 19 08:43:14 2020 +0000
Revision:
20:0756a01f7415
Parent:
19:51558e307b28 
Final V0.5

Who changed what in which revision?

UserRevisionLine numberNew 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 20:0756a01f7415 8 // CAN BUS connection
sornpraram 0:433e4ec5de88 9 RMD::RMD(PinName rx, PinName tx) : CAN(rx,tx,1000000) {
sornpraram 0:433e4ec5de88 10 frequency(1000000);
sornpraram 0:433e4ec5de88 11
sornpraram 0:433e4ec5de88 12 }
sornpraram 0:433e4ec5de88 13
sornpraram 20:0756a01f7415 14 // Reading current velocity output (degree per sec)
sornpraram 13:09e4446b41cc 15 int RMD::status_velocity(){
sornpraram 0:433e4ec5de88 16 if (read(msg)){
sornpraram 1:5c31dcbaa375 17
sornpraram 2:d1f33fc9383b 18 int fv = (((uint16_t)msg.data[5] << 8) + ((uint16_t)msg.data[4])) / ((uint16_t)6);
sornpraram 19:51558e307b28 19 return fv;
sornpraram 19:51558e307b28 20 }
sornpraram 19:51558e307b28 21 else (){
sornpraram 19:51558e307b28 22 return 0;
sornpraram 19:51558e307b28 23 }
sornpraram 0:433e4ec5de88 24 }
sornpraram 0:433e4ec5de88 25
sornpraram 20:0756a01f7415 26 // Reading current position output (degree)
sornpraram 5:07e41b6c6692 27 void RMD::status_position(){
sornpraram 5:07e41b6c6692 28 if (read(msg)){
sornpraram 5:07e41b6c6692 29
sornpraram 9:defd39d3fdf1 30 int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[6]));
sornpraram 20:0756a01f7415 31 return fv;
sornpraram 5:07e41b6c6692 32 }
sornpraram 5:07e41b6c6692 33 }
sornpraram 5:07e41b6c6692 34
sornpraram 20:0756a01f7415 35 // Reading everything
sornpraram 0:433e4ec5de88 36 void RMD::status(){
sornpraram 0:433e4ec5de88 37 if (read(msg)){
sornpraram 0:433e4ec5de88 38 printf("Message received: %d\n", msg.id);
sornpraram 0:433e4ec5de88 39 printf(" Length: %d\n", msg.len);
sornpraram 0:433e4ec5de88 40 printf(" Command: %2x\n", msg.data[0]);
sornpraram 0:433e4ec5de88 41 printf(" Motor temp: %2x\n", msg.data[1]); // Null
sornpraram 0:433e4ec5de88 42 printf(" Torque current low byte: %2x\n", msg.data[2]);
sornpraram 0:433e4ec5de88 43 printf(" Torque current high byte: %2x\n", msg.data[3]);
sornpraram 0:433e4ec5de88 44 printf(" Speed low byte: %2x\n", msg.data[4]);
sornpraram 0:433e4ec5de88 45 printf(" Speed high byte: %2x\n", msg.data[5]);
sornpraram 0:433e4ec5de88 46 printf(" Position low byte: %2x\n", msg.data[6]);
sornpraram 0:433e4ec5de88 47 printf(" Position high byte: %2x\n", msg.data[7]);
sornpraram 0:433e4ec5de88 48 }
sornpraram 0:433e4ec5de88 49 }
sornpraram 0:433e4ec5de88 50
sornpraram 20:0756a01f7415 51 // Helper function for convert int to byte (used inside library only)
sornpraram 0:433e4ec5de88 52 void RMD::int2byte(int num){
sornpraram 0:433e4ec5de88 53 CANdata[0] = (num >> 24) & 0xFF;
sornpraram 0:433e4ec5de88 54 CANdata[1] = (num >> 16) & 0xFF;
sornpraram 0:433e4ec5de88 55 CANdata[2] = (num >> 8) & 0xFF;
sornpraram 0:433e4ec5de88 56 CANdata[3] = num & 0xFF;
sornpraram 0:433e4ec5de88 57 }
sornpraram 0:433e4ec5de88 58
sornpraram 20:0756a01f7415 59 // Helper function for convert int to byte (used inside library only)
sornpraram 10:5221a88e2240 60 void RMD::int2byte2(int pos, int speed){
sornpraram 11:9ba7b854016f 61 CANdata[0] = speed & 0xFF;
sornpraram 11:9ba7b854016f 62 CANdata[1] = (speed >> 8) & 0xFF;
sornpraram 11:9ba7b854016f 63 CANdata[2] = pos & 0xFF;
sornpraram 11:9ba7b854016f 64 CANdata[3] = (pos >> 8) & 0xFF;
sornpraram 11:9ba7b854016f 65 CANdata[4] = (pos >> 16) & 0xFF;
sornpraram 11:9ba7b854016f 66 CANdata[5] = (pos >> 24) & 0xFF;
sornpraram 0:433e4ec5de88 67 }
sornpraram 0:433e4ec5de88 68
sornpraram 20:0756a01f7415 69 // Drive speed. Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity)
sornpraram 0:433e4ec5de88 70 // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX)
sornpraram 0:433e4ec5de88 71 bool RMD::send_speed(int id, int speed){
sornpraram 20:0756a01f7415 72 int2byte(6*speed);
sornpraram 0:433e4ec5de88 73 CANsent[0] = 0xA2;
sornpraram 0:433e4ec5de88 74 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 75 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 76 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 77 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 78 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 79 CANsent[6] = CANdata[1];
sornpraram 0:433e4ec5de88 80 CANsent[7] = CANdata[0];
sornpraram 0:433e4ec5de88 81 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 82 return true;
sornpraram 0:433e4ec5de88 83 }
sornpraram 0:433e4ec5de88 84 else {
sornpraram 0:433e4ec5de88 85 return false;
sornpraram 0:433e4ec5de88 86 }
sornpraram 0:433e4ec5de88 87 }
sornpraram 0:433e4ec5de88 88
sornpraram 20:0756a01f7415 89 // Drive position. Output is degree vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity)
sornpraram 20:0756a01f7415 90 // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX)
sornpraram 10:5221a88e2240 91 bool RMD::send_position(int id, int position, int speed){
sornpraram 10:5221a88e2240 92 int2byte2(position, 6*speed);
sornpraram 11:9ba7b854016f 93 CANsent[0] = 0xA4;
sornpraram 10:5221a88e2240 94 CANsent[1] = 0x00;
sornpraram 11:9ba7b854016f 95 CANsent[2] = CANdata[0];
sornpraram 11:9ba7b854016f 96 CANsent[3] = CANdata[1];
sornpraram 11:9ba7b854016f 97 CANsent[4] = CANdata[2];
sornpraram 11:9ba7b854016f 98 CANsent[5] = CANdata[3];
sornpraram 11:9ba7b854016f 99 CANsent[6] = CANdata[4];
sornpraram 11:9ba7b854016f 100 CANsent[7] = CANdata[5];
sornpraram 10:5221a88e2240 101 //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
sornpraram 10:5221a88e2240 102
sornpraram 10:5221a88e2240 103 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 10:5221a88e2240 104
sornpraram 10:5221a88e2240 105 return true;
sornpraram 10:5221a88e2240 106 }
sornpraram 10:5221a88e2240 107 else {
sornpraram 10:5221a88e2240 108 return false;
sornpraram 10:5221a88e2240 109 }
sornpraram 10:5221a88e2240 110 }
sornpraram 0:433e4ec5de88 111 /*
sornpraram 0:433e4ec5de88 112
sornpraram 0:433e4ec5de88 113 // Torque Unit is Nm
sornpraram 0:433e4ec5de88 114 // Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0) [16bits is (−32768, +32767)]
sornpraram 0:433e4ec5de88 115 // Input current mapping to (-32A, 32A)
sornpraram 0:433e4ec5de88 116 // Torque Output is Nm vary from ()
sornpraram 0:433e4ec5de88 117 bool RMD::send_torque(int id, int torque){
sornpraram 0:433e4ec5de88 118 printf(" ID: %d\n", id);
sornpraram 0:433e4ec5de88 119 printf(" Torque: %d\n", speed);
sornpraram 0:433e4ec5de88 120 printf(" ----------------------- \n");
sornpraram 0:433e4ec5de88 121 int2byte(speed);
sornpraram 0:433e4ec5de88 122
sornpraram 0:433e4ec5de88 123 CANsent[0] = 0xA1;
sornpraram 0:433e4ec5de88 124 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 125 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 126 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 127 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 128 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 129 CANsent[6] = 0x00;
sornpraram 0:433e4ec5de88 130 CANsent[7] = 0x00;
sornpraram 0:433e4ec5de88 131 //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00};
sornpraram 0:433e4ec5de88 132
sornpraram 0:433e4ec5de88 133 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 134 return true;
sornpraram 0:433e4ec5de88 135 }
sornpraram 0:433e4ec5de88 136 else {
sornpraram 0:433e4ec5de88 137 return false;
sornpraram 0:433e4ec5de88 138 }
sornpraram 0:433e4ec5de88 139 }
sornpraram 0:433e4ec5de88 140
sornpraram 0:433e4ec5de88 141 bool RMD::send_position(int id, int position){
sornpraram 0:433e4ec5de88 142 printf(" ID: %d\n", id);
sornpraram 0:433e4ec5de88 143 printf(" Speed: %d\n", speed);
sornpraram 0:433e4ec5de88 144 printf(" ----------------------- \n");
sornpraram 0:433e4ec5de88 145 int2byte(speed*6);
sornpraram 0:433e4ec5de88 146
sornpraram 0:433e4ec5de88 147 CANsent[0] = 0xA3;
sornpraram 0:433e4ec5de88 148 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 149 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 150 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 151 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 152 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 153 CANsent[6] = CANdata[1];
sornpraram 0:433e4ec5de88 154 CANsent[7] = CANdata[0];
sornpraram 0:433e4ec5de88 155 //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
sornpraram 0:433e4ec5de88 156
sornpraram 0:433e4ec5de88 157 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 158 return true;
sornpraram 0:433e4ec5de88 159 }
sornpraram 0:433e4ec5de88 160 else {
sornpraram 0:433e4ec5de88 161 return false;
sornpraram 0:433e4ec5de88 162 }
sornpraram 0:433e4ec5de88 163 }
sornpraram 0:433e4ec5de88 164
sornpraram 0:433e4ec5de88 165 */