Problem Occured
- After soldering and testing the accelerometer, the degree of standing still the accelerometer is strayed from 90°.
- To solved the problem, the accelerometer need to be calibrated so the degree of standing still is stayed on 90 °. By doing this, the programming code need to change a bit.
#include <Servo.h>
Servo leftServo, rightServo;
//Analog read pins
const int xPin = 0;
const int yPin = 1;
const int zPin = 2;
//The minimum and maximum values that came from
//the accelerometer while standing still
//You very well may need to change these
int minVal = 330;
int maxVal = 360;
- The red lettering shows the value that need to be calibrated.
//to hold the caculated values
double x;
double y;
double z;
void setup(){
Serial.begin(9600);
leftServo.attach(9);
rightServo.attach(8);
}
void loop(){
//read the analog values from the accelerometer
int xRead = analogRead(xPin);
int yRead = analogRead(yPin);
int zRead = analogRead(zPin);
//convert read values to degrees -90 to 90 - Needed for atan2
int xAng = map(xRead, minVal, maxVal, -90, 90);
int yAng = map(yRead, minVal, maxVal, -90, 90);
//int zAng = map(zRead, minVal, maxVal, -90, 90);
//Caculate 360deg values like so: atan2(-yAng, -zAng)
//atan2 outputs the value of -π to π (radians)
//We are then converting the radians to degrees
//x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
//y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);
z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
//Output the caculations
//Serial.print("x: ");
//Serial.print(x);
//Serial.print(" | y: ");
//Serial.print(y);
Serial.print(" | z: ");
Serial.println(z);
//////////////////////////
/*
Serial.print("x: ");
Serial.print(xRead);
Serial.print(" | y: ");
Serial.print(yRead);
Serial.print(" | z: ");
Serial.println(zRead);
*/
//Left motor
if ((z <= 50) && (z > 30))
{
leftServo.write(50);
}
else if ((z <= 30) && (z > 0))
{
leftServo.write(30);
}
else if ((z >= 270) && ( z <= 360))
{
leftServo.write(30);
}
else
{
leftServo.write(90);
}
/////////////////
//right motor
if ((z >= 130) && (z < 150))
{
rightServo.write(130);
}
else if ((z >= 150) && (z < 270))
{
rightServo.write(150);
}
else
{
rightServo.write(90);
}
delay(100);//just here to slow down the serial output - Easier to read
}
.bmp)
