Hi,
Working on making a robot that uses multiple Teensy 4.0's to actuate many pairs of motors (5 motors controlled by each Teensy). Each of these Teensy's is controlled by a master Teensy 4.0 that communicates over CANbus and sends specific messages with ID's corresponding to each slave Teensy's motion commands (PWM and direction bit). Below are the complete pieces of source code for the slave and master teensy's, so far I have only 1 slave and 1 master.
The odd behavior I am seeing is that when I unplug the slave teensy from my computer's USB power, all of the motors controlled by the slave teensy start running at full speed until I kill my DC power supply. This only occurs when the Master teensy is also turned on (ie, if the master teensy is off, the behavior does not occur, although of course I cannot control the slave teensy at all without the master).
Thoughts?
Working on making a robot that uses multiple Teensy 4.0's to actuate many pairs of motors (5 motors controlled by each Teensy). Each of these Teensy's is controlled by a master Teensy 4.0 that communicates over CANbus and sends specific messages with ID's corresponding to each slave Teensy's motion commands (PWM and direction bit). Below are the complete pieces of source code for the slave and master teensy's, so far I have only 1 slave and 1 master.
Code:
// Slave Teensy
#include "Arduino.h" // arduino library
#include "USBHost_t36.h" // usb host library
#include "FlexCAN_T4.h" // CAN library
#include <TeensyThreads.h>// threading library
#include <iostream> // std::cout
#include <cmath> // std::abs
USBHost myusb;
USBHIDParser hid1(myusb);
JoystickController joystick1(myusb);
// PIN DEFINITIONS
#define PHASE_1 2 // DIGITAL
#define ENABLE_1 3 // ANALOG
#define PHASE_4 4 // DIGITAL
#define ENABLE_4 5 // ANALOG
#define PHASE_3 6 // DIGITAL
#define ENABLE_3 7 // ANALOG
#define ENABLE_5 18 // ANALOG
#define ENABLE_2 19 // ANALOG
#define PHASE_5 20 // DIGITAL
#define PHASE_2 21 // DIGITAL
#define CAN_TX 22 // CAN BUS TX
#define CAN_RX 23 // CAN BUS RX
// MISC
#define BAUDRATE 9600
#define DEBUG 1
#define CANDUMP 1
int user_axis[64];
uint32_t buttons_prev = 0;
// Set up motor control data structures
const uint8_t motorPin[2][5] = {{ENABLE_1,ENABLE_2,ENABLE_3,ENABLE_4,ENABLE_5},
{PHASE_1 ,PHASE_2 ,PHASE_3 ,PHASE_4 ,PHASE_5 }};
uint8_t motorSet[2][5] = {{0,0,0,0,0},
{0,0,0,0,0}};
uint8_t currentSegment = 0;
bool masterRecv = false;
// Set up encoder values
uint8_t masterCommand[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float_t jointAnglesDesired[5] = {0.0,0.0,0.0,0.0,0.0};
float_t jointAnglesActual[5] = {0.0,0.0,0.0,0.0,0.0};
// Initialize CAN data structures
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can1;
CAN_message_t msg;
uint8_t sendVal = 0;
uint8_t hexVal = 0;
USBDriver *drivers[] = {&joystick1, &hid1};
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
const char * driver_names[CNT_DEVICES] = {"JOY1D", "HID1"};
bool driver_active[CNT_DEVICES] = {false, false};
// Lets also look at HID Input devices
USBHIDInput *hiddrivers[] = {&joystick1};
#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
const char * hid_driver_names[CNT_DEVICES] = {"Joystick1"};
bool hid_driver_active[CNT_DEVICES] = {false};
bool show_changed_only = false;
uint8_t joystick_left_trigger_value = 0;
uint8_t joystick_right_trigger_value = 0;
uint64_t joystick_full_notify_mask = (uint64_t) - 1;
void canSniff(const CAN_message_t &msg) {
Serial.print("MB "); Serial.print(msg.mb);
Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun);
Serial.print(" LEN: "); Serial.print(msg.len);
Serial.print(" EXT: "); Serial.print(msg.flags.extended);
Serial.print(" TS: "); Serial.print(msg.timestamp);
Serial.print(" ID: "); Serial.print(msg.id, HEX);
Serial.print(" Buffer: ");
for ( uint8_t i = 0; i < msg.len; i++ ) {
Serial.print(msg.buf[i], HEX); Serial.print(" ");
} Serial.println();
}
void canWrite() {
while(1) {
if (DEBUG) {Serial.println("write CAN loops");}
can1.events();
// Write CAN Message
static uint32_t timeout = millis();
msg.buf[0] = sendVal;
hexVal += 0.05;
sendVal = (sin(hexVal) + 1) * 255 / 2;
msg.id = 0xAB;
can1.write(msg);
timeout = millis();
// canSniff(msg);
if (DEBUG) {Serial.println("write CAN about to delay");}
threads.delay(50);
// Serial.println("write CAN about to yield");
// threads.yield();
}
}
void canRead() {
while(1) {
masterRecv = false;
if (DEBUG) {Serial.println("read CAN - start loop");}
// Read CAN Message
// uint32_t tik = millis();
while (can1.read(msg)) {
if (msg.id == 0xAA) {
masterRecv = true;
if (CANDUMP) {
Serial.print("\nReceived Message: ");
Serial.print("CAN1 ");
Serial.print("MB: "); Serial.print(msg.mb);
Serial.print(" ID: 0x"); Serial.print(msg.id, HEX );
Serial.print(" EXT: "); Serial.print(msg.flags.extended );
Serial.print(" LEN: "); Serial.print(msg.len);
Serial.print(" DATA: ");
}
for ( uint8_t i = 0; i < 8; i++ ) {
masterCommand[i] = msg.buf[i];
if (DEBUG) {
Serial.print(masterCommand[i]);
Serial.print(" ");
}
}
Serial.println("");
}
}
Serial.println("");
if (!masterRecv) {
Serial.println("resetting");
for ( uint8_t i = 0; i < 8; i++ ) {
masterCommand[i] = 0;
}
}
if (DEBUG) {Serial.println("read CAN delays");}
threads.delay(50);
}
}
void runMotor() {
while(1) {
for (uint8_t j = 0; j < 5; j++) {
if (masterRecv) {
motorSet[0][j] = masterCommand[j];
motorSet[1][j] = (bool)((masterCommand[5] >> j) & 0b00000001);
}
else {
motorSet[0][j] = 0;
motorSet[1][j] = 0;
}
analogWrite(motorPin[0][j], motorSet[0][j]);
digitalWrite(motorPin[1][j], motorSet[1][j]);
if (DEBUG) {
Serial.printf("\nSeg%d motorPWM:%d motorDir:%d ", j, motorSet[0][j], motorSet[1][j]);
}
}
if (DEBUG) {Serial.println("\nrun Motor about to yield");}
threads.delay(50);
Serial.println();
}
}
void setup() {
// while (!Serial) ; // wait for Arduino Serial Monitor
delay(1000);
Serial.println("\n\nUSB Host Testing");
Serial.println(sizeof(USBHub), DEC);
myusb.begin();
//=============================================================================
// Set OUTPUT pins
//=============================================================================
pinMode(ENABLE_1, OUTPUT);
pinMode(PHASE_1, OUTPUT);
pinMode(ENABLE_2, OUTPUT);
pinMode(PHASE_2, OUTPUT);
pinMode(ENABLE_3, OUTPUT);
pinMode(PHASE_3, OUTPUT);
pinMode(ENABLE_4, OUTPUT);
pinMode(PHASE_4, OUTPUT);
pinMode(ENABLE_5, OUTPUT);
pinMode(PHASE_5, OUTPUT);
//=============================================================================
// Initialize CAN communication
//=============================================================================
can1.begin();
msg.buf[0] = 0x00;
can1.setBaudRate(1000000);
// Open serial communications and wait for port to open:
Serial.begin(9600);
Serial.println("Serial started!");
can1.read(msg);
can1.setMB(MB1,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB2,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB3,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB4,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB5,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB6,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB7,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB8,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB9,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMBFilter(MB1, 0x5041401);
can1.setMBFilter(MB2, 0x5041402);
can1.setMBFilter(MB3, 0x5041403);
can1.setMBFilter(MB4, 0x5041404);
can1.setMBFilter(MB5, 0x5041405);
can1.setMBFilter(MB6, 0x5041406);
can1.setMBFilter(MB7, 0x5041407);
can1.setMBFilter(MB8, 0x5041408);
can1.setMBFilter(MB9, 0xAA);
//=============================================================================
// Setup threads
//=============================================================================
// threads.addThread(canWrite);
threads.addThread(canRead);
// threads.addThread(runMotor);
}
void loop() {
runMotor();
}
void OnHIDExtrasRelease(uint32_t top, uint16_t key)
{
Serial.print("HID (");
Serial.print(top, HEX);
Serial.print(") key release:");
Serial.println(key, HEX);
}
Code:
// Master Teensy
#include "Arduino.h" // arduino library
#include "USBHost_t36.h" // usb host library
#include "FlexCAN_T4.h" // CAN library
#include <TeensyThreads.h>// threading library
#include <iostream> // std::cout
#include <cmath> // std::abs
USBHost myusb;
USBHIDParser hid1(myusb);
JoystickController joystick1(myusb);
// PIN DEFINITIONS
#define PHASE_1 2 // DIGITAL
#define ENABLE_1 3 // ANALOG
#define PHASE_4 4 // DIGITAL
#define ENABLE_4 5 // ANALOG
#define PHASE_3 6 // DIGITAL
#define ENABLE_3 7 // ANALOG
#define ENABLE_5 18 // ANALOG
#define ENABLE_2 19 // ANALOG
#define PHASE_5 20 // DIGITAL
#define PHASE_2 21 // DIGITAL
#define CAN_TX 22 // CAN BUS TX
#define CAN_RX 23 // CAN BUS RX
// MISC
#define BAUDRATE 9600
#define DEBUG 0
#define CANDUMP 0
int user_axis[64];
uint32_t buttons_prev = 0;
// Set up motor control data structures
uint8_t deadMan = 1;
const uint8_t motorPin[2][5] = {{ENABLE_1,ENABLE_2,ENABLE_3,ENABLE_4,ENABLE_5},
{PHASE_1 ,PHASE_2 ,PHASE_3 ,PHASE_4 ,PHASE_5 }};
uint8_t motorSet[2][10] = {{0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0}};
uint8_t currentSegment = 0;
// boolean slaveMove[12] = {false,
// false,
// false,
// false,
// false,
// false,
// false,
// false,
// false,
// false,
// false,
// false
// }
// Set up encoder values
float_t jointAnglesDesired[12][5] ={{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0}};
float_t jointAnglesActual[12][5] ={{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0}};
// Initialize CAN data structures
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can1;
CAN_message_t msg;
uint8_t sendVal = 0;
uint8_t hexVal = 0;
USBDriver *drivers[] = {&joystick1, &hid1};
#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
const char * driver_names[CNT_DEVICES] = {"JOY1D", "HID1"};
bool driver_active[CNT_DEVICES] = {false, false};
// Lets also look at HID Input devices
USBHIDInput *hiddrivers[] = {&joystick1};
#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
const char * hid_driver_names[CNT_DEVICES] = {"Joystick1"};
bool hid_driver_active[CNT_DEVICES] = {false};
bool show_changed_only = false;
uint8_t joystick_left_trigger_value = 0;
uint8_t joystick_right_trigger_value = 0;
uint64_t joystick_full_notify_mask = (uint64_t) - 1;
void canSniff(const CAN_message_t &msg) {
Serial.print("MB "); Serial.print(msg.mb);
Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun);
Serial.print(" LEN: "); Serial.print(msg.len);
Serial.print(" EXT: "); Serial.print(msg.flags.extended);
Serial.print(" TS: "); Serial.print(msg.timestamp);
Serial.print(" ID: "); Serial.print(msg.id, HEX);
Serial.print(" Buffer: ");
for ( uint8_t i = 0; i < msg.len; i++ ) {
Serial.print(msg.buf[i], HEX); Serial.print(" ");
} Serial.println();
}
void canWrite() {
while(1) {
if (DEBUG) {Serial.println("write CAN loops");}
can1.events();
// Write CAN Message
// static uint32_t timeout = millis();
msg.buf[0] = motorSet[0][5];
msg.buf[1] = motorSet[0][6];
msg.buf[2] = motorSet[0][7];
msg.buf[3] = motorSet[0][8];
msg.buf[4] = motorSet[0][9];
msg.buf[5] = motorSet[1][5] * 1 + motorSet[1][6] * 2 + motorSet[1][7] * 4
+ motorSet[1][8] * 8 + motorSet[1][9] * 16;
msg.buf[6] = deadMan;
msg.buf[7] = 0;
msg.id = 0xAA;
can1.write(msg);
// timeout = millis();
if (CANDUMP) {canSniff(msg);}
if (DEBUG) {Serial.println("write CAN about to delay");}
threads.delay(50);
// Serial.println("write CAN about to yield");
// threads.yield();
}
}
void canRead() {
while(1) {
if (DEBUG) {Serial.println("read CAN - start loop");}
// Read CAN Message
// uint32_t tik = millis();
for (int i = 0; i < 10; i++)
if ( can1.read(msg)) {
Serial.print("\nReceived Message: ");
Serial.print("CAN1 ");
Serial.print("MB: "); Serial.print(msg.mb);
Serial.print(" ID: 0x"); Serial.print(msg.id, HEX );
Serial.print(" EXT: "); Serial.print(msg.flags.extended );
Serial.print(" LEN: "); Serial.print(msg.len);
Serial.print(" DATA: ");
for ( uint8_t j = 0; j < 8; j++ ) {
Serial.print(msg.buf[j]);
Serial.print(" ");
if (msg.id > 0x5041400) {
if (j == 6) {
jointAnglesActual[(msg.id - 0x5041401) / 5][(msg.id - 0x5041401) % 5] = (360./255.)*msg.buf[j];
}
else if (j == 7) {
jointAnglesActual[(msg.id - 0x5041401) / 5][(msg.id - 0x5041401) % 5] += (1./(255.))*msg.buf[j];
}
}
}
}
Serial.println("");
// uint32_t time_elapsed = millis() - tik;
// Serial.printf("read CAN about to delay - took %f ms\n", time_elapsed);
if (DEBUG) {Serial.println("read CAN delays");}
for (int i = 0; i < 10; i++){
Serial.print("\n link angle "); Serial.print(i); Serial.print(": ");
Serial.print(jointAnglesActual[i / 5][i % 5]);
}
threads.delay(50);
// threads.yield();
}
}
void runMotor() {
while(1) {
if (DEBUG) {Serial.println("runMotor loops");}
myusb.Task(); //cause state to update
if (Serial.available()) {
int ch = Serial.read(); // get the first char.
while (Serial.read() != -1) ;
if ((ch == 'b') || (ch == 'B')) {
Serial.println("Only notify on Basic Axis changes");
joystick1.axisChangeNotifyMask(0x3ff);
} else if ((ch == 'f') || (ch == 'F')) {
Serial.println("Only notify on Full Axis changes");
joystick1.axisChangeNotifyMask(joystick_full_notify_mask);
} else {
if (show_changed_only) {
show_changed_only = false;
Serial.println("\n*** Show All fields mode ***");
} else {
show_changed_only = true;
Serial.println("\n*** Show only changed fields mode ***");
}
}
}
for (uint8_t i = 0; i < CNT_DEVICES; i++) {
if (*drivers[i] != driver_active[i]) {
if (driver_active[i]) {
Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
driver_active[i] = false;
} else {
Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
driver_active[i] = true;
const uint8_t *psz = drivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = drivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = drivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
}
}
}
for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
if (*hiddrivers[i] != hid_driver_active[i]) {
if (hid_driver_active[i]) {
Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
hid_driver_active[i] = false;
} else {
Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
hid_driver_active[i] = true;
const uint8_t *psz = hiddrivers[i]->manufacturer();
if (psz && *psz) Serial.printf(" manufacturer: %s\n", psz);
psz = hiddrivers[i]->product();
if (psz && *psz) Serial.printf(" product: %s\n", psz);
psz = hiddrivers[i]->serialNumber();
if (psz && *psz) Serial.printf(" Serial: %s\n", psz);
}
}
}
// Collect servomotor input and update
if (joystick1.available()) {
uint64_t axis_mask = joystick1.axisMask();
uint64_t axis_changed_mask = joystick1.axisChangedMask();
//Serial.print("Joystick: buttons = ");
uint32_t buttons = joystick1.getButtons();
//Serial.print(buttons, HEX);
if (show_changed_only) {
for (uint8_t i = 0; axis_changed_mask != 0; i++, axis_changed_mask >>= 1) {
if (axis_changed_mask & 1) {
//Serial.printf(" %d:%d", i, joystick1.getAxis(i));
}
}
}
else { // Control snake positions
for (uint8_t i = 0; axis_mask != 0; i++, axis_mask >>= 1) {
if (axis_mask & 1) {
uint8_t axisValue = joystick1.getAxis(i);
if (i==2) { // Use right thumbpad input to set motorSet and direction
if (axisValue < 134 && axisValue > 122) {
motorSet[0][currentSegment] = 0; // if in deadband region set to 0
}
else {
motorSet[0][currentSegment] = 2*abs(axisValue-127)-1; // motor speed scaled by offset from center
}
if (axisValue < 128) { // if down, phase of 1 = CW
motorSet[1][currentSegment] = 1;
}
else { // if up, phase of 0 = CCW
motorSet[1][currentSegment] = 0;
}
}
// else if (i==5) {
// motorSet[1][0] = scaledValue;
// }
else if (i==9) {
if (axisValue==0 && currentSegment < 9) {
currentSegment++;
}
else if (axisValue==4 && currentSegment > 0) {
currentSegment--;
}
}
}
for (uint8_t j = 0; j < 5; j++) {
analogWrite(motorPin[0][j], motorSet[0][j]);
digitalWrite(motorPin[1][j], motorSet[1][j]);
// if (DEBUG) {Serial.printf("\nSeg%d motorPWM:%d motorDir:%d ", j, motorSet[0][j], motorSet[1][j]);}
}
if (DEBUG) {
for (uint8_t j = 0; j < 10; j++) {
Serial.printf("\nSeg%d motorPWM:%d motorDir:%d ", j, motorSet[0][j], motorSet[1][j]);
}
}
// threads.delay(250);
// for (uint8_t j = 0; j < 5; j++) {
// motorSet[0][currentSegment] = 0; // set to 0 after thread ends
// motorSet[1][currentSegment] = 0; // set to 0 after thread
// analogWrite(motorPin[0][j], motorSet[0][j]);
// digitalWrite(motorPin[1][j], motorSet[1][j]);
// }
// }
Serial.println();
Serial.printf("CS:%d ", currentSegment);
}
Serial.println();
joystick1.joystickDataClear();
// threads.yield();
}
}
if (DEBUG) {Serial.println("run Motor about to yield");}
threads.delay(50);
}
}
void setup() {
// while (!Serial) ; // wait for Arduino Serial Monitor
delay(1000);
Serial.println("\n\nUSB Host Testing");
Serial.println(sizeof(USBHub), DEC);
myusb.begin();
//=============================================================================
// Set OUTPUT pins
//=============================================================================
pinMode(ENABLE_1, OUTPUT);
pinMode(PHASE_1, OUTPUT);
pinMode(ENABLE_2, OUTPUT);
pinMode(PHASE_2, OUTPUT);
pinMode(ENABLE_3, OUTPUT);
pinMode(PHASE_3, OUTPUT);
pinMode(ENABLE_4, OUTPUT);
pinMode(PHASE_4, OUTPUT);
pinMode(ENABLE_5, OUTPUT);
pinMode(PHASE_5, OUTPUT);
//=============================================================================
// Initialize CAN communication
//=============================================================================
can1.begin();
msg.buf[0] = 0x00;
can1.setBaudRate(1000000);
// Open serial communications and wait for port to open:
Serial.begin(9600);
Serial.println("Serial started!");
can1.read(msg);
can1.setMB(MB1,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB2,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB3,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB4,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB5,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB6,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB7,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB8,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB9,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMB(MB10,RX,EXT); // Set mailbox as receiving extended frames.
can1.setMBFilter(MB1, 0x5041401);
can1.setMBFilter(MB2, 0x5041402);
can1.setMBFilter(MB3, 0x5041403);
can1.setMBFilter(MB4, 0x5041404);
can1.setMBFilter(MB5, 0x5041405);
can1.setMBFilter(MB6, 0x5041406);
can1.setMBFilter(MB7, 0x5041407);
can1.setMBFilter(MB8, 0x5041408);
can1.setMBFilter(MB9, 0x5041409);
can1.setMBFilter(MB10, 0x5041410);
//=============================================================================
// Setup threads
//=============================================================================
threads.addThread(canWrite);
threads.addThread(canRead);
// threads.addThread(runMotor);
}
void loop() {
runMotor();
}
void OnHIDExtrasRelease(uint32_t top, uint16_t key)
{
Serial.print("HID (");
Serial.print(top, HEX);
Serial.print(") key release:");
Serial.println(key, HEX);
}
The odd behavior I am seeing is that when I unplug the slave teensy from my computer's USB power, all of the motors controlled by the slave teensy start running at full speed until I kill my DC power supply. This only occurs when the Master teensy is also turned on (ie, if the master teensy is off, the behavior does not occur, although of course I cannot control the slave teensy at all without the master).
Thoughts?