2024-03-18 05:38 AM
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(" |");*/
}
2024-03-18 05:49 AM
@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!
2024-03-18 08:33 AM
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(" |");
}
2024-03-18 08:47 AM
So does that code that you just posted work or not?