Hi everyone,
I'm pretty new with programming on Arduino IDE, I have worked on the communication between PC and other electric devices (including Arduino board) for several months, there are still so many things cannot be figured out.
I'm working on a soft robot arm control project, more specifically it's about fall mitigation, we designed a backpack with two pneumatic driven soft arms, and we have four IMUs, two of them were mounted on the end-effectors of two arms, one was mounted in the centre of the backpack to work as a global coordinate and the last one was also mounted in the backpack to record the human inclination angle. The former three were connected with ESP32 and the last one was connected by Arduino Mega 2560. I need all the data from all the sensors sent back to PC (or MATLAB) for processing, then a new group of data (our control) will be sent from PC to Arduino and it will tell the pneumatic regulator about how much pressure we need through a DAC. However, I found that when using writeline function in MATLAB, data block issue will happen even at the beginning of the communication, which means that I could only receive one or two groups of data from Arduino, then it stuck. I have really no idea about this issue, I have done a lot of investigations in the past few weeks, so now I'm consulting help from you, thank you in advance (I will post my codes afterwards).
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation is used in I2Cdev.h.
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// Class default I2C address is 0x68, specific I2C addresses may be passed as a parameter here.
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
#define OUTPUT_READABLE_YAWPITCHROLL //This will provide you with the angle outputs you need //for your project
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
//DAC Setup
#include <Wire.h>
#include <Adafruit_MCP4725.h>
Adafruit_MCP4725 dac;
volatile bool flag = false;
float time_stamp = 0;
// 2023.09.05
// The repeated use variables will be established each time if we define them in the void loop () function, and taking them out and putting them into the void setup () function still does not work,
// so we have to put them all here ahead of all the functions.
// - - - - - - SOFT ROBOT ARM MANIPULATOR PARAMETER DEFINITION - - - - - - //
int pins[12] = {51, 39 , 37, 49, 35, 47 ,33, 45, 31, 43, 29, 41};
// 2023.09.07
// It will be a good choice to use struct to distinguish the variables from left and right arms, but it turns out not so successful, no matter what kind of method we tried, it did not work.
int pressure[12]; int p_L[6]; int p_R[6];
unsigned int PIN_L[6]={51,39,35,47,37,49};
unsigned int PIN_R[6]={33,45,31,43,29,41};
// Modified Version
String proc=""; // The data will be changed from character with " " into string.
String num_char; // This variable pass will each separated character number to another variable with double type, then it will be cleared each time just to save space.
struct INTM // intermediate variable
{
char temp; // Import the data read from MATLAB waiting for being processed.
int ls=0; // Calculate the length of the string converted.
int n_space=0; // Compute the number of space between the data from MATLAB.
int loc_space[12]; // We have 6 chambers for each arm, so the maximum size of the array is 6*2=12.
// 2023.09.05
// New array added to store the voltage potential for all chambers. We need to change this parameter if our system is getting more complex with more sensors.
int vp[6];
// double vp[6];
};
struct INTM PC;
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
mpuInterrupt = true;
}
void interruptFunction()
{
flag = digitalRead(18);
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
// 2023.09.14
// Wire.setWireTimeout(3000,true);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200); //Outputs can be viewed on your serial monitor (you can open in the tool //menu). Make sure to set the baud rate to 115200
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
//Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// pinMode(INTERRUPT_PIN, INPUT);
// verify connection
//Serial.println(F("Testing device connections..."));
//Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
//Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0)
{
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
// Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
// Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
// Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
//attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
//Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
//DACs have been assigned on pin 63
dac.begin(0x63);
// Set Arduino Pins as Output
for( int i= 0; i<12; i++)
{
pinMode(pins[i], OUTPUT);
}
attachInterrupt(digitalPinToInterrupt(18), interruptFunction, CHANGE);
pinMode(18, INPUT_PULLUP); // This needs to be investigated.
for(int i =0 ; i <12 ; i++)
{
pressure[i] = 0;
digitalWrite(pins[i], HIGH);
dac.setVoltage(pressure[i],false);
delay(5);
digitalWrite(pins[i],LOW);
}
delay(2000);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop()
{
// 2023.09.06
// The interruption event using flag feature has been removed since sometimes it will cause a bad data communication between PC and Arduino.
if (!flag)
{
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
// Get the Latest packet
// Step 1: Get input values of yaw, pitch and roll from the IMU
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// 2023.04.28
// This part is to send the states of the human to PC (MATLAB) and display them in serial monitor if necessary The exported data for roll angle suffered from ovf: overflow issue, which means that the value is going to be
// printed is larger than 4294967040.0 or smaller than -4294967040.0.
// 2023.06.05
// To avoid the issue that "ovf" appears for very large number (excedding the byte limitation), and since it only happens on roll angles, we shall use some special treatments. Since "ovf" still might appear with a
// secondary treatment in the if loop to judge if the roll angle is greater than PI, we are going to use while loop instead of if sentence.
// *** However, at some points, there exist some "nan" terms, we need to cope with them in MATLAB.
while (abs(ypr[3])>2*M_PI)
{
ypr[3]=ypr[3]-floor(ypr[3]/(2*M_PI))*2*M_PI;
}
if (abs(ypr[3])>M_PI)
{
if (ypr[3]>0) { ypr[3]=ypr[3]-M_PI; }
if (ypr[3]<0) { ypr[3]=ypr[3]+M_PI; }
}
time_stamp=millis();
// 2023.07.19
// BIG NEWS: The data sent to MATLAB were correct themselves, we need to set the precision on the Arduino side.
Serial.print(time_stamp/1000); Serial.print("\t");
Serial.print(ypr[1],4); Serial.print("\t");
Serial.print(ypr[2],4); Serial.print("\t");
Serial.print(ypr[3],4); Serial.print("\n");
#endif
}
}
// ======== DATA PROCESSING ======== //
// Example Input:
// 1.23 2.34 3.45 4.56 5.67 6.78
// 2200 -3.14 1.013 -250 9600 6666
// 2023.06.23
// "while" has been changed to "if" for the loop below to avoid the issue that an infinite loop in another bigger infinite loop will cause some of data of human lose.
// 2023.06.26
// It has to be "while" loop otherwise the data after processing is wrong.
while (Serial.available())
{
PC.temp=Serial.read();
proc=proc+PC.temp;
}
if (proc!="")
{
PC.ls=proc.length();
for (int i=0;i<PC.ls;i++)
{
// Temporarily the algorithm cannot assess the situation that there are more than one space or blank between two adjacent numbers, we assume that the formatting of the incoming data from MATLAB has been well edited.
if (proc[i]==' ')
{
PC.loc_space[PC.n_space]=i;
PC.n_space++;
}
}
if (PC.n_space==5)
{
for (int i=0;i<PC.n_space+1;i++)
{
if (i==0)
{
num_char=proc.substring(0,PC.loc_space[i]);
PC.vp[i]=num_char.toInt();
// PC.vp[i]=num_char.toDouble();
// num_char="";
// Serial.print(PC.vp[i]); Serial.print("\t");
}
else if (i<PC.n_space)
{
num_char=proc.substring(PC.loc_space[i-1]+1,PC.loc_space[i]);
PC.vp[i]=num_char.toInt();
// PC.vp[i]=num_char.toDouble();
// num_char="";
// Serial.print(PC.vp[i]); Serial.print("\t");
}
else
{
num_char=proc.substring(PC.loc_space[i-1]+1,PC.ls-1);
PC.vp[i]=num_char.toInt();
// PC.vp[i]=num_char.toDouble();
// num_char="";
// Serial.print(PC.vp[i]); Serial.print("\n");
}
}
// 2023.06.23 - Protection/Safety Strategy
// Assign pressure values to each PIN, please pay attention that this only applys when the pressure potential sent from MATLAB is of single arm.
for (int i=0;i<PC.n_space+1;i++)
{
if (PC.vp[i]>1800) { PC.vp[i]=1800; }
else if (PC.vp[i]<0) { PC.vp[i]=0; }
}
// Safety Examination //
// for (int i=0;i<PC.n_space+1;i++)
// {
// if (i<PC.n_space) { Serial.print(PC.vp[i]); Serial.print("\t"); }
// else { Serial.print(PC.vp[i]); Serial.print("\n"); }
// }
// ====== WARNING! WARNING! WARNING! ====== //
// DO NOT ACTIVATE THE FOLLOWING LINES OF CODES UNTIL YOU FIGURE OUT THE INPUTS FOR PRESSURE REGULATORS ARE SAFE ENOUGH.
for (int i=0;i<3;i++)
{
digitalWrite(PIN_L[2*i],HIGH); digitalWrite(PIN_L[2*i+1],HIGH);
dac.setVoltage(PC.vp[i],false);
digitalWrite(PIN_L[2*i],LOW); digitalWrite(PIN_L[2*i+1],LOW);
digitalWrite(PIN_R[2*i],HIGH); digitalWrite(PIN_R[2*i+1],HIGH);
dac.setVoltage(PC.vp[i+3],false);
digitalWrite(PIN_R[2*i],LOW); digitalWrite(PIN_R[2*i+1],LOW);
}
}
proc=""; PC.ls=0; PC.n_space=0; // These parameters need to be initialized after each void loop.
}
// ======== PC STREAMING DATA PROCESSING END ======== //
}