Hello @ all
Here ist my Program:
// Libraries
#include <Wire.h>
#include <CMPS10.h>
#include <NewPing.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
// Create instance of digital compass
CMPS10 compass;
// Motor pins
#define CMPS10Addr 0x60 // Addresse des CMPS10
#define leftMtrDirPin1 12// Motor Block A
#define leftMtrDirPin2 9
#define leftMtrSpdPin 3
#define rightMtrDirPin1 13 //Motor Block B
#define rightMtrDirPin2 8
#define rightMtrSpdPin 11
#define SONAR3_TRIGGER_PIN 26
#define SONAR3_ECHO_PIN 27
#define SONAR3_MAX_DISTANCE 50
NewPing sonar3(SONAR3_TRIGGER_PIN, SONAR3_ECHO_PIN, SONAR3_MAX_DISTANCE); // Sensor links
#define SONAR2_TRIGGER_PIN 28
#define SONAR2_ECHO_PIN 29
#define SONAR2_MAX_DISTANCE 50
NewPing sonar2(SONAR2_TRIGGER_PIN, SONAR2_ECHO_PIN, SONAR2_MAX_DISTANCE); // Sensor mitte
#define SONAR1_TRIGGER_PIN 30
#define SONAR1_ECHO_PIN 31
#define SONAR1_MAX_DISTANCE 50
NewPing sonar1(SONAR1_TRIGGER_PIN, SONAR1_ECHO_PIN, SONAR1_MAX_DISTANCE); // Sensor rechts
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); // Set the LCD I2C address
// Variables for direction control
float initial_heading;
float target_heading;
float diff_heading;
bool turn_init;
bool target_reached;
//Servo Initialisierung
Servo servo1; //Servo Objekte erzeugen
Servo servo2;
const int servoPin1 = 33; // Servo vaagrechte bewegung
int servoPos1 = 70;
const int servoPin2 = 34; // Servo senkrechte bewegung
int servoPos2 = 140;
//Ultraschallsensoren initialisierung
const byte TRIGGER1 = 26; //Sensor Links
const byte TRIGGER2 = 28; // Sensor mitte
const byte TRIGGER3 = 30; // Sensor rechts
const byte ECHO1 = 27; //sensor links
const byte ECHO2 = 29; //sensor mitte
const byte ECHO3 = 31; //sensor rechts
// Left & right
#define LEFT 0
#define RIGHT 1
void setup(void)
{
// Start Serial
Serial.begin(9600);
Wire.begin(); // Connect to I2C
lcd.begin(16, 2);
servo1.attach(servoPin1); // Servo waagerecht
servo2.attach(servoPin2);
// Declare motor pins as outputs
pinMode(leftMtrSpdPin, OUTPUT); // initialiesierung Motor Kanal A
pinMode(leftMtrDirPin1, OUTPUT);
pinMode(leftMtrDirPin2, OUTPUT);
pinMode(rightMtrDirPin1, OUTPUT);// initialiesierung Motor Kanal B
pinMode(rightMtrDirPin2, OUTPUT);
pinMode(rightMtrSpdPin, OUTPUT);
// Initialise direction variables
turn_init = false;
target_reached = false;
}
void loop(void)
{
delay(30);
unsigned int uS1 = sonar1.ping(); //Sensor rechts
//Serial.print("Ping: ");
//Serial.print(uS1 / US_ROUNDTRIP_CM);
//Serial.println("cm");
uS1 = uS1 / US_ROUNDTRIP_CM;
if (uS1 == 0) {
uS1 = 50;
}
delay(30);
unsigned int uS2 = sonar2.ping(); // Sensor mitte
//Serial.print("Ping: ");
//Serial.print(uS2 / US_ROUNDTRIP_CM);
//Serial.println("cm");
uS2 = uS2 / US_ROUNDTRIP_CM;
if (uS2 == 0) {
uS2 = 50;
}
delay(30);
unsigned int uS3 = sonar3.ping(); // Sensor links
//Serial.print("Ping: ");
//Serial.print(uS3 / US_ROUNDTRIP_CM);
//Serial.println("cm");
uS3 = uS3 / US_ROUNDTRIP_CM;
if (uS3 == 0) {
uS3 = 50;
}
// Drehung begrenzen
if (servoPos1 < 20) servoPos1 = 20;
if (servoPos1 > 140) servoPos1 = 140;
if (servoPos2 < 100) servoPos2 = 100;
if (servoPos2 > 170) servoPos2 = 170;
// Position der Servos setzen
servo1.write(servoPos1);
delay(20);
servo2.write(servoPos2);
delay(20);
// Nach Rechts
if (uS3 < 16) {
turn(90, RIGHT);
}
// Nach Links
if (uS1 < 16) {
turn(90, LEFT);
}
}
// Turn of a given angle
void turn(float angle, bool turn_direction) {
// Get initial heading
if (!turn_init) {
Serial.println("Starting turn ...");
initial_heading = get_heading();
if (turn_direction) {
target_heading = initial_heading + angle;
}
else {
target_heading = initial_heading - angle;
}
if (target_heading > 359){ //korrectur der 360°
target_heading = target_heading-360;
}
Serial.print("Initial heading: ");
Serial.println(initial_heading);
Serial.print("Target heading: ");
Serial.println(target_heading);
turn_init = true;
target_reached = false;
diff_heading = 0;
}
float heading = get_heading();
if (turn_direction) {
diff_heading = target_heading - heading;
}
else {
diff_heading = heading - target_heading;
}
while (diff_heading > 0 && !target_reached) {
// Move & stop
if (turn_direction) {
digitalWrite(leftMtrDirPin1, LOW); //Nach Rechts drehen
digitalWrite(leftMtrDirPin2, LOW);
digitalWrite(rightMtrDirPin1, HIGH);
digitalWrite(rightMtrDirPin2, LOW);
analogWrite(leftMtrSpdPin, 155);
analogWrite(rightMtrSpdPin, 155);
}
else {
digitalWrite(leftMtrDirPin1, HIGH); // Nach links drehen
digitalWrite(leftMtrDirPin2, LOW);
digitalWrite(rightMtrDirPin1, LOW);
digitalWrite(rightMtrDirPin2, LOW);
analogWrite(leftMtrSpdPin, 155);
analogWrite(rightMtrSpdPin, 155);
}
delay(150);
analogWrite(leftMtrSpdPin, 0);
analogWrite(rightMtrSpdPin, 0);
delay(50);
// Measure heading again
float heading = get_heading();
if (turn_direction) {
diff_heading = target_heading - heading;
}
else {
diff_heading = heading - target_heading;
}
if (diff_heading >359){ //mein code
diff_heading = diff_heading-360;
} //mein Code Ende
Serial.print("Difference heading (degrees): "); Serial.println(diff_heading);
//lcd.setCursor(0, 0);
//lcd.print("Difference heading ");
//lcd.setCursor(0, 1);
//lcd.println(diff_heading);
if (diff_heading < 0 && !target_reached) {
target_reached = true;
turn_init = false;
diff_heading = 0;
}
}
// Stop
Serial.println("Stopping turn ...");
}
// Get heading from the compass
float get_heading() {
/////////////////////////////////////////////////////////////
// Kompass Modul Abfrage und senden der Daten ///
////////////////////////////////////////////////////////////
byte byteHigh, byteLow; // byteHigh / byteLow für Bearing
char pitch, roll; // Pitch und Roll
int bearing; // Bearing
Wire.beginTransmission(CMPS10Addr); // Kommunikation mit CMPS10
Wire.write(2); // Start Register (2)
Wire.endTransmission();
Wire.requestFrom(CMPS10Addr, 4); // Abfrage von 4 Bytes vom CMPS10
while (Wire.available() < 4) ; // Warten, bis 4 Bytes verfügbar
byteHigh = Wire.read(); // High-Byte für Bearing speichern
byteLow = Wire.read(); // Low-Byte für Bearing speichern
pitch = Wire.read(); // Byte für Pitch speichern
roll = Wire.read(); // Byte für Roll speichern
bearing = ((byteHigh << 8) + byteLow) / 10; // Bearing berechnen
sendData(bearing, pitch, roll); // Daten versenden
float headingDegrees = bearing;
return headingDegrees;
}
void sendData(int bearing, int pitch, int roll) {
String data = String(bearing) + ";" + String(pitch) + ";" + String(roll);
lcd.setCursor(0, 0);
lcd.print("bear-pitch-roll");
lcd.setCursor(0, 1);
lcd.print(data);
// Serial.println(data);
delay(100);
}
Unfortunetly I have a problem with the heading value of my compass module CMPS 10 (full Tilt compensated Module ) .
My robot ist turn Pefect the Pos. à 90 ° only then when the heading is between 0 an 270 ° (360°- 90° = max up 270° )
If I am, or better say, my compass heading higher than 270° the target is geting out oft he range ( for example heading = 286 + 90 = 376 ° ?!! )
The result is I can never reach the target position and the robot is endless turning . So I have to turn him off
What can i do to compensate the target „error“ in my code ?
Ok i can compensate the error with:
If (target_heading >359){
target _heading=target_heading-360;
}
But in this case is the diff_heading out of the range…
You see with solving my first problem i get anothen one … and so far…
What can i do to fix the problem and turning my vehicle also above the heading bigger then 270° ?
I hope you can help me.
greatings
Chris