Hello everyone, i am working on a tilt sensing application wich should adjust servo position according to data coming from an accellerometer. I connected a small laser to the servo, in this way i can watch on a wall if the device is working.
I'm currently using LSM6DS3 sensor and the servo included in arduino starter kit (waiting for a parallax 900-00005). The problem happens in static condition: accelerometer is locked on a certain angulation but the servo didn't stop. I can see the laser moving on the wall. It's not a big movement, i think 1 degree of oscillation around the wished postion.
I think that it's a problem of the bad quality servo included in starter kit, that's why i ordered a parallax.
Anyone has an idea?
Here's the code
//Programmazione di Arduino per il tilt sensing e motorizzazione di un servo
//affinchè segua le variazioni angolari su singolo asse di un accelerometro
//Include le librerie necesarie
#include "SparkFunLSM6DS3.h"
#include "Wire.h"
#include "SPI.h"
#include "Servo.h"
//Crea gli oggetti e le variabili
LSM6DS3 myIMU;
Servo servo;
double AcX,AcY,AcZ;
int Pitch;
float sommaX, sommaY, sommaZ = 0;
int counter = 0;
void setup() {
Serial.begin(9600); //Imposta il data rate in baud per la comunicazione seriale
pinMode(3, OUTPUT);
digitalWrite(3, LOW); //Accende il laser
servo.attach(8); //Servomotore collegato al pin 8 delle uscite digitali di Arduino
myIMU.begin(); //Inizializzazione MPU
myIMU.settings.accelEnabled = 1; //Abiita accelerometro
myIMU.settings.accelRange = 2; //Massima forza di gravità percepibile
myIMU.settings.accelSampleRate = 1600; //Sample rate dell'accelerometro in Hz.
myIMU.settings.accelBandWidth = 50; //Larghezza di banda dell'accelerometro in Hz.
}
void loop()
{
//Acquisizione degli assi X, Y, Z dell'accelerometro
sommaX = sommaX + myIMU.readFloatAccelX();
sommaY = sommaY + myIMU.readFloatAccelY();
sommaZ = sommaZ + myIMU.readFloatAccelZ();
conta++;
if (coounter >= 20){
AcX = sommaX/counter;
AcY = sommaY/counter;
AcZ = sommaZ/counter;
counter = 0;
sommaX = sommaY = sommaZ = 0;
}
//Calcolo dell'angolo di pitch (asse X)
Pitch = FunctionPitch(AcY, AcX, AcZ);
Serial.println(Pitch);
//Mapping dei valori dell'angolo di pitch da un internvallo (-90,90) ad uno (0,180)
int ServoPitch = map(Pitch, -90, 90, 180, 0);
//Il servo acquiisce la stessa posizione dell'angolo di pitch
servo.write(ServoPitch);
}
//Funzione per il calcolo dell'angolo di Pitch
double FunctionPitch(double A, double B, double C){
double DatoA, DatoB, Value;
DatoA = A;
DatoB = (B*B) + (C*C);
DatoB = sqrt(DatoB);
Value = atan2(DatoA, DatoB);
Value = ceil(Value * 180/3.14);
return (int)Value;
}