Arduino UNO R3 with the x-nucleo-iks01a3

[Giuseppe1]

Hi, i connected the Arduino UNO R3 with the motion MEMS and environmental sensor evaluation board system: X-NUCLEO-IKS01A3 of ST. I installed the library (STM32duino,LSM6DSO), and tried to create a code for a self-balancing robot. But it doesn't print the right angle.

 

 

 

 

 

 

 

 

// Includes
#include <LSM6DSOSensor.h>

#ifdef ARDUINO_SAM_DUE
#define DEV_I2C Wire1
#elif defined(ARDUINO_ARCH_STM32)
#define DEV_I2C Wire
#elif defined(ARDUINO_ARCH_AVR)
#define DEV_I2C Wire
#else
#define DEV_I2C Wire
#endif
#define SerialPort Serial

int16_t accY, accZ, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;

unsigned long currTime, prevTime=0, loopTime;

#define leftMotorPWMPin 6
#define leftMotorDirPin 7
#define rightMotorPWMPin 5
#define rightMotorDirPin 4

#define Kp 40
#define Kd 0.05
#define Ki 40
#define sampleTime 0.005
#define targetAngle -2.5


// Components
LSM6DSOSensor AccGyr(&DEV_I2C);


void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
 if(leftMotorSpeed >= 0) {
 analogWrite(leftMotorPWMPin, leftMotorSpeed);
 digitalWrite(leftMotorDirPin, LOW);
 }
 else {
 analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
 digitalWrite(leftMotorDirPin, HIGH);
 }
 if(rightMotorSpeed >= 0) {
 analogWrite(rightMotorPWMPin, rightMotorSpeed);
 digitalWrite(rightMotorDirPin, LOW);
 }
 else {
 analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
 digitalWrite(rightMotorDirPin, HIGH);
 }
}

void init_PID() { 
 // initialize Timer1
 cli(); // disable global interrupts
 TCCR1A = 0; // set entire TCCR1A register to 0
 TCCR1B = 0; // same for TCCR1B 
 // set compare match register to set sample time 5ms
 OCR1A = 9999; 
 // turn on CTC mode
 TCCR1B |= (1 << WGM12);
 // Set CS11 bit for prescaling by 8
 TCCR1B |= (1 << CS11);
 // enable timer compare interrupt
 TIMSK1 |= (1 << OCIE1A);
 sei(); // enable global interrupts
}

void setup() {

 // set the motor control and PWM pins to output mode
 pinMode(leftMotorPWMPin, OUTPUT);
 pinMode(leftMotorDirPin, OUTPUT);
 pinMode(rightMotorPWMPin, OUTPUT);
 pinMode(rightMotorDirPin, OUTPUT);

 // Led.
 pinMode(LED_BUILTIN, OUTPUT);

 // Initialize serial for output.
 SerialPort.begin(9600);
 
 // Initialize I2C bus.
 DEV_I2C.begin();
 
 AccGyr.begin();
 AccGyr.Enable_X();
 AccGyr.Enable_G();
 // initialize PID sampling loop
 init_PID();
}

void loop() {

 // Read accelerometer and gyroscope.
 int32_t accelerometer[3];
 int32_t gyroscope[3];
 AccGyr.Get_X_Axes(accelerometer);
 AccGyr.Get_G_Axes(gyroscope);

accZ = accelerometer[2];
 accY = accelerometer[1];

 // set motor power after constraining it
 motorPower = constrain(motorPower, -255, 255);
 setMotors(motorPower, motorPower);

gyroX = gyroscope[0];
}

 // The ISR will be called every 5 milliseconds
