I have a Teensy 4.1 hooked up to a Raspberry Pi. I have communication working with an example sketch (arduino IDE) and i can request data with the Raspberry pi multiple times without any problem. I'm using this library instead of the default "Wire" because it has a 'slave' function: https://github.com/Richard-Gemmell/teensy4_i2c
i then put this exact code in my other code project, and a data request from the pi gives the wanted result, but only the first time!
after that, the I2C slave (Teensy) cannot be found by the raspberry pi and i need to manually reset it by pressing the button before it starts working again.
I have no idea what can cause this and why, so any help is greatly appreciated.
Example sketch (Working always):
Sketch with my other code added but identical I2C code (Working once then crashes):
Raspberry pi Python code (Master device):
What i also notice is that when i do a address scan from the pi ($ i2cdetect -y 1) it finishes in about 100ms, but when the teensy crashes that same scan takes about 10 seconds.
In the end, what i'm trying to do is send an array (uint16 gotArray[13 values]) over i2c to the pi on the pi's request.
i then put this exact code in my other code project, and a data request from the pi gives the wanted result, but only the first time!
after that, the I2C slave (Teensy) cannot be found by the raspberry pi and i need to manually reset it by pressing the button before it starts working again.
I have no idea what can cause this and why, so any help is greatly appreciated.
Example sketch (Working always):
Code:
// Wire Slave Sender
// by Nicholas Zambetti <http://www.zambetti.com>
// Modified by Richard Gemmell Oct 2019
// Demonstrates use of the Wire library
// Sends data as an I2C/TWI slave device
// Refer to the "Wire Master Reader" example for use with this
// To use it, connect a master to the Teensy on pins 18 and 19.
//
// Consider using the I2CRegisterSlave class instead of Wire to
// create an I2C slave.
// Created 29 March 2006
// This example code is in the public domain.
#include <Arduino.h>
#include <i2c_driver.h>
#include <i2c_driver_wire.h>
void requestEvent();
int led = LED_BUILTIN;
void setup()
{
pinMode(led, OUTPUT);
Wire1.begin(8); // join i2c bus with address #8
Wire1.onRequest(requestEvent); // register event
}
void loop()
{
delay(100);
}
// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
uint16_t gotArray[13] = {1,2,3,4,5,6,7,8,9,10,11,12,13};
digitalWrite(led, HIGH); // briefly flash the LED
Wire1.write(gotArray[13]); // respond with message of 6 bytes
// as expected by master
digitalWrite(led, LOW);
}Sketch with my other code added but identical I2C code (Working once then crashes):
Code:
#include <Arduino.h>
#include "AdvServo.h"
#include "FootSensor.h"
#include "InverseKinematics.h"
#include "Util.h"
#include <Servo.h>
//MANO'S REMOTE STUFF
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
RF24 radio(37, 38); // CE, CSN
const byte address[6] = "00001"; // CHANGE TO A UNIQUE ADDRESS (SAME AS ON THE REMOTE)
uint16_t gotArray[13];
// END OF MANO'S REMOTE STUFF
//I2C STUFF BELOW
#include <i2c_driver.h>
#include <i2c_driver_wire.h>
// END I2C STUFF
using namespace std;
String serialResponse = "";
char msg0[] = "0,-999.9,-999.9,-999.9"; // general structure for normal position command
char msg1[] = "0,-999.9,-999.9,-999.9,-999.9,-999.9,-999.9"; // general structure for normal position/speed command
bool ESTOPPED = false;
int max_speed = 500; // deg / sec
AdvServo BR_Hip, BR_Shoulder, BR_Wrist, BL_Hip, BL_Shoulder, BL_Wrist, FR_Hip, FR_Shoulder, FR_Wrist, FL_Hip, FL_Shoulder, FL_Wrist;
FootSensor FL_sensor, FR_sensor, BL_sensor, BR_sensor;
AdvServo * hips[4] = {&FL_Hip, &FR_Hip, &BL_Hip, &BR_Hip};
AdvServo * shoulders[4] = {&FL_Shoulder, &FR_Shoulder, &BL_Shoulder, &BR_Shoulder};
AdvServo * wrists[4] = {&FL_Wrist, &FR_Wrist, &BL_Wrist, &BR_Wrist};
Util util;
InverseKinematics ik;
void attach_servos() {
// HIPS
FL_Hip.init(4, 135, -2);
FR_Hip.init(11, 135, 0);
BL_Hip.init(7, 135, 0);
BR_Hip.init(8, 135, -3);
//SHOULDERS
FL_Shoulder.init(2, 180, -3);
FR_Shoulder.init(13, 90, -14);
BL_Shoulder.init(5, 180, 5); // +
BR_Shoulder.init(10, 90, 0); // -
//WRISTS
FL_Wrist.init(3, 0, 2);
FR_Wrist.init(12, 270, -2);
BL_Wrist.init(6, 0, 2);
BR_Wrist.init(9, 270, -1);
//FL_sensor.init(A9, 17);
//FR_sensor.init(A8, 16);
//BL_sensor.init(A7, 15);
//BR_sensor.init(A6, 14);
setLegJointIDS();
delay(1000);
}
void detach_servos() {
// HIPS
FL_Hip.detach();
FR_Hip.detach();
BR_Hip.detach();
BL_Hip.detach();
// SHOULDER
FL_Shoulder.detach();
FR_Shoulder.detach();
BR_Shoulder.detach();
BL_Shoulder.detach();
// WRIST
FL_Wrist.detach();
FR_Wrist.detach();
BR_Wrist.detach();
BL_Wrist.detach();
}
void update_servos(){
// HIPS
FL_Hip.update_clk();
FR_Hip.update_clk();
BR_Hip.update_clk();
BL_Hip.update_clk();
// SHOULDER
FL_Shoulder.update_clk();
FR_Shoulder.update_clk();
BR_Shoulder.update_clk();
BL_Shoulder.update_clk();
// WRIST
FL_Wrist.update_clk();
FR_Wrist.update_clk();
BR_Wrist.update_clk();
BL_Wrist.update_clk();
}
void update_sensors() {
FL_sensor.update_clk();
FR_sensor.update_clk();
BL_sensor.update_clk();
BR_sensor.update_clk();
}
void setLegJointIDS() {
// HIPS
FL_Hip.setType(0, 0);
FR_Hip.setType(1, 0);
BL_Hip.setType(2, 0);
BR_Hip.setType(3, 0);
//SHOULDERS
FL_Shoulder.setType(0, 1);
FR_Shoulder.setType(1, 1);
BL_Shoulder.setType(2, 1);
BR_Shoulder.setType(3, 1);
//WRISTS
FL_Wrist.setType(0, 2);
FR_Wrist.setType(1, 2);
BL_Wrist.setType(2, 2);
BR_Wrist.setType(3, 2);
}
void readRemote(){
while(radio.available()){
radio.read( &gotArray, sizeof(gotArray) );
//Serial.print("Recieved array:\n");
//for (byte i=0; i<13; i++){
//Serial.print(gotArray[i]);
//Serial.print(",");
//}
//Serial.println("\n");
}
}
void requestEvent();
void setup() {
// SETUP RADIO FOR MANO'S REMOTE
SPI1.setCS(38);
SPI1.setMISO(39);
SPI1.begin();
//Serial.begin(2000000);
//Serial.println("starting....");
radio.begin();
radio.openReadingPipe(0, address); //Setting the address at which we will receive the data
radio.setPALevel(RF24_PA_MAX); //You can set this as minimum or maximum depending on the distance between the transmitter and receiver.
radio.setDataRate(RF24_2MBPS);
radio.startListening(); //This sets the module as receiver
//END
//I2C initialization
Wire1.begin(8); // join i2c bus with address #8
Wire1.onRequest(requestEvent); // register event
// END I2C
Serial1.begin(500000);
ik.init(8.7, 59, 107, 130); // hip offset 0, hip_offset 1, shoulder length, wrist length
FL_Hip.init(4, 135, -2);
FR_Hip.init(11, 135, 0);
BL_Hip.init(7, 135, 0);
BR_Hip.init(8, 135, -3);
//SHOULDERS
FL_Shoulder.init(2, 180, -3);
FR_Shoulder.init(13, 90, -14);
BL_Shoulder.init(5, 180, 5); // +
BR_Shoulder.init(10, 90, 0); // -
//WRISTS
FL_Wrist.init(3, 0, 2);
FR_Wrist.init(12, 270, -2);
BL_Wrist.init(6, 0, 2);
BR_Wrist.init(9, 270, -1);
FL_sensor.init(A9, 17);
FR_sensor.init(A8, 16);
BL_sensor.init(A7, 15);
BR_sensor.init(A6, 14);
setLegJointIDS();
delay(1000);
//attach_servos();
}
void loop() {
readRemote();
//MANO'S REMOTE SERVO POWER OFF
if(gotArray[11]==1){
detach_servos();
} //else {
// attach_servos();
// }
if(!ESTOPPED){
update_servos();
} else {
detach_servos();
}
//update_sensors();
if (Serial1.available()) {
serialResponse = Serial1.readStringUntil('\r\n');
// Convert from String Object to String.
char buf[sizeof(msg0)];
serialResponse.toCharArray(buf, sizeof(buf));
char *ptr = buf;
char *str;
int index = 0;
int leg = -1; //0=FL, 1=FR, 2=BL, 3=BR
double x = -9999;
double y = -9999;
double z = -9999;
while ((str = strtok_r(ptr, ",", &ptr)) != NULL) { // delimiter is the dash
if(strcmp(str, "e") == 0 || strcmp(str, "E") == 0) {
ESTOPPED = true;
}
if(index == 0) {
util.upper(str);
if(strcmp(str, "0") == 0){
leg = 0;
}
if(strcmp(str, "1") == 0){
leg = 1;
}
if(strcmp(str, "2") == 0){
leg = 2;
}
if(strcmp(str, "3") == 0){
leg = 3;
}
}
if(index == 1) {
x = atof(str);
}
if(index == 2) {
y = atof(str);
}
if(index == 3) {
z = atof(str);
}
index++;
}
//COMPLETE MESSAGE CHECK
if(leg != -9999 || x != -9999 || y != -9999 || z != -9999){
Serial.println("complete message");
double *p;
p = ik.run(leg, x, y, z);
double temp_hip = util.toDegrees(*p);
if(leg == 1 || leg == 2){
temp_hip *= -1;
}
double hip_angle = util.angleConversion(leg, 0, temp_hip);
double shoulder_angle = util.angleConversion(leg, 1, util.toDegrees(*(p+1)));
double wrist_angle = util.angleConversion(leg, 2, util.toDegrees(*(p+2)));
double h_dist = abs(hip_angle - (*hips[leg]).getPosition());
double s_dist = abs(shoulder_angle - (*shoulders[leg]).getPosition());
double w_dist = abs(wrist_angle - (*wrists[leg]).getPosition());
double scaling_factor = util.max(h_dist, s_dist, w_dist);
h_dist /= scaling_factor;
s_dist /= scaling_factor;
w_dist /= scaling_factor;
(*hips[leg]).setPosition(hip_angle, max_speed * h_dist);
(*shoulders[leg]).setPosition(shoulder_angle, max_speed * s_dist);
(*wrists[leg]).setPosition(wrist_angle, max_speed * w_dist);
}
}
}
//I2C Data request response
void requestEvent()
{
Wire1.write(gotArray[0]); // respond with message of 6 bytes
}Raspberry pi Python code (Master device):
Code:
#!/usr/bin/python
import smbus
bus = smbus.SMBus(1) # 0 = /dev/i2c-0 (port I2C0), 1 = /dev/i2c-1 (port I2C1)
#DEVICE_ADDRESS = 0x15 #7 bit address (will be left shifted to add the read$
DEVICE_REG_MODE1 = 0x00
DEVICE_REG_LEDOUT0 = 0x1d
# Read a block of 16 bytes from address 80, offset 0
count = 0
while count < 5:
block = bus.read_i2c_block_data(8, 0, 16)
# Returned value is a list of 16 bytes
print(block)What i also notice is that when i do a address scan from the pi ($ i2cdetect -y 1) it finishes in about 100ms, but when the teensy crashes that same scan takes about 10 seconds.
In the end, what i'm trying to do is send an array (uint16 gotArray[13 values]) over i2c to the pi on the pi's request.
Last edited: