Week 9 FYP 2 : Troubleshoot the Programming

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

}