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@19:51558e307b28, 2020-11-04 (annotated)

Committer:
sornpraram
Date:
Wed Nov 04 14:34:47 2020 +0000
Revision:
19:51558e307b28
Parent:
18:8a26f917ad56
Child:
20:0756a01f7415
123

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