Salve a tutti ho un piccolo problema con il progetto di maturità (quindi sicuramente sarà un programma elementare) del mio fratello e non riesco ad aiutarlo perchè non sono molto pratico di questo programma XD
in pratica ha realizzato una maschera fissata ad un telaio a sua volta fissata ad un servomotore che la fa muovere.
i movimenti del servomotore sono pilotati da un magnetometro e da un accelerometro. i movimenti della maschera in verticale (pilotati dall'accelerometro) sono stati fissati a giù centro e su .... mentre i valori orizzontali sono stati lasciati liberi ...
il tutto funziona molto bene tranne per un fatto. i valori letti dal giroscopio sono affetti da rumore e anche a sensore fermo appoggiato sul tavolo i valori continuano ad oscillare di qualche grado e di conseguenza la testa trema in orizzontale XD
Vorrei chiedere a voi che sicuramente siete più esperti di me come correggere questo problema aggiungendo qualche stringa allo sketch... Per esempio applicando un filtro.
ecco il codice
#include <Wire.h>
#include <Servo.h>
Servo myservo_dx_sx;
Servo myservo_su_giu;
const int analogInY = A1; // X-axis analog input
const int sleep = 2; // Sleep mode pin
float valueY = 0; // value read from the asse X
int risultato=0;
#define MAG_ADDR 0x0E //7-bit address for the MAG3110, doesn't change
void setup()
{
myservo_dx_sx.attach(9);
myservo_su_giu.attach(10);
pinMode(sleep, OUTPUT); // initialize the Sleep pin as digital output
digitalWrite(sleep, HIGH); // remove the accelerometer from the sleep mode
analogReference(EXTERNAL);
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial for output
config(); // turn the MAG3110 on
}
void loop()
{
print_values();
delay(5);
int valoreX=readx();
Serial.println(valoreX);
valoreX = map(valoreX,550 , 330, 30, 130);
myservo_dx_sx.write(valoreX);
// in questa prima parte gestiamo il motore destra - centro - sinistra
// qui sotto gestiamo il motore su - giù
valueY = (analogRead(analogInY) * (3.3 / 1023.0));
valueY=valueY*10;
Serial.print("Y = ");
Serial.println(valueY);
delay(100);
if (valueY<12.9){
myservo_su_giu.write(90);
}
if ((valueY>13.0)&(valueY<17.6)){
myservo_su_giu.write(60);
}
if ((valueY>17.7)){
myservo_su_giu.write(30);
}
}
void config(void)
{
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x11); // cntrl register2
Wire.write(0x80); // send 0x80, enable auto resets
Wire.endTransmission(); // stop transmitting
delay(15);
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x10); // cntrl register1
Wire.write(1); // send 0x01, active mode
Wire.endTransmission(); // stop transmitting
}
void print_values(void)
{
Serial.print("x=");
Serial.println(readx());
}
int readx(void)
{
int xl, xh; //define the MSB and LSB
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x01); // x MSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
xh = Wire.read(); // receive the byte
}
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x02); // x LSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
xl = Wire.read(); // receive the byte
}
int xout = (xl|(xh << 8)); //concatenate the MSB and LSB
return xout;
}
int ready(void)
{
int yl, yh; //define the MSB and LSB
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x03); // y MSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
yh = Wire.read(); // receive the byte
}
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x04); // y LSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
yl = Wire.read(); // receive the byte
}
int yout = (yl|(yh << 8)); //concatenate the MSB and LSB
return yout;
}
int readz(void)
{
int zl, zh; //define the MSB and LSB
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x05); // z MSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
zh = Wire.read(); // receive the byte
}
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.beginTransmission(MAG_ADDR); // transmit to device 0x0E
Wire.write(0x06); // z LSB reg
Wire.endTransmission(); // stop transmitting
delayMicroseconds(2); //needs at least 1.3us free time between start and stop
Wire.requestFrom(MAG_ADDR, 1); // request 1 byte
while(Wire.available()) // slave may send less than requested
{
zl = Wire.read(); // receive the byte
}
int zout = (zl|(zh << 8)); //concatenate the MSB and LSB
return zout;
