First post, so I apologize for any formatting errors / posting in the wrong place. If there is another topic that I haven't found that addresses this issue, my apologies.
I am trying to make a rotary inverted pendulum so that I can explore a couple different control strategies. The unique kicker about this project is that it uses an AK80-6A series motor (AK80-6_AK Series Dynamical Modular_Robot Dynamics_T-MOTOR Store-Official Store for T-motor drone motor,ESC,Propeller), so I'm forced to make a unique testing stand. This motor has a PID controller attached uses CAN communication (with unique protocols) to actuate the motor. I'm using an Arduino Uno R3 with a Sparkfun CAN-bus shield to handle this communication as well as handle the rod encoder readings (http://domoticx.com/sensor-lpd3806-optical-rotary-encoder/).
I have been able to individually get CAN communication and rod encoder readings to work through the CAN bus shield, but not at the same time. Specifically, if I disable the CAN send / reply, the encoder readings display correctly. When I enable CAN, encoder readings are either unresponsive or incorrectly move at random intervals.
I've received advice that it may just be where / if interrupts are enabled. I have tried hard-coding enabling and disabling interrupts, and the above issue happens when interrupts are enabled (and when it should work?). I suspect that this issue is caused by large amounts of info being passed via CAN "blocking out" the encoder info, but I have no idea if that's a thing (or how to fix it) and that's why I'm here.
I haven't found any other posts that suggest CAN communication and an encoder running on the same system. Is there some easy fix that I'm missing, or is this issue deeper than it seems?
// Furuta Motor Control
#include <mcp_can.h>
#include <SPI.h>
/*SAMD core*/
#ifdef ARDUINO_SAMD_VARIANT_COMPLIANCE
#define SERIAL SerialUSB
#else
#define SERIAL Serial
#endif
// Define Joystick connection pins
#define UP A1
#define DOWN A3
#define LEFT A2
#define RIGHT A5
#define CLICK A4
// Define LED pins
#define LED2 8
#define LED3 7
// Value Limits
#define P_MIN -95.5f
#define P_MAX 95.5f
#define V_MIN -45.0f
#define V_MAX 45.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
float p_in = 0.0f;
float v_in = 0.0f;
float kp_in = 50.0f;
float kd_in = 1.0f;
float t_in = 0.0f;
// measured values
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;
const int SPI_CS_PIN = 10;
MCP_CAN CAN(SPI_CS_PIN);
void setup() {
Serial.begin(115200);
delay(1000);
while (CAN_OK != CAN.begin(CAN_1000KBPS))
{
SERIAL.println("CAN BUS Shield init fail");
SERIAL.println("Init CAN BUS Shield again");
delay(100);
}
SERIAL.println("CAN BUS Shield init ok!");
// Initialize pins as necessary
pinMode(UP, INPUT);
pinMode(DOWN, INPUT);
pinMode(LEFT, INPUT);
pinMode(RIGHT, INPUT);
pinMode(CLICK, INPUT);
pinMode(LED2, OUTPUT);
pinMode(LED3, OUTPUT);
// Pull analog pins high to enable reading of joystick movements
digitalWrite(UP, HIGH);
digitalWrite(DOWN, HIGH);
digitalWrite(LEFT, HIGH);
digitalWrite(RIGHT, HIGH);
digitalWrite(CLICK, HIGH);
// Write LED pins low to turn them off by default
digitalWrite(LED2, LOW);
digitalWrite(LED3, LOW);
setup_encoder(); // pinMode, attachInterrupt
}
long previousMillis = 0;
void loop() {
float p_step = 0.01;
if (digitalRead(UP) == LOW)
{
// move motor forward
p_in = p_in + p_step;
}
if (digitalRead(DOWN) == LOW)
{
// move motor backward
p_in = p_in - p_step;
}
p_in = constrain(p_in, P_MIN, P_MAX);
if (digitalRead(RIGHT) == LOW)
{
// Enable
EnterMotorMode();
digitalWrite(LED2, HIGH);
}
if (digitalRead(LEFT) == LOW)
{
// Disable
ExitMotorMode();
digitalWrite(LED2, LOW);
}
if (digitalRead(CLICK) == LOW)
{
// Zero Motor
Zero();
}
// send CAN
pack_cmd();
// receive CAN
if (CAN_MSGAVAIL == CAN.checkReceive())
{
unpack_reply();
}
else
{
Serial.println("No data");
}
// print data
Serial.print(millis() - previousMillis);
previousMillis = millis();
Serial.print("\t");
Serial.print("P_in: ");
Serial.print(p_in);
Serial.print("\t");
Serial.print("P_out: ");
Serial.print(p_out);
Serial.print("\t");
Serial.print("V_out: ");
Serial.print(v_out);
Serial.print("\t");
Serial.print("T_out: ");
Serial.print(t_out);
Serial.print("\t");
Serial.print("Rod Encoder: ");
float angle = read_encoder(); // in [deg]
Serial.println(angle); // print angle
delay(50);
}
// CAN References
void EnterMotorMode() {
// Enter Motor Mode (enable)
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() {
// Exit Motor Mode (disable)
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() {
// Zero motor
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); //fminf(fmaxf(P_MIN, p_in), 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);
/// convert floats to unsigned ints ///
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 the 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(0x01, 0, 8, buf);
}
void unpack_reply() {
byte len = 0;
byte buf[8];
CAN.readMsgBuf(&len, buf); //read data
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);
t_out = uint_to_float(i_int, -T_MAX, T_MAX, 12);
}
unsigned int float_to_uint(float x, float x_min, float x_max, int bits) {
/// Converts 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 unsigned int to float, given range and number
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;
}
// Rod Encoder References
#define CHA 2 // defines variable CHA to D2
#define CHB 3 // defines variable CHB to D3
volatile long count = 0; // set count without specifying datatype
void isr() { // set of code that pairs with attachInterrupt
count += digitalRead(CHB) ? 1 : -1; // if count = count + input from D3, use 1, else use -1
}
void setup_encoder() { // custom function to set up encoder parameters
pinMode(CHA, INPUT_PULLUP); // sets pin 2 to a pullup pin
pinMode(CHB, INPUT_PULLUP); // sets pin 3 to a pullup pin
attachInterrupt(digitalPinToInterrupt(CHA), isr, RISING); // sets interrupt pin, set of code to reference, and what kind of trigger activates it
}
float read_encoder() { // custom function to read encoder
return (count * 360 / 600); // returns calculation for displacement in deg
}