I have tried this schematic diagram to run my motor, but the motor doesn’t seem to be responding.
Here is the motor setup:
set_band_width Current Bandwidth (Hz) 100 2000 1000.0
set_can_id CAN ID 0 127 1
set_master_id CAN Master ID 0 127 0
set_current_limit Current Limit (A) 0.0 60.0 60.0
set_fw_current_limit FW Current Limit (A) 0.0 33.0 0.0
set_can_time_out CAN Timeout (cycles)(0 = none) 0 100000 0
Here below is the code:
#include <mcp_can.h>
#include <SPI.h>
// the cs pin of the version v1.4 is default to D10
// Set CS pin
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif
// Value Limits
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -65.0f
#define V_MAX 65.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
// Set Values
unsigned int id = 0;
float p_in = 0.0f;
float v_in = 0.0f;
float kp_in = 20.0f;
float kd_in = 1.0f;
float t_in = 0.0f;
//measured values - responses from the motor
float p_out = 0.0f; // actual position
float v_out = 0.0f; // actual velocity
float t_out = 0.0f; // actual torque
const int SPI_CS_PIN = 10;
MCP_CAN CAN(SPI_CS_PIN); //Set CS pin
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
delay(1000);
while (CAN_OK != CAN.begin(MCP_ANY, CAN_1000KBPS, MCP_16MHZ)) {
Serial.println(“CAN BUS Shield init fail”);
Serial.println(“Init CAN BUS Shield again”);
delay(1000);
}
Serial.println(“CAN BUS Shield init ok!”);
}
long previousMillis = 0;
boolean newData = false;
void loop() {
char rc;
float p_step = 0.1;
//delay(200);
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
Serial.println(rc);
if (rc == ‘s’) {
Serial.println(“True”);
}
if (rc == 'u') {
p_in = p_in + p_step;
}
if (rc == 'd') {
p_in = p_in - p_step;
}
p_in = constrain(p_in, P_MIN, P_MAX);
if (rc == 'o') {
EnterMotorMode();
}
if (rc == 'f') {
ExitMotorMode();
}
// send CAN
pack_cmd();
}
// receive CAN
if (CAN_MSGAVAIL == CAN.checkReceive()) // check if data coming
{
unpack_reply();
}
Serial.print(millis() - previousMillis);
previousMillis = millis();
Serial.print("ID ");
Serial.print(id);
Serial.print("position “);
Serial.print(p_in);
Serial.print(” ");
Serial.print("position output “);
Serial.print(p_out);
Serial.print(” ");
Serial.print("velocity output “);
Serial.print(v_out);
Serial.print(” ");
Serial.print("torque output ");
Serial.println(t_out);
}
void EnterMotorMode() {
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFC;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void ExitMotorMode() {
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFD;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void Zero() {
byte buf[8];
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
buf[3] = 0xFF;
buf[4] = 0xFF;
buf[5] = 0xFF;
buf[6] = 0xFF;
buf[7] = 0xFE;
CAN.sendMsgBuf(0x01, 0, 8, buf);
}
void pack_cmd() {
byte buf[8];
// limit data to be within bounds ///
float p_des = constrain(p_in, P_MIN, P_MAX);
float v_des = constrain(v_in, V_MIN, V_MAX);
float kp = constrain(kp_in, KP_MIN, KP_MAX);
float kd = constrain(kd_in, KD_MIN, KD_MAX);
float t_ff = constrain(t_in, T_MIN, T_MAX);
unsigned int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
unsigned int v_int = float_to_uint(v_des, V_MIN, V_MAX, 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);
unsigned int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
//pack ints into can buffer ///
buf[0] = p_int >> 8;
buf[1] = p_int & 0xFF;
buf[2] = v_int >> 4;
buf[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);
buf[4] = kp_int & 0xFF;
buf[5] = kd_int >> 4;
buf[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);
buf[7] = t_int & 0xFF;
CAN.sendMsgBuf(0x02, 0, 8, buf);
}
void unpack_reply() {
unsigned long int canId;
byte len = 0;
byte buf[8];
CAN.readMsgBuf(&canId, &len, buf);
//unsigned long canId = CAN.getCanId();
///unpack ints from can buffer///
unsigned int id = buf[0];
unsigned int p_int = (buf[1] << 8) | buf[2];
unsigned int v_int = (buf[3] << 4) | (buf[4] >> 4);
unsigned int i_int = ((buf[4] & 0xF) << 8) | buf[5];
/// convert uints to floats ///
p_out = uint_to_float(p_int, P_MIN, P_MAX, 16);
v_out = uint_to_float(v_int, V_MIN, V_MAX, 12);
p_out = uint_to_float(i_int, -T_MIN, T_MAX, 12);
}
unsigned int float_to_uint(float x, float x_min, float x_max, float bits) {
//convert a float to an unsigned int, given range and number of bits ///
float span = x_max - x_min;
float offset = x_min;
unsigned int pgg = 0;
if (bits == 12) {
pgg = (unsigned int)((x - offset) * 4095.0 / span);
}
if (bits == 16) {
pgg = (unsigned int)((x - offset) * 65535.0 / span);
}
return pgg;
}
float uint_to_float(unsigned int x_int, float x_min, float x_max, int bits) {
/// converts unsined int to float, given range and numbers of bits///
float span = x_max - x_min;
float offset = x_min;
float pgg = 0;
if (bits == 12) {
pgg = ((float)x_int) * span / 4095.0 + offset;
}
if (bits == 16) {
pgg = ((float)x_int) * span / 65535.0 + offset;
}
return pgg;
}