6축 힘/토크 센서 - CAN

#*********************************************************************** # # Author: AIDIN ROBOTICS <info@aidinrobotics.com> # Date: 12-08-2023 # # ---------------------------------------------------------------------- # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation; either version 2 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; # #***********************************************************************/ import struct import socket IP_ADDR = '192.168.0.223' PORT = 4001 CMD_TYPE_SENSOR_ID = '01' SENSOR_TRANSMIT_TYPE_MODE = '03' SENSOR_TRANSMIT_TYPE_SET_TEMP = '01' s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) def main(): global s s.settimeout(2.0) s.connect((IP_ADDR, PORT)) sendData = '04' + '00' + '00' + '01' + '02'+ '06' + CMD_TYPE_SENSOR_ID + SENSOR_TRANSMIT_TYPE_MODE+SENSOR_TRANSMIT_TYPE_SET_TEMP sendData = bytearray.fromhex(sendData) s.send(sendData) recvData = recvMsg() #Initializing Fx=0 Fy=0 Fz=0 Tx=0 Ty=0 Tz=0 for i in range(10000): recvData = recvMsg() if recvData[4]==0x01: Fx = ((recvData[6]*256+recvData[7])/100.)-300. Fy = ((recvData[8]*256+recvData[9])/100.)-300. Fz = ((recvData[10]*256+recvData[11])/100.)-300. # print("Fx : " + str(round(Fx,2)) + " " + "Fy : " + str(round(Fy,2)) + " " + "Fz : " + str(round(Fz,2)) + " ") elif recvData[4]==0x02: Tx = ((recvData[6]*256+recvData[7])/500.)-50. Ty = ((recvData[8]*256+recvData[9])/500.)-50. Tz = ((recvData[10]*256+recvData[11])/500.)-50. # print("Tx : " + str(round(Tx,2)) + " " + "Ty : " + str(round(Ty,2)) + " " + "Tz : " + str(round(Tz,2)) + " ") print("Fx : " + str(round(Fx,2)) + " " + "Fy : " + str(round(Fy,2)) + " " + "Fz : " + str(round(Fz,2)) + " " +"Tx : " + str(round(Tx,2)) + " " + "Ty : " + str(round(Ty,2)) + " " + "Tz : " + str(round(Tz,2)) + " ") s.close() def recvMsg(): recvData = bytearray(s.recv(14)) printMsg(recvData) return recvData def printMsg(msg): dataStr = "DATA: " for i in range(6, 14): dataStr += str(msg[i]) + " " # print(dataStr) if __name__ == "__main__": main()

/*********************************************************************** * * Author: AIDIN ROBOTICS <info@aidinrobotics.com> * Date: 12-08-2023 * * ---------------------------------------------------------------------- * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; * ************************************************************************/ //Std #pragma comment(lib, "ws2_32.lib") #include <iostream> //Windows Socket #include <winsock2.h> #include <ws2tcpip.h> #define IP_ADDR "192.168.0.223" #define PORT 4001 #define CMD_TYPE_SENSOR_ID 0x01 #define SENSOR_TRANSMIT_TYPE_MODE 0x03 #define SENSOR_TRANSMIT_TYPE_SET_TEMP 0x01 void printMsg(const char *msg); void recvMsg(char *msg); double bytesToDouble(const char *bytes); SOCKET socket_desc; int main() { WSADATA wsadata; int error = WSAStartup(0x0202, &wsadata); if (wsadata.wVersion != 0x0202) { WSACleanup(); //Clean up Winsock return false; } socket_desc = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); if (socket_desc < 0) std::cout << "error opening socket" << std::endl; struct sockaddr_in server; server.sin_addr.s_addr = inet_addr(IP_ADDR); server.sin_family = AF_INET; server.sin_port = htons(PORT); std::cout << "Connecting..." << std::endl; if(error = connect(socket_desc , (SOCKADDR*)&server , sizeof(server))) { std::cout << "Error code: " << error << std::endl; return 1; } std::cout << "Connected to the AFT200-D80 Module" << std::endl; char recvData[50] = {0}; char sendData[10] = {0}; sendData[0] = 0x04; sendData[1] = 0x00; sendData[2] = 0x00; sendData[3] = 0x01; sendData[4] = 0x02; sendData[5] = 0x06; sendData[6] = CMD_TYPE_SENSOR_ID; sendData[7] = SENSOR_TRANSMIT_TYPE_MODE; sendData[8] = SENSOR_TRANSMIT_TYPE_SET_TEMP; send(socket_desc, sendData, 10, 0); recvMsg(recvData); double Fx, Fy, Fz, Tx, Ty, Tz; for(size_t i = 0; i < 10000; ++i) { recvMsg(recvData); if(recvData[4]==0x01){ Fx = ((recvData[6]*256+recvData[7])/100.)-300.; Fy = ((recvData[8]*256+recvData[9])/100.)-300.; Fz = ((recvData[10]*256+recvData[11])/100.)-300.; } else if(recvData[4]==0x02){ Tx = ((recvData[6]*256+recvData[7])/500.)-50.; Ty = ((recvData[8]*256+recvData[9])/500.)-50.; Tz = ((recvData[10]*256+recvData[11])/500.)-50.; } std::cout << Fx << " " << Fy << " " << Fz << " " << Tx << " " << Ty << " " << Tz << std::endl; } closesocket(socket_desc); return 0; } void recvMsg(char *msg) { size_t received = recv(socket_desc, msg, 14, 0); printMsg(msg); } void printMsg(const char *msg) { for(size_t i = 6; i < 14; ++i) { //std::cout << (int)msg[i] << " "; } //std::cout << std::endl << std::endl; };

/*********************************************************************** * * Author: AIDIN ROBOTICS <info@aidinrobotics.com> * Date: 12-08-2023 * * ---------------------------------------------------------------------- * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; * ************************************************************************/ //Std #include <iostream> //Linux Socket #include <sys/socket.h> #include <arpa/inet.h> #include <unistd.h> #define IP_ADDR "192.168.0.223" #define PORT 4001 #define CMD_TYPE_SENSOR_ID 0x01 #define SENSOR_TRANSMIT_TYPE_MODE 0x03 #define SENSOR_TRANSMIT_TYPE_SET_TEMP 0x01 void printMsg(const char *msg); void recvMsg(char *msg); double bytesToDouble(const char *bytes); int socket_desc; int main() { socket_desc = socket(AF_INET, SOCK_STREAM, 0); struct sockaddr_in server; server.sin_addr.s_addr = inet_addr(IP_ADDR); server.sin_family = AF_INET; server.sin_port = htons(PORT); if(connect(socket_desc , (struct sockaddr *)&server , sizeof(server)) < 0) { return 1; } std::cout << "Connected to the CANToEth Module" << std::endl; char recvData[50] = {0}; char sendData[10] = {0}; sendData[0] = 0x04; sendData[1] = 0x00; sendData[2] = 0x00; sendData[3] = 0x01; sendData[4] = 0x02; sendData[5] = 0x06; sendData[6] = CMD_TYPE_SENSOR_ID; sendData[7] = SENSOR_TRANSMIT_TYPE_MODE; sendData[8] = SENSOR_TRANSMIT_TYPE_SET_TEMP; send(socket_desc, sendData, 10, 0); recvMsg(recvData); double Fx, Fy, Fz, Tx, Ty, Tz; for(size_t i = 0; i < 10000; ++i) { recvMsg(recvData); if(recvData[4]==0x01){ Fx = ((recvData[6]*256+recvData[7])/100.)-300.; Fy = ((recvData[8]*256+recvData[9])/100.)-300.; Fz = ((recvData[10]*256+recvData[11])/100.)-300.; } else if(recvData[4]==0x02){ Tx = ((recvData[6]*256+recvData[7])/500.)-50.; Ty = ((recvData[8]*256+recvData[9])/500.)-50.; Tz = ((recvData[10]*256+recvData[11])/500.)-50.; } std::cout << Fx << " " << Fy << " " << Fz << " " << Tx << " " << Ty << " " << Tz << std::endl; } close(socket_desc); return 0; } void recvMsg(char *msg) { size_t received = recv(socket_desc, msg, 14, 0); printMsg(msg); } void printMsg(const char *msg) { for(size_t i = 6; i < 14; ++i) { //std::cout << (int)msg[i] << " "; } //std::cout << std::endl << std::endl; };