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@0:433e4ec5de88, 2020-11-04 (annotated)

Committer:
sornpraram
Date:
Wed Nov 04 09:20:53 2020 +0000
Revision:
0:433e4ec5de88
Child:
1:5c31dcbaa375
0

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 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 0:433e4ec5de88 15 int v_high = msg.data[5];
sornpraram 0:433e4ec5de88 16 int v_low = msg.data[4];
sornpraram 0:433e4ec5de88 17 v_high = v_high << 8 ;
sornpraram 0:433e4ec5de88 18 int fv = (v_high + v_low)/6;
sornpraram 0:433e4ec5de88 19
sornpraram 0:433e4ec5de88 20 printf(" Speed : %d\n", fv);
sornpraram 0:433e4ec5de88 21 printf(" Speed high byte: %x\n", msg.data[5]);
sornpraram 0:433e4ec5de88 22 printf(" Speed low byte: %x\n", msg.data[4]);
sornpraram 0:433e4ec5de88 23 printf(" --------------------------- \n");
sornpraram 0:433e4ec5de88 24 }
sornpraram 0:433e4ec5de88 25
sornpraram 0:433e4ec5de88 26 }
sornpraram 0:433e4ec5de88 27
sornpraram 0:433e4ec5de88 28 void RMD::status(){
sornpraram 0:433e4ec5de88 29 if (read(msg)){
sornpraram 0:433e4ec5de88 30 printf("Message received: %d\n", msg.id);
sornpraram 0:433e4ec5de88 31 printf(" Length: %d\n", msg.len);
sornpraram 0:433e4ec5de88 32 printf(" Command: %2x\n", msg.data[0]);
sornpraram 0:433e4ec5de88 33 printf(" Motor temp: %2x\n", msg.data[1]); // Null
sornpraram 0:433e4ec5de88 34 printf(" Torque current low byte: %2x\n", msg.data[2]);
sornpraram 0:433e4ec5de88 35 printf(" Torque current high byte: %2x\n", msg.data[3]);
sornpraram 0:433e4ec5de88 36 printf(" Speed low byte: %2x\n", msg.data[4]);
sornpraram 0:433e4ec5de88 37 printf(" Speed high byte: %2x\n", msg.data[5]);
sornpraram 0:433e4ec5de88 38 printf(" Position low byte: %2x\n", msg.data[6]);
sornpraram 0:433e4ec5de88 39 printf(" Position high byte: %2x\n", msg.data[7]);
sornpraram 0:433e4ec5de88 40 }
sornpraram 0:433e4ec5de88 41 }
sornpraram 0:433e4ec5de88 42
sornpraram 0:433e4ec5de88 43 void RMD::int2byte(int num){
sornpraram 0:433e4ec5de88 44
sornpraram 0:433e4ec5de88 45 CANdata[0] = (num >> 24) & 0xFF;
sornpraram 0:433e4ec5de88 46 CANdata[1] = (num >> 16) & 0xFF;
sornpraram 0:433e4ec5de88 47 CANdata[2] = (num >> 8) & 0xFF;
sornpraram 0:433e4ec5de88 48 CANdata[3] = num & 0xFF;
sornpraram 0:433e4ec5de88 49 }
sornpraram 0:433e4ec5de88 50
sornpraram 0:433e4ec5de88 51 void RMD::read_pid(){
sornpraram 0:433e4ec5de88 52 CANsent[0] = 0x30;
sornpraram 0:433e4ec5de88 53 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 54 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 55 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 56 CANsent[4] = 0x00;
sornpraram 0:433e4ec5de88 57 CANsent[5] = 0x00;
sornpraram 0:433e4ec5de88 58 CANsent[6] = 0x00;
sornpraram 0:433e4ec5de88 59 CANsent[7] = 0x00;
sornpraram 0:433e4ec5de88 60 //CANsent = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
sornpraram 0:433e4ec5de88 61 write(CANMessage(id, CANsent));
sornpraram 0:433e4ec5de88 62 //_data_back = _can.read(msg);
sornpraram 0:433e4ec5de88 63 //return _data_back;
sornpraram 0:433e4ec5de88 64 }
sornpraram 0:433e4ec5de88 65
sornpraram 0:433e4ec5de88 66 // Speed Unit is Degree per seconds
sornpraram 0:433e4ec5de88 67 // Speed Sensitivity is 0.01 dps per LSB (Least Significant Bit)
sornpraram 0:433e4ec5de88 68 // 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 69 // Speed Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm)
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 0:433e4ec5de88 72 //printf(" ID: %d\n", id);
sornpraram 0:433e4ec5de88 73 //printf(" Speed: %d\n", speed);
sornpraram 0:433e4ec5de88 74 //printf(" ----------------------- \n");
sornpraram 0:433e4ec5de88 75 int2byte(6*speed);
sornpraram 0:433e4ec5de88 76
sornpraram 0:433e4ec5de88 77 CANsent[0] = 0xA2;
sornpraram 0:433e4ec5de88 78 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 79 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 80 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 81 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 82 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 83 CANsent[6] = CANdata[1];
sornpraram 0:433e4ec5de88 84 CANsent[7] = CANdata[0];
sornpraram 0:433e4ec5de88 85 //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
sornpraram 0:433e4ec5de88 86
sornpraram 0:433e4ec5de88 87 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 88
sornpraram 0:433e4ec5de88 89 return true;
sornpraram 0:433e4ec5de88 90 }
sornpraram 0:433e4ec5de88 91 else {
sornpraram 0:433e4ec5de88 92 return false;
sornpraram 0:433e4ec5de88 93 }
sornpraram 0:433e4ec5de88 94
sornpraram 0:433e4ec5de88 95 }
sornpraram 0:433e4ec5de88 96
sornpraram 0:433e4ec5de88 97 /*
sornpraram 0:433e4ec5de88 98
sornpraram 0:433e4ec5de88 99 // Torque Unit is Nm
sornpraram 0:433e4ec5de88 100 // Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0) [16bits is (−32768, +32767)]
sornpraram 0:433e4ec5de88 101 // Input current mapping to (-32A, 32A)
sornpraram 0:433e4ec5de88 102 // Torque Output is Nm vary from ()
sornpraram 0:433e4ec5de88 103 bool RMD::send_torque(int id, int torque){
sornpraram 0:433e4ec5de88 104 printf(" ID: %d\n", id);
sornpraram 0:433e4ec5de88 105 printf(" Torque: %d\n", speed);
sornpraram 0:433e4ec5de88 106 printf(" ----------------------- \n");
sornpraram 0:433e4ec5de88 107 int2byte(speed);
sornpraram 0:433e4ec5de88 108
sornpraram 0:433e4ec5de88 109 CANsent[0] = 0xA1;
sornpraram 0:433e4ec5de88 110 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 111 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 112 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 113 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 114 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 115 CANsent[6] = 0x00;
sornpraram 0:433e4ec5de88 116 CANsent[7] = 0x00;
sornpraram 0:433e4ec5de88 117 //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00};
sornpraram 0:433e4ec5de88 118
sornpraram 0:433e4ec5de88 119 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 120 return true;
sornpraram 0:433e4ec5de88 121 }
sornpraram 0:433e4ec5de88 122 else {
sornpraram 0:433e4ec5de88 123 return false;
sornpraram 0:433e4ec5de88 124 }
sornpraram 0:433e4ec5de88 125 }
sornpraram 0:433e4ec5de88 126
sornpraram 0:433e4ec5de88 127 bool RMD::send_position(int id, int position){
sornpraram 0:433e4ec5de88 128 printf(" ID: %d\n", id);
sornpraram 0:433e4ec5de88 129 printf(" Speed: %d\n", speed);
sornpraram 0:433e4ec5de88 130 printf(" ----------------------- \n");
sornpraram 0:433e4ec5de88 131 int2byte(speed*6);
sornpraram 0:433e4ec5de88 132
sornpraram 0:433e4ec5de88 133 CANsent[0] = 0xA3;
sornpraram 0:433e4ec5de88 134 CANsent[1] = 0x00;
sornpraram 0:433e4ec5de88 135 CANsent[2] = 0x00;
sornpraram 0:433e4ec5de88 136 CANsent[3] = 0x00;
sornpraram 0:433e4ec5de88 137 CANsent[4] = CANdata[3];
sornpraram 0:433e4ec5de88 138 CANsent[5] = CANdata[2];
sornpraram 0:433e4ec5de88 139 CANsent[6] = CANdata[1];
sornpraram 0:433e4ec5de88 140 CANsent[7] = CANdata[0];
sornpraram 0:433e4ec5de88 141 //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
sornpraram 0:433e4ec5de88 142
sornpraram 0:433e4ec5de88 143 if (write(CANMessage(id, CANsent)) == true) {
sornpraram 0:433e4ec5de88 144 return true;
sornpraram 0:433e4ec5de88 145 }
sornpraram 0:433e4ec5de88 146 else {
sornpraram 0:433e4ec5de88 147 return false;
sornpraram 0:433e4ec5de88 148 }
sornpraram 0:433e4ec5de88 149 }
sornpraram 0:433e4ec5de88 150
sornpraram 0:433e4ec5de88 151 */