Hallo zusammen,
Habe einen Rover5 mit 4 Motoren und eine MCP und Arduino Mega verbaut. on Top ist das Kompass Modul flach montiert. Ich habe keine pullup Widerstände verbaut sondern nutze die im Arduino eingebauten an 5V.
Ich bekomme von meinem CMPS11 keine sinnvollen Werte. roll und pitch verändern sich wenn ich das Fahrzeug manuell bewege, fallen aber rasch auf die gezeigten Werte zurück, auch wenn ich das Fahrzeug schräg halte. die Kompasswerte ändern sich ebenfalls aber nicht im richtigen Mass. (Angel 16 bit schwankt je nach Richtung zwischen 200 und 300 Grad, angel 8 bit entsprechend)
Battery 9V: 9.99 V
Battery 7V: 7.91 V
Battery 5V: 5.00 V
Motor 1 Current: 0.00 A
Motor 2 Current: 0.00 A
Motor 3 Current: 0.00 A
Motor 4 Current: 0.00 A
roll: -35 pitch: 35 angle 16 bit: 329.0 angle 8 bit: 233
Nun habe ich nach Anleitung eine Kalibrierung durchgeführt. Dazu habe ich die Codesequenzen der Messung auskommentiert und die Kalibrierungssequenzen aktiviert.
Das Verhalten des CMPS11 während der Kalibrierung war wie beschrieben (LED aus nach dem Start, Blinken während der Drehung des Fahrzeugs). Das Endergebnis sieht leider nicht besser aus.
Hat jemand einen Tip was ich falsch mache?
Hier noch mein Arduino Code (Die Kalibrierung ist wieder auskommentiert)
#include <Wire.h>
#define CMPS11_ADDRESS 0x60 // Address of CMPS11 shifted right one bit for arduino wire library
#define CMPS11_Byte1 0xF0
#define CMPS11_Byte2 0xF5
#define CMPS11_Byte3 0xF7
#define CMPS11_Byte4 0xF8
#define ANGLE_8 1 // Register to read 8bit angle from
unsigned char high_byte, low_byte, angle8;
char pitch, roll;
unsigned int angle16;
int ledPin = 13;
int battery9VProbe = A0;
int battery9VRawValue = 0;
float battery9VFinalValue = 0.0;
int battery7VProbe = A1;
int battery7VRawValue = 0;
float battery7VFinalValue = 0.0;
int battery5VProbe = A2;
int battery5VRawValue = 0;
float battery5VFinalValue = 0.0;
int motor1CurrentProbe = A15;
int motor1RawValue = 0;
float motor1FinalValue = 0.0;
int motor2CurrentProbe = A14;
int motor2RawValue = 0;
float motor2FinalValue = 0.0;
int motor3CurrentProbe = A13;
int motor3RawValue = 0;
float motor3FinalValue = 0.0;
int motor4CurrentProbe = A12;
int motor4RawValue = 0;
float motor4FinalValue = 0.0;
void setup()
{
Serial.begin(9600);
pinMode(ledPin, OUTPUT);
Wire.begin();
// Initiate calibration
//Wire.beginTransmission(CMPS11_ADDRESS); //starts communication with CMPS11
//Wire.write(byte(0x00)); //set register pointer to command register
//Wire.write(CMPS11_Byte1); //Send command
//Wire.endTransmission();
//delay(20);
//Wire.beginTransmission(CMPS11_ADDRESS); //starts communication with CMPS11
//Wire.write(byte(0x00)); //set register pointer to command register
//Wire.write(CMPS11_Byte2); //Send command
//Wire.endTransmission();
//delay(20);
//Wire.beginTransmission(CMPS11_ADDRESS); //starts communication with CMPS11
//Wire.write(byte(0x00)); //set register pointer to command register
//Wire.write(CMPS11_Byte3); //Send command
//Wire.endTransmission();
//delay(20);
// Stop calibration
//Wire.beginTransmission(CMPS11_ADDRESS); //starts communication with CMPS11
//Wire.write(byte(0x00)); //set register pointer to command register
//Wire.write(CMPS11_Byte4); //Send command
//Wire.endTransmission();
//delay(20);
// initialize timer1 (500ms)
noInterrupts(); // disable all interrupts
TCCR1A = 0;
TCCR1B = 0;
TCNT1 = 0;
OCR1A = 31250; // compare match register 16MHz/256/2Hz
TCCR1B |= (1 << WGM12); // CTC mode
TCCR1B |= (1 << CS12); // 256 prescaler
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
interrupts(); // enable all interrupts
// initialize timer2 (1 ms)
}
ISR(TIMER1_COMPA_vect) // timer compare interrupt service routine
{
digitalWrite(ledPin, digitalRead(ledPin) ^ 1); // toggle LED pin by XOR
// Read battery probe
battery9VRawValue = analogRead(battery9VProbe);
battery7VRawValue = analogRead(battery7VProbe);
battery5VRawValue = analogRead(battery5VProbe);
battery9VFinalValue = battery9VRawValue*10.0/1024.0;
battery7VFinalValue = battery7VRawValue*10.0/1024.0;
battery5VFinalValue = battery5VRawValue*5.0/1024.0;
// read motor current probe
motor1RawValue = analogRead(motor1CurrentProbe);
motor2RawValue = analogRead(motor2CurrentProbe);
motor3RawValue = analogRead(motor3CurrentProbe);
motor4RawValue = analogRead(motor4CurrentProbe);
motor1FinalValue = motor1RawValue * 5.0 / 1024.0;
motor2FinalValue = motor2RawValue * 5.0 / 1024.0;
motor3FinalValue = motor3RawValue * 5.0 / 1024.0;
motor4FinalValue = motor4RawValue * 5.0 / 1024.0;
}
void loop()
{
// put your main code here, to run repeatedly:
Wire.beginTransmission(CMPS11_ADDRESS); //starts communication with CMPS11
Wire.write(ANGLE_8); //Sends the register we wish to start reading from
Wire.endTransmission();
// Request 5 bytes from the CMPS11
// this will give us the 8 bit bearing,
// both bytes of the 16 bit bearing, pitch and roll
Wire.requestFrom(CMPS11_ADDRESS, 5);
while(Wire.available() < 5); // Wait for all bytes to come back
angle8 = Wire.read(); // Read back the 5 bytes
high_byte = Wire.read();
low_byte = Wire.read();
pitch = Wire.read();
roll = Wire.read();
angle16 = high_byte; // Calculate 16 bit angle
angle16 <<= 8;
angle16 += low_byte;
Serial.print("Battery 9V: ");
Serial.print(battery9VFinalValue);
Serial.println(" V");
Serial.print("Battery 7V: ");
Serial.print(battery7VFinalValue);
Serial.println(" V");
Serial.print("Battery 5V: ");
Serial.print(battery5VFinalValue);
Serial.println(" V");
Serial.print("Motor 1 Current: ");
Serial.print (motor1FinalValue);
Serial.println(" A");
Serial.print("Motor 2 Current: ");
Serial.print (motor2FinalValue);
Serial.println(" A");
Serial.print("Motor 3 Current: ");
Serial.print (motor3FinalValue);
Serial.println(" A");
Serial.print("Motor 4 Current: ");
Serial.print (motor4FinalValue);
Serial.println(" A");
Serial.print("roll: "); // Display roll data
Serial.print(roll, DEC);
Serial.print(" pitch: "); // Display pitch data
Serial.print(pitch, DEC);
Serial.print(" angle 16 bit: "); // Display 16 bit angle with decimal place
Serial.print(angle16 / 10, DEC);
Serial.print(".");
Serial.print(angle16 % 10, DEC);
Serial.print(" angle 8 bit: "); // Display 8bit angle
Serial.println(angle8, DEC);
delay(5000);
}
Alles anzeigen
Danke für Eure Unterstützung