Sensors getting crazy

Forum 2005-2010 (read only) Hardware Interfacing
1

Hello,
I am developing an opensource inverted pendulum with Arduino. I have tested all the hardware components (timer, accelerometer, gyroscope and driver) in separate programs and they work properly.
Unfortunately, now I have a very rare error that I don't know why it is happening. The error occurs when I try to send a PWM signal to the driver. In this case, the driver works properly but the accelerometer gives values with a bias. It is to say, when the accelerometer should output 0 radians, the accelerometer outputs 45 radians. The most weird think is that even when the PWM signal produces an output of 0v to the motor, the accelerometer does not work properly.
Finally, I have to say that th Vcc of the driver is directly connected with the Vcc of the accelerometer to the port of 3.3volts of the Arduino.

I'm really lost :S

Pau

Here you have ALL the code:

#include <avr/interrupt.h> 
#include <avr/io.h> 

//sensors
float accelerometer_angle;
float accelerometer;
float giroscope;
float giroscope_angular_velocity;

//driver
#define out_STBY 8
#define out_A_PWM 10
#define out_A_IN2 9
#define out_A_IN1 11
int motor_impulse = 0;

//timer
#define INIT_TIMER_COUNT 6 
#define RESET_TIMER2 TCNT2 = INIT_TIMER_COUNT 
int int_counter = 0; 
volatile int second = 0;

//kalman filter
//kalman matrix
float Kk[2][2];
//sensors noise
float Qk[2][2];
//sensors votality
float Rk[2][2];
//system matrix
float Fk[2][2];
//current states
float xk_k[2];
//current covariance matrix
float Pk_k[2][2];
//sensor inputs
float zk[2];
//time interval
float ti;

void setup()
{
  //serial setup
  Serial.begin(9600);

  //timer setup
  TCCR2A = 0;
  TCCR2B = 1<<CS22 | 1<<CS21;
  TIMSK2 = 1<<TOIE2;
  TCNT2 = 0;

  //kalman setup
  //time interval
  ti = 1.0f/64.0f;
  //kalman matrix
  Kk[0][0] = 1.0f;
  Kk[0][1] = 0.0f;
  Kk[1][0] = 0.0f;
  Kk[1][1] = 1.0f;
  //system matrix
  Fk[0][0] = 1.0f;
  Fk[0][1] = ti;
  Fk[1][0] = 0.0f;
  Fk[1][1] = 1.0f;  
  //sensors noise
  Qk[0][0] = 0.001f;
  Qk[0][1] = 0.0f;
  Qk[1][0] = 0.0f;
  Qk[1][1] = 0.002f;
  //sensors votality
  Rk[0][0] = 0.59f;
  Rk[0][1] = 0.0f;
  Rk[1][0] = 0.0f;
  Rk[1][1] = 0.59f;
  //current states
  xk_k[0] = 0.0f;
  xk_k[1] = 0.0f;
  //current covariance matrix
  Pk_k[0][0] = 0.001f;
  Pk_k[1][0] = 0.0f;
  Pk_k[0][1] = 0.0f;
  Pk_k[1][1] = 0.002f;
  //sensor input
  zk[0] = 0.0f;
  zk[1] = 0.0f;
  
  //driver
  motor_impulse = 0;
  pinMode(out_STBY,OUTPUT);
  pinMode(out_A_PWM,OUTPUT);
  pinMode(out_A_IN1,OUTPUT);
  pinMode(out_A_IN2,OUTPUT);  
  motor_standby(false);
}

ISR(TIMER2_OVF_vect) { 
  RESET_TIMER2; 
  int_counter += 1; 
  //256 -> 1 sec => 4 -> 1/64 sec
  if (int_counter == 4) { 
    int_counter = 0;
    read_sensors();
    kalman();
    control();
  } 
};

void read_sensors(){
    accelerometer = analogRead(0);
    accelerometer_angle = - ( (accelerometer * 1.3846 ) - 360 - 90 ) ;
    giroscope = analogRead(3);
    giroscope_angular_velocity = ( ( giroscope * 0.8875 ) - 300 ) * 3.1416 / 180;
    zk[0] = accelerometer_angle;
    zk[1] = giroscope_angular_velocity;
}