ISR(TIMER1_COMPA_vect)
{

 accAngle = atan2(accY, accZ)*RAD_TO_DEG;
 
 if(isnan(accAngle));
 else
 Serial.println(accAngle);
 

 currTime = millis();
 loopTime = currTime - prevTime;
 prevTime = currTime;

 gyroRate = map(gyroX, -32768, 32767, -250, 250);
 gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000;
 currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);

 error = currentAngle - targetAngle;
 errorSum = errorSum + error; 
 errorSum = constrain(errorSum, -300, 300);
 //calculate output from P, I and D values
 motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
 prevAngle = currentAngle;

 // Output data.
 //Serial.println(gyroAngle);
 //Serial.println(accAngle);
 Serial.println(currentAngle);
 /* SerialPort.print("| Acc[mg]: ");
 SerialPort.print(accelerometer[0]);
 SerialPort.print(" ");
 SerialPort.print(accelerometer[1]);
 SerialPort.print(" ");
 SerialPort.print(accelerometer[2]);
 SerialPort.print(" | Gyr[mdps]: ");
 SerialPort.print(gyroscope[0]);
 SerialPort.print(" ");
 SerialPort.print(gyroscope[1]);
 SerialPort.print(" ");
 SerialPort.print(gyroscope[2]);
 SerialPort.println(" |");*/
}

 

 

 

 

 

 

 

 

[Andrew Neil]

---

@Giuseppe1 wrote:

the Arduino UNO R3

---

Is that an AVR-based (R3 or earlier) or ARM-based (R4) UNO ?

 

---

@Giuseppe1 wrote:

 I installed the library (STM32duino,LSM6DSO), 

---

STM32duino is an Arduino Core for running on STM32 chips - so not relevant here?

 

---

@Giuseppe1 wrote:

 tried to create a code for a self-balancing robot. But it doesn't print the right angle.

---

So before leaping into all the complexities of self-balancing robots, did you first get the basic printing and sensor reading working?

You need that foundation working first before you move on to the advanced stuff!

[Giuseppe1]

It is AVR-based. 
I tried to print the x,y,z values of the accelerometer and gyroscope, and it worked(code below)
Then i followed this tutorial: https://www.instructables.com/Arduino-Self-Balancing-Robot-1/ 
So i changed the lines where there is the MPU sensor. But it print (for current angle) always like -560(it never change the value).

// Includes
#include <LSM6DSOSensor.h>

#ifdef ARDUINO_SAM_DUE
#define DEV_I2C Wire1
#elif defined(ARDUINO_ARCH_STM32)
#define DEV_I2C Wire
#elif defined(ARDUINO_ARCH_AVR)
#define DEV_I2C Wire
#else
#define DEV_I2C Wire
#endif
#define SerialPort Serial

// Components
LSM6DSOSensor AccGyr(&DEV_I2C);

void setup() {
 // Led.
 pinMode(LED_BUILTIN, OUTPUT);

 // Initialize serial for output.
 SerialPort.begin(115200);
 
 // Initialize I2C bus.
 DEV_I2C.begin();
 
 AccGyr.begin();
 AccGyr.Enable_X();
 AccGyr.Enable_G();
}

void loop() {
 // Led blinking.
 digitalWrite(LED_BUILTIN, HIGH);
 delay(250);
 digitalWrite(LED_BUILTIN, LOW);
 delay(250);

 // Read accelerometer and gyroscope.
 int32_t accelerometer[3];
 int32_t gyroscope[3];
 AccGyr.Get_X_Axes(accelerometer);
 AccGyr.Get_G_Axes(gyroscope);

 // Output data.
 SerialPort.print("| Acc[mg]: ");
 SerialPort.print(accelerometer[0]);
 SerialPort.print(" ");
 SerialPort.print(accelerometer[1]);
 SerialPort.print(" ");
 SerialPort.print(accelerometer[2]);
 SerialPort.print(" | Gyr[mdps]: ");
 SerialPort.print(gyroscope[0]);
 SerialPort.print(" ");
 SerialPort.print(gyroscope[1]);
 SerialPort.print(" ");
 SerialPort.print(gyroscope[2]);
 SerialPort.println(" |");
}

 

[Andrew Neil]

So does that code that you just posted work or not?