From 7cd5954fe50fc5428fc3f06ac72900872e76fc64 Mon Sep 17 00:00:00 2001 From: Siwat Sirichai Date: Tue, 1 Nov 2022 21:48:43 +0700 Subject: [PATCH] initial commit --- VibAuto.py | 71 ++++++++ VibRecord.py | 71 ++++++++ vibrator_firmware/.gitignore | 5 + vibrator_firmware/.vscode/extensions.json | 10 ++ vibrator_firmware/include/README | 39 ++++ vibrator_firmware/lib/README | 46 +++++ vibrator_firmware/platformio.ini | 15 ++ vibrator_firmware/src/main.cpp | 207 ++++++++++++++++++++++ vibrator_firmware/test/README | 11 ++ 9 files changed, 475 insertions(+) create mode 100644 VibAuto.py create mode 100644 VibRecord.py create mode 100644 vibrator_firmware/.gitignore create mode 100644 vibrator_firmware/.vscode/extensions.json create mode 100644 vibrator_firmware/include/README create mode 100644 vibrator_firmware/lib/README create mode 100644 vibrator_firmware/platformio.ini create mode 100644 vibrator_firmware/src/main.cpp create mode 100644 vibrator_firmware/test/README diff --git a/VibAuto.py b/VibAuto.py new file mode 100644 index 0000000..8b436ac --- /dev/null +++ b/VibAuto.py @@ -0,0 +1,71 @@ +import csv +import threading +from time import perf_counter, sleep +import serial +serial = serial.Serial("COM5",baudrate=115200) +sleep(5) +serial.readline() +serial.readline() +serial.readline() + +def set_pos(pos: float): + serial.write(("1"+str(pos)+";").encode("ascii")) +def set_velo(velo: float): + serial.write(("2"+str(velo)+";").encode("ascii")) +def set_Kp(Kp: float): + serial.write(("3"+str(Kp)+";").encode("ascii")) +def set_Kd(Kd: float): + serial.write(("4"+str(Kd)+";").encode("ascii")) +def reset_pos(): + serial.write("5;".encode("ascii")) + +global theta,theta_dot,time_stamp,current,data,start_time +theta = 0 +theta_dot = 0 +time_stamp = 0 +start_time = 0 +current = 0 +data = [] +def decode_packet(packet: bytes): + global theta,theta_dot,time_stamp,current,data,start_time + dp = packet.decode("ascii") + dspt = dp.split(',') + try: + time_stamp = float(dspt[0]) + theta = float(dspt[2]) + theta_dot = float(dspt[3]) + current = float(dspt[4]) + data.append([time_stamp-start_time,theta,theta_dot,current]) + except: + pass + +Kp_set = [0.3,0.4,0.5,1.0,1.5,2.0,3.0,4.0,5.0] +Kd_set = [0.0,0.05,0.1,0.15,0.20,0.25,0.30,0.35] +x_step = 2.0 +time_step = 5 + +def read_serial(): + global theta,theta_dot,time_stamp,current,data,start_time + while serial.in_waiting: + decode_packet(serial.readline()) +threading.Thread(target=read_serial).start() + +for Kd in Kd_set: + for Kp in Kp_set: + dset = [] + set_Kd(Kd) + set_Kp(Kp) + reset_pos() + set_pos(x_step) + start_time = time_stamp; + perf_start = perf_counter() + print("Performing Experiment for Kp = "+str(Kp)+", Kd = "+str(Kd)) + while perf_counter()-perf_start < time_step: + sleep(0.01) + f = open("vib_result_kp"+str(Kp)+"_kd"+str(Kd)+".csv", 'w', newline='') + writer = csv.writer(f) + writer.writerows(data) + f.close() + data = [] +print("Program Completed!, Data Saved to CSV") + diff --git a/VibRecord.py b/VibRecord.py new file mode 100644 index 0000000..e6ad871 --- /dev/null +++ b/VibRecord.py @@ -0,0 +1,71 @@ +import csv +import threading +from time import perf_counter, sleep +import serial +serial = serial.Serial("COM5",baudrate=115200) +sleep(5) +serial.readline() +serial.readline() +serial.readline() + +def set_pos(pos: float): + serial.write(("1"+str(pos)+";").encode("ascii")) +def set_velo(velo: float): + serial.write(("2"+str(velo)+";").encode("ascii")) +def set_Kp(Kp: float): + serial.write(("3"+str(Kp)+";").encode("ascii")) +def set_Kd(Kd: float): + serial.write(("4"+str(Kd)+";").encode("ascii")) +def reset_pos(): + serial.write("5;".encode("ascii")) + +global theta,theta_dot,time_stamp,current,data,start_time +theta = 0 +theta_dot = 0 +time_stamp = 0 +start_time = 0 +current = 0 +data = [] +def decode_packet(packet: bytes): + global theta,theta_dot,time_stamp,current,data,start_time + dp = packet.decode("ascii") + dspt = dp.split(',') + try: + time_stamp = float(dspt[0]) + theta = float(dspt[2]) + theta_dot = float(dspt[3]) + current = float(dspt[4]) + data.append([time_stamp-start_time,theta,theta_dot,current]) + except: + pass + + + +def read_serial(): + global theta,theta_dot,time_stamp,current,data,start_time + while serial.in_waiting: + decode_packet(serial.readline()) +threading.Thread(target=read_serial).start() + +x_step = 2.0 +record_dur = 10 +Kp = 0 +Kd = 0 + + +dset = [] +set_Kd(Kd) +set_Kp(Kp) +reset_pos() +set_pos(x_step) +start_time = time_stamp; +perf_start = perf_counter() +print("Recording Experiment for Kp = "+str(Kp)+", Kd = "+str(Kd)) +while perf_counter()-perf_start < record_dur: + sleep(0.01) +f = open("vib_freespin_result_kp"+str(Kp)+"_kd"+str(Kd)+".csv", 'w', newline='') +writer = csv.writer(f) +writer.writerows(data) +f.close() +print("Program Completed!, Data Saved to CSV") + diff --git a/vibrator_firmware/.gitignore b/vibrator_firmware/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/vibrator_firmware/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/vibrator_firmware/.vscode/extensions.json b/vibrator_firmware/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/vibrator_firmware/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/vibrator_firmware/include/README b/vibrator_firmware/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/vibrator_firmware/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/vibrator_firmware/lib/README b/vibrator_firmware/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/vibrator_firmware/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/vibrator_firmware/platformio.ini b/vibrator_firmware/platformio.ini new file mode 100644 index 0000000..9bdbb9f --- /dev/null +++ b/vibrator_firmware/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:uno] +platform = atmelavr +board = uno +framework = arduino +lib_deps = seeed-studio/CAN_BUS_Shield@^2.3.3 \ No newline at end of file diff --git a/vibrator_firmware/src/main.cpp b/vibrator_firmware/src/main.cpp new file mode 100644 index 0000000..54ae8df --- /dev/null +++ b/vibrator_firmware/src/main.cpp @@ -0,0 +1,207 @@ +#include + +// demo: CAN-BUS Shield, send data +// demo: CAN-BUS Shield, receive data with check mode +// loovee@seeed.cc + +//#include + +#include +#define CAN_2515 + +void pack_cmd(); +float read_data(); +unsigned int float_to_uint(float x, float x_min, float x_max, unsigned int bits); +float uint_to_float(unsigned int x_int, float x_min, float x_max, unsigned int bits); +// Set SPI CS Pin according to your hardware + +#if defined(SEEED_WIO_TERMINAL) && defined(CAN_2518FD) +// For Wio Terminal w/ MCP2518FD RPi Hat： +// Channel 0 SPI_CS Pin: BCM 8 +// Channel 1 SPI_CS Pin: BCM 7 +// Interupt Pin: BCM25 +const int SPI_CS_PIN = BCM8; +const int CAN_INT_PIN = BCM25; +#else + +// For Arduino MCP2515 Hat: +// the cs pin of the version after v1.1 is default to D9 +// v0.9b and v1.0 is default D10 +const int SPI_CS_PIN = 9; +const int CAN_INT_PIN = 2; +#endif + +#ifdef CAN_2515 +#include "mcp2515_can.h" +mcp2515_can CAN(SPI_CS_PIN); // Set CS pin +#endif + +// Motor Limits +#define P_MIN -12.5f +#define P_MAX 12.5f +#define V_MIN1 -20.94f // AK70-10 24V +#define V_MAX1 20.94f +#define T_MIN1 -24.8f // AK70-10 +#define T_MAX1 24.8f +#define I_MIN1 -26.1f // AK70-10 +#define I_MAX1 26.1f +#define KP_MIN 0.0f +#define KP_MAX 500.0f +#define KD_MIN 0.0f +#define KD_MAX 5.0f +#define MAP1 2.0f + +float p_des = 0.0; // AK70-10 +float v_des = 0.0; +float t_ff = 0.0; +float kp = 2; +float kd = 0; + +float p = 0.0; + +float tout = 0.0; +unsigned long dT; +unsigned long timeold; // Variable for point time in the last cycle + +// CAN to T Motor +unsigned char stmp[8] = {0, 0, 0, 0, 0, 0, 0, 0}; +byte MotorModeEnt[8] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFC}; +byte MotorModeExt[8] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFD}; +byte MotorSetZero[8] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE}; + + +void setup() { + SERIAL_PORT_MONITOR.begin(115200); + //while(!Serial){}; + //while (!SERIAL_PORT_MONITOR) {} + + while (CAN_OK != CAN.begin(CAN_1000KBPS)) { // init can bus : baudrate = 500k + SERIAL_PORT_MONITOR.println("CAN init fail, retry..."); + delay(100); + } + SERIAL_PORT_MONITOR.println("CAN init ok!"); + + CAN.sendMsgBuf(0x01, 0, 8, MotorModeEnt); + CAN.sendMsgBuf(0x01, 0, 8, MotorSetZero); + delay(1000); + + pack_cmd(); + p = read_data(); + delay(1000); + +} + +void loop() { + if(SERIAL_PORT_MONITOR.available()) { + String sbuff = SERIAL_PORT_MONITOR.readStringUntil(';'); + int cb = sbuff.substring(0,1).toInt(); + if(cb==1) { + float val = sbuff.substring(1,sbuff.length()).toFloat(); + p_des = val; + } + else if(cb==2) { + float val = sbuff.substring(1,sbuff.length()).toFloat(); + v_des = val; + } + else if(cb==3) { + float val = sbuff.substring(1,sbuff.length()).toFloat(); + kp=val; + } + else if(cb==4){ + float val = sbuff.substring(1,sbuff.length()).toFloat(); + kd=val; + } else if(cb==5) { + CAN.sendMsgBuf(0x01, 0, 8, MotorSetZero); + } + + } + dT = millis()-timeold; + tout = tout + (0.001f*dT); + timeold = millis(); + SERIAL_PORT_MONITOR.print("\n"); + SERIAL_PORT_MONITOR.print(tout); + SERIAL_PORT_MONITOR.print(","); + pack_cmd(); + p = read_data(); + delay(20); + + // CAN.sendMsgBuf(0x01, 0, 8, MotorModeExt); +} + +void pack_cmd() { + unsigned int p_int = float_to_uint(p_des,P_MIN,P_MAX,16); + unsigned int v_int = float_to_uint(v_des,V_MIN1,V_MAX1,12); + unsigned int t_int = float_to_uint(t_ff,T_MIN1,T_MAX1,12); + unsigned int kp_int = float_to_uint(kp,KP_MIN,KP_MAX,12); + unsigned int kd_int = float_to_uint(kd,KD_MIN,KD_MAX,12); + byte bufs[8]; + bufs[0] = p_int >> 8; + bufs[1] = p_int & 0xFF; + bufs[2] = v_int >> 4; + bufs[3] = ((v_int & 0xF) << 4) | (kp_int >> 8); + bufs[4] = kp_int & 0xFF; + bufs[5] = kd_int >> 4; + bufs[6] = ((kd_int & 0xF) << 4) | (t_int >> 8); + bufs[7] = t_int & 0xFF; + CAN.sendMsgBuf(0x01, 0, 8, bufs); +} + +float read_data(){ + unsigned char len = 0; + unsigned char buf[8]; + + if (CAN_MSGAVAIL == CAN.checkReceive()) { // check if data coming + + CAN.readMsgBuf(&len, buf); // read data, len: data length, buf: data buf + + //unsigned int canId = CAN.getCanId(); + unsigned int id = buf[0]; + unsigned int p_int2 = (buf[1] << 8) | buf[2]; + unsigned int v_int2 = (buf[3] << 4) | (buf[4] >> 4); + unsigned int i_int2 = ((buf[4] & 0xF) << 8) | buf[5]; + + float p = uint_to_float(p_int2,P_MIN,P_MAX,16); + SERIAL_PORT_MONITOR.print("0x"); + SERIAL_PORT_MONITOR.print(id, HEX); + SERIAL_PORT_MONITOR.print(","); + SERIAL_PORT_MONITOR.print(p); + SERIAL_PORT_MONITOR.print(","); + + float v = uint_to_float(v_int2,V_MIN1,V_MAX1,12); + float i = uint_to_float(i_int2,I_MIN1,I_MAX1,12); + i = MAP1*i; // Return Torque (N-m) + SERIAL_PORT_MONITOR.print(v); + SERIAL_PORT_MONITOR.print(","); + SERIAL_PORT_MONITOR.print(i); + + return p; + } +} + +unsigned int float_to_uint(float x, float x_min, float x_max, unsigned int bits) { + //convert a float to an unsigned int, given range and number of bits /// + float span = x_max-x_min; + float offset = x-x_min; + + unsigned int pgg = 0; + if(bits == 12){ + pgg = (unsigned int)((offset/span)*4095.0f); + } + if(bits == 16){ + pgg = (unsigned int)((offset/span)*65535.0f); + } + return pgg; +} + +float uint_to_float(unsigned int x_int, float x_min, float x_max, unsigned int bits) { + //convert unsingned int to float, given range and number of bits /// + float span = x_max-x_min; + float pgg = 0; + if(bits == 12){ + pgg = (float)(x_int*span/4095.0f) + x_min; + } + if(bits == 16){ + pgg = (float)(x_int*span/65535.0f) + x_min; + } + return pgg; +} diff --git a/vibrator_firmware/test/README b/vibrator_firmware/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/vibrator_firmware/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html