void control(){
}

void loop() 
{
  // THIS PROVOKES AN ERROR IN THE ACCELEROMETER !!!
  motor_speed2(motor_impulse);
  Serial.print(accelerometer_angle, DEC);
  Serial.print("\n");  
  delay(300);
}

void kalman(){
  //predict
  //update index
  float xk_1_k_1[2];
  xk_1_k_1[0] = xk_k[0];
  xk_1_k_1[1] = xk_k[1];

  float Pk_1_k_1[2][2];
  Pk_1_k_1[0][0] = Pk_k[0][0];
  Pk_1_k_1[0][1] = Pk_k[0][1];
  Pk_1_k_1[1][0] = Pk_k[1][0];
  Pk_1_k_1[1][1] = Pk_k[1][1];

  //xk_k_1 = (Fk * xk_1_k_1)

  float xk_k_1[2];
  xk_k_1[0] = (Fk[0][0] * xk_1_k_1[0]) + (Fk[0][1] * xk_1_k_1[1]);
  xk_k_1[1] = (Fk[1][0] * xk_1_k_1[0]) + (Fk[1][1] * xk_1_k_1[1]);

  //Pk_k_1 = (Fk * Pk_1_k_1 * Fk^t) + Qk

  float Fk_t[2][2];
  Fk_t[0][0] = Fk[0][0];
  Fk_t[0][1] = Fk[1][0];
  Fk_t[1][0] = Fk[0][1];
  Fk_t[1][1] = Fk[1][1];

  float PF[2][2];
  PF[0][0] = (Pk_1_k_1[0][0] * Fk_t[0][0]) + (Pk_1_k_1[0][1] * Fk_t[1][0]);
  PF[0][1] = (Pk_1_k_1[0][0] * Fk_t[0][1]) + (Pk_1_k_1[0][1] * Fk_t[1][1]);
  PF[1][0] = (Pk_1_k_1[1][0] * Fk_t[0][0]) + (Pk_1_k_1[1][1] * Fk_t[1][0]);
  PF[1][1] = (Pk_1_k_1[1][0] * Fk_t[0][1]) + (Pk_1_k_1[1][1] * Fk_t[1][1]);

  float FPF[2][2];
  FPF[0][0] = (Fk[0][0] * PF[0][0]) + (Fk[0][1] * PF[1][0]);
  FPF[0][1] = (Fk[0][0] * PF[0][1]) + (Fk[0][1] * PF[1][1]);
  FPF[1][0] = (Fk[1][0] * PF[0][0]) + (Fk[1][1] * PF[1][0]);
  FPF[1][1] = (Fk[1][0] * PF[0][1]) + (Fk[1][1] * PF[1][1]);

  float Pk_k_1[2][2];
  Pk_k_1[0][0] = FPF[0][0] + Qk[0][0];
  Pk_k_1[0][1] = FPF[0][1] + Qk[0][1];
  Pk_k_1[1][0] = FPF[1][0] + Qk[1][0];
  Pk_k_1[1][1] = FPF[1][1] + Qk[1][1];

  //update

  //yk = zk - (Hk * xk_k_1)

  float yk[2];
  yk[0] = zk[0] - xk_k_1[0];
  yk[1] = zk[1] - xk_k_1[1];

  //Sk = (Hk * Pk_k_1 * Hk^t) + Rk

  float Sk[2][2];
  Sk[0][0] = Pk_k_1[0][0] + Rk[0][0];
  Sk[0][1] = Pk_k_1[0][1] + Rk[0][1];
  Sk[1][0] = Pk_k_1[1][0] + Rk[1][0];
  Sk[1][1] = Pk_k_1[1][1] + Rk[1][1];

  //Kk = Pk_k_1 * Hk^t * Sk^-1

  float determinant = 1.0f / ( (Sk[0][0] * Sk[1][1]) - (Sk[1][0] * Sk[0][1]) );

  float Sk_1[2][2];
  Sk_1[0][0] = determinant * Sk[1][1];
  Sk_1[0][1] = determinant * (-Sk[0][1]); 
  Sk_1[1][0] = determinant * (-Sk[1][0]);
  Sk_1[1][1] = determinant * Sk[0][0];

  Kk[0][0] = (Pk_k_1[0][0] * Sk_1[0][0]) + (Pk_k_1[0][1] * Sk_1[1][0]);
  Kk[0][1] = (Pk_k_1[0][0] * Sk_1[0][1]) + (Pk_k_1[0][1] * Sk_1[1][1]);
  Kk[1][0] = (Pk_k_1[1][0] * Sk_1[0][0]) + (Pk_k_1[1][1] * Sk_1[1][0]);
  Kk[1][1] = (Pk_k_1[1][0] * Sk_1[0][1]) + (Pk_k_1[1][1] * Sk_1[1][1]);

  //xk_k = xk_k_1 + (Kk * yk)

  xk_k[0] = xk_k_1[0] + ( (Kk[0][0] * yk[0]) + (Kk[0][1] * yk[1]) );
  xk_k[1] = xk_k_1[1] + ( (Kk[1][0] * yk[0]) + (Kk[1][1] * yk[1]) );

  //Pk_k = (I - (Kk * Hk)) * Pk_k_1

  float IKH[2][2];
  IKH[0][0] = (1.0f - Kk[0][0]);
  IKH[0][1] = (0.0f - Kk[0][1]);
  IKH[1][0] = (0.0f - Kk[1][0]);
  IKH[1][1] = (1.0f - Kk[1][1]);

  Pk_k[0][0] = (IKH[0][0] * Pk_k_1[0][0]) + (IKH[0][1] * Pk_k_1[1][0]);
  Pk_k[0][1] = (IKH[0][0] * Pk_k_1[0][1]) + (IKH[0][1] * Pk_k_1[1][1]);
  Pk_k[1][0] = (IKH[1][0] * Pk_k_1[0][0]) + (IKH[1][1] * Pk_k_1[1][0]);
  Pk_k[1][1] = (IKH[1][0] * Pk_k_1[0][1]) + (IKH[1][1] * Pk_k_1[1][1]);
}
 
void motor_speed2(char speed) { //speed from -100 to 100
  byte PWMvalue=0;
  PWMvalue = map(abs(speed),0,100,50,255); //anything below 50 is very weak
  if (speed > 0)
    motor_speed(0,PWMvalue);
  else if (speed < 0)
    motor_speed(1,PWMvalue);
  else {
    motor_coast();
  }
}
void motor_speed(boolean direction, byte speed) { //speed from 0 to 255
    if (direction == 0) {
      digitalWrite(out_A_IN1,HIGH);
      digitalWrite(out_A_IN2,LOW);
    } else {
      digitalWrite(out_A_IN1,LOW);
      digitalWrite(out_A_IN2,HIGH);
    }
    analogWrite(out_A_PWM,speed);
}
void motor_standby(boolean state) { //low power mode
  if (state == true)
    digitalWrite(out_STBY,LOW);
  else
    digitalWrite(out_STBY,HIGH);
}
void motor_brake() {
    digitalWrite(out_A_IN1,HIGH);
    digitalWrite(out_A_IN2,HIGH);
}
void motor_coast() {
    digitalWrite(out_A_IN1,LOW);
    digitalWrite(out_A_IN2,LOW);
    digitalWrite(out_A_PWM,HIGH);
}
2

Is your circuit adequately decoupled?

3

The only connection that the driver shares with other components is the arduino Vcc source of 3.3v. All the components (gyroscope, accelerometer and driver) use this port.
The driver uses as inputs the ports: 11, 10, 9 and 8. Moreover, it uses the Vmotor and the ground motor ports that are connected to a 9v battery. Finally, A01 and A02 are connected to the motor.
I think there might be a backward current that interferences my circuit so I have to wire a diode somewhere... :S

4

Now I found the problem! : the driver needed 5v, not 3.3v
It is difficult to read Japanese electronic specifications :slight_smile: