initial commit
This commit is contained in:
commit
7cd5954fe5
|
|
@ -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")
|
||||
|
||||
|
|
@ -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")
|
||||
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
.pio
|
||||
.vscode/.browse.c_cpp.db*
|
||||
.vscode/c_cpp_properties.json
|
||||
.vscode/launch.json
|
||||
.vscode/ipch
|
||||
|
|
@ -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"
|
||||
]
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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 <Foo.h>
|
||||
#include <Bar.h>
|
||||
|
||||
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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,207 @@
|
|||
#include <Arduino.h>
|
||||
|
||||
// demo: CAN-BUS Shield, send data
|
||||
// demo: CAN-BUS Shield, receive data with check mode
|
||||
// loovee@seeed.cc
|
||||
|
||||
//#include <Servo.h>
|
||||
|
||||
#include <SPI.h>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue