Arduino Reading 9DOF Sensors

MPU6050 + HMC5883L

Arduino IDE - Reading Compass Deflection Function

 COMPASS SENSORS READING ALGORITHM

 ILLUSTRATING COMPASS DEFLECTION READING

Let's follow the first table below, if you are facing on a direction and look at your compass heading and reads 2deg, assume that this is your set-point. Then turn right so that you will be facing at compass heading of 355deg, counting on the compass reference marks with your finger you know that you have turned (or deflect) 7deg right from your original pace (see shaded yellow).

On the second table below, if you are facing on a direction and look at your compass heading which reads 356deg and assume that this is your set-point, then  then turn left so that you will be facing at compass heading of 3deg, counting on the compass reference marks with your finger, you know that you have turned (or deflect) 7deg left from your original pace(see shaded yellow)..

Sometimes the real world is much easier to visualize and understand at almost an instance than to put it into mathematical language - indeed it took me a while with the help of excel spreadsheet slowly plotting and figure out as shown below.




The table shows a simulated reading of deflection from known set point where compass was deflecting passing through end-points (i.e. 0/360) which without proper algorithm leads to erroneous reading as shown on second rows (i.e. Reading Difference). With proper algorithm we can actually measure the deflection  (highlighted in yellow) from set-point to any point even as it passes the 0/360 endpoints of the compass as shown on third row of the table.

Heading - is a virtual representation  of 0-360 degree compass

Reading Difference - is the numerical calculation of compass travel base on heading value.

Deflection - is the angle changed from set-point.

Setpoint - is your fix or desired orientation Deflection - Also as error, is angle away from desired or reference orientation. ReadNow - is your current orientation which is then compare with desired/fixed orientation. if Setpoint and ReadNow  is the same, then you are maintaining your orientation.

ALGORITHM IN ARDUINO C:

From the table above, I have derived a function that can be of use for application about compass as shown below.

double az=0; //"az" is variable that read or store real-time data from compass sensor (range 0 to 360)

void setup()
{ Serial.begin(115200);}

void loop()
{double error=0, readNow=0, Setpoint=0;

readNow = az;

error = readCompass(readNow, Setpoint); 

Serial.println(error);
}

//Function to calculate the deflection of needle from setpoint 

double readCompass(double readNow, double Setpoint)
{double eRR;
 
  if (abs(readNow-Setpoint)<=180)  

       {eRR = readNow - Setpoint; Serial.print("readNow - Setpoint");Serial.print("\t");}
  else 
        if (Setpoint<readNow)
        {eRR = readNow - Setpoint - 360; Serial.print("readNow - Setpoint - 360");Serial.print("\t");}
        else
        {eRR = readNow - Setpoint + 360; Serial.print("readNow - Setpoint + 360");Serial.print("\t");}  
  return eRR;
 }


Notes: The algorithm sense the deflection up to maximum of 180 degrees turn from a defined set-points. These algorithm is good enough for Quad-copter application (i.e. to maintain it's heading) as an application example.

TO BE CONTINUE.....................

 

void setup()  { }

void   loop()  { }         

 

void Compute_Rudder()

{float Output_ZMax=100, Output_ZMin=-100, Output_zz;

 

   /*IDENTIFY SETPOINT*/

    if (RD_Pulse>=1493 && RD_Pulse<=1507 && StickCenter==true)

    {//SETPOINT AT CENTER STICK

         if (n<1)

         {SetRuddrAbout_Z=Mag_z;       

          StickCenter=false;  n=+1;

         }//catch/reads value only once while stick on center

    } 

    else if (RD_Pulse<1493 || RD_Pulse>1507)

    {//SETPOINT AT MOVING STICK, MCU

     //exit only when stick back within center range

     SetRuddrAbout_Z=Mag_z; StickCenter=true; n=0;

    }

   

       

   /*COMPUTE ERROR or DEFLECTION*/

   error_Z = readCompass (Mag_z, SetRuddrAbout_Z);

 
 

 /*Filter  error_Z – Rejects low value error*/

int  filter=0.50;

   if (abs(error_Z)< filter)

    {

       error_Z=0; //disregard error_Z less than 0.50

     }

   else

     {

      if (error_Z<0)  error_Z=error_Z + filter; //example   -1 + 0.50 = - 0.50

      if (error_Z>0)  error_Z=error_Z - filter;  // example  +1 – 0.50 = + 0.50

     }//End filter

                        

 

/*COMPUTE PID*/

    if (TH_Pulse>1170)

    {

            errSum_Z = errSum_Z + (error_Z * timeChange/1000);

            dErr_Z = (error_Z - lastErr_Z)*1000 / timeChange;

       

            Output_zz = kpz * error_Z + kiz * errSum_Z - kdz * dErr_Z; 

                    

            if          (Output_zz>Output_ZMax)     {Output_Z= Output_ZMax; }

            else if  (Output_zz<Output_ZMin)  {Output_Z= Output_ZMin; }

            else      Output_Z=Output_zz;

     }

     else

     {

      Reset_Z();

     }//End of if TH_Pulse>1170

}//End Compute Rudder                             

 

 

  

/*COMPASS DEFLECTION FUNCTION*/

float readCompass(float readNow, float SetRuddrAbout_Z)

 {float eRR;

  

  if (abs(readNow-SetRuddrAbout_Z)<=180) 

       {eRR = readNow - SetRuddrAbout_Z;}

   else

        if (SetRuddrAbout_Z<readNow)

        {eRR = readNow - SetRuddrAbout_Z - 360; }

        else

        {eRR = readNow - SetRuddrAbout_Z + 360; } 

   return eRR;

  }//End Compass function

Arduino, ESC and Brushless Motor

 

Arduino controlling a Brushless DC Motor via an ESC

Brushless DC Motor :

A brushless DC motor (BLDC) requires a driver called Electronic Speed Controller (ESC) to power and spin it. Hence, controlling the ESC we can control the BLDC motor.

ESC:

The ESC feeds a 3-phase electrical sine wave pulse to the motor via the three wires (Black, yellow, and red wire) of the motor. Varying the frequency of the sine wave, you can varies the speed of the motor, higher the frequency the higher the speed of the motor.  Everything about running the motor is taken care by ESC.

Therefore on this topic we will not cover in details how the ESC spin the motor but we are interested what are the "inputs" needed by the ESC in order for it to run, speedup, slowdown, or stop the motor.

ESC INPUTS:

While the output of ESC is a sine wave to run the motor, its input is a square wave or PWM wave.
For the ESC to work, we need two things.
 

1. Arm the ESC, by creating a minimum pulse of 1100us (Typical on most ESC) at power up. Once the  ESC recognizes this pulse (about 1 to 3 sec from power on of MCU), it creates a 3 fast beeps  which tells you that it is armed, and ready to Rock! - the ESC wont spin or power the motor yet!
2. Once Armed, you can increase or decrease the pulse in the range 1100us to 1900us. This will turn the motor slower at shorter  pulse (above 1100us) and faster at longer pulse max 1900us. (NOTE: Typically ESC is designed to power up or spin the motor at about 1180us.)

Note that once the ESC was armed, the pulse at anytime should not be lower than arming pulse which is 1100us, otherwise you have to re-armed it again.

HOW ARDUINO CREATES A PULSE:

The Arduino can create a pulse in many ways from simple sketch to a more advance but precise sketch which will be presented next....

Power and Wiring Setup:

Buttons: There are three button (connected at pin 2, 3, and 4 via blue wire) act as a speed controller switch i.e. to control the pulse the Arduino feeds to ESC data wire (orange wire). Each buttons is equipped with 5k pull-down resistors (connected to common GND pin via green wire) to avoid stray current going to input pins.

ESC: The Electronic Speed Controller (ESC) has three main wire as follows:

•  The power input 2-thick-wire (-Black and + Red ),   hooked to a Battery (Typical Lipo battery provides 7.4v, 11.1v, 14.8v, etc.) to provide power to ESC, Motor, and the Battery eliminator wire.
• The output 3-thick-wires (Black, Yellow, and Green) which provide power to the motor - however the output power is a regulated by ESC in a sine wave.
• The 5v output wire (thin Red) also called "battery eliminator circuit" but this is simply a regulate step down power to supply 5v electronics when needed.
• The Data Input wire is the control wire of the ESC, the wire receives and accepts square wave PWM signal which the ESC interprets and process to create a sine wave output to motor.

ESC can accept pulse at different frequencies from 50Hz (20ms Period) to 500Hz (2ms Period) at an amplitude from 3.6 to 5.0v at its data wire. These values are typical range on most ESC on the market use on hobby toys (e.g. helicopter, airplane, cars, etc.). However, these may varies on some manufacturers and the most typical range I know at this point is 50Hz (20ms Period) by default, and most of them can be re-program up to 400Hz using ESC program cards. f = 1/Period = 1/20 = 50Hz

Arduino MCU: The microcontroller unit (MCU) is responsible for creating a pulse sends to its output pin. For this illustration, PIN 6 is selected to be an output pin connected which has to be connected to Data wire (Orange thin wire) of ESC. ESC  ground wire (Brown thin wire) must be connected to MCU's common ground pin.




Arduino Sketch:

As discussed, the microcontroller must create and feed PWM pulse to ESC data line. The sketch below Version 1.0  will create a default pulse 1100us to arm the ESC at 50Hz then can be incremented or decremented by 1us at button press.

Version 1.0:

int Pulse=1100; //minimum pulse for arming ESC

int wait=0; // delay time to wait human to acknowledge the press

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

    pinMode(2, INPUT);
    pinMode(3, INPUT);
    pinMode(4, INPUT);
    pinMode(6, OUTPUT);
    pinMode(13, OUTPUT);

  } // end of setup
 
void loop ()
  {
    //The function that control the speed of the motor
    Speedy();

     //This is the sketch that generate PWM
     digitalWrite(6, HIGH);
     delayMicroseconds(Pulse);
     digitalWrite(6, LOW);
     delayMicroseconds(10000-Pulse);
     delayMicroseconds(10000);
     
Serial.println(Pulse); //So you can view Pulse on serial window
  }  // end of loop


void Speedy()
  {digitalWrite(13, LOW); 
    
//Scan for button switch starts here:
    if (digitalRead(2)==HIGH)//Start Speedup button at Pin 2
    {wait=wait+1; 
    if (wait>10) {Pulse=Pulse+1; wait=0;digitalWrite(13, HIGH);}
    }//End speedup button at Pin 2
   
    else if (digitalRead(3)==HIGH)//Start slow down button at Pin 3
      {wait=wait+1; 
        if (wait>10) {Pulse=Pulse-1; wait=0; digitalWrite(13, HIGH);}
          if (Pulse<1100) {Pulse=1100;}//In any case the ESC will remain Arm 
       }//End slow down button at Pin 3
   
    else if (digitalRead(4)==HIGH)  //Cut-off button at Pin 4
    {Pulse=1100; digitalWrite(13, HIGH);}

    else {Pulse=Pulse;digitalWrite(13, LOW);}//If no button press, pulse remains as last pulse
 }

The above is the simplest sketch to create a PWM signal using digitalWrite() function and will be fine in most non-demanding or non-precision application.

Improving to Version 1.1:

For a more efficient speed control, we will be upgrading the function Speedy() by creating a delay function incWait() (different from the built-in delayMicroseconds() function of Arduino) without affecting the PWM when pressing the button, that is the incWait() will not pause or stop the loop() function from looping.

int Pulse=1100;

int Delay=0;
double  wait=0;

double timePrev, timeNow;

void setup()

{  Serial.begin(9600);

    pinMode(2, INPUT);

    pinMode(3, INPUT);
    pinMode(4, INPUT);
    pinMode(6, OUTPUT);
    pinMode(13, OUTPUT); delay(500);
 }

void loop()
{
     //This is the sketch that generate PWM
     digitalWrite(6, HIGH);
     delayMicroseconds(Pulse);
     digitalWrite(6, LOW);
     delayMicroseconds(10000-Pulse);
     delayMicroseconds(10000);
     Speedy();

}

void Speedy()
{
  //Scan for button switch starts here:
    if (digitalRead(2)==HIGH && Delay==0)
    { Pulse=Pulse+1; digitalWrite(13, HIGH); Delay= 1; timeNow=millis();
     } //End speedup button at Pin 2
   
    else if (digitalRead(3)==HIGH && Delay== 0)
      { Pulse=Pulse-1; digitalWrite(13, HIGH); Delay=1; timeNow=millis();
        if (Pulse<1100) {Pulse=1100;}//In any case the ESC will remain Arm
      }//End speed down button at Pin 3

    else if (digitalRead(4)==HIGH)  //Cut-off motor button at Pin 4
      {
      Pulse=1100; digitalWrite(13, HIGH); Delay=1; timeNow=millis();
      }
   

    else {Pulse=Pulse;} //If no button press, pulse remains the same as previous

    incWait(); //Always checking Delay    
}

//Delay function without pausing/delay on the loop() function

void incWait()

 {
       if ((wait)<200 && Delay==1)
       { timeNow=millis();
         wait = timeNow - timePrev;
         Delay= 1; 

        

         //Data view for debug purposes only

         Serial.print((wait));Serial.print("\t\t");

         Serial.print(timeNow);Serial.print("\t\t");

         Serial.print(timePrev);Serial.print("\t\t");

         Serial.println(Pulse);

       }
      else
       { Delay= 0;  wait =0, timeNow=0, timePrev=millis();

         digitalWrite(13, LOW);

         //Data view for debug purposes only

         Serial.print((wait));Serial.print("\t\t");

         Serial.print(timeNow);Serial.print("\t\t");

         Serial.print(timePrev);Serial.print("\t\t");

         Serial.println(Pulse);

       }
 }

Updates: Version 1.1 has better delay timing function with introduction of function incWait() compare to Version 1.0 which uses loop iteration to delay the button press. However either can have it's practical advantage depending on purpose.

The main purpose we use button switch on this application is to precisely control the speed of motor by precise incrementing the pulse (widen to speedup or narrow it to slow down) at even smallest amount, in this case at every 1us per press (you can lower it to eve less say 500 nanoseconds per press). One application I can think of is testing or gathering data of motor performance at increment power.

Then the very purpose of providing delay upon buttons press is to avoid unnecessary increments. On this example the microcontroller can loops at 50 times a second, 50Hz, or at every 20ms . At this processing speed and without using delay, pressing the button can create multiple increment before the slow "human" (you) can release the button.

Improving to Version 2.0

Version 2.0 focus on improving the PWM by using the Arduino servo library to create the pulse. This versions now uses two virtual clock, the  Timer0 which is the default timer uses by loop() functions and Timer1 which is solely use by servo library that creates the pulse PWM.

#include <Servo.h>

int Pulse=1100; //Default for Arming ESC

int Delay=0;
float timePrev=0, timeNow=0, wait=0;

Servo myservo;  // create servo object to control a servo

void setup()
{

  Serial.begin(9600);

  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);

  pinMode(13, OUTPUT);
  myservo.attach(6);  // attaches the servo on pin 9 to the servo object
}


void loop()
{
   myservo.writeMicroseconds(Pulse);

   Speedy();
}

void Speedy()
  {

  //Scan for button switch starts here:
    if (digitalRead(2)==HIGH && Delay== 0)

    { Pulse=Pulse+1; digitalWrite(13, HIGH); Delay= 1; timeNow =millis();

    } //End speedup button at Pin 2
   
    else if (digitalRead(3)==HIGH && Delay== 0)
     { Pulse=Pulse-1; digitalWrite(13, HIGH); Delay=1; timeNow =millis();
        if (Pulse<1100) {Pulse=1100;}//In any case the ESC will remain Arm 
     }//End speed down button at Pin 3
    else if (digitalRead(4)==HIGH)  //Cut-off motor button at Pin 4
     {
      Pulse=1100; digitalWrite(13, HIGH); Delay=1; timeNow =millis();
     }
    else {
              Pulse=Pulse; 
            } //If no button press, pulse remains as it is

    incWait(); //Always checking button Delay

  }

 void incWait()

 {
       if (Delay==1 && wait<200)
       { timeNow=millis();                                     
        wait = timeNow - timePrev;
        Delay= 1;
        Serial.print(wait); Serial.print("\t");
        Serial.print(Delay); Serial.print("\t");
        Serial.print(timeNow); Serial.print("\t\t");
        Serial.print(timePrev); Serial.print("\t\t");
        Serial.println(Pulse);
       }
        else
       {

       Delay= 0;  wait =0; timePrev = millis();digitalWrite(13, LOW);

        Serial.print(wait); Serial.print("\t");
        Serial.print(Delay); Serial.print("\t");
        Serial.print(timeNow); Serial.print("\t\t");
        Serial.print(timePrev); Serial.print("\t\t");
        Serial.println(Pulse);
       }
 }

Updates: Unlike the version 1.0 and 1.1 which uses Timer0 to create the pulse, this version uses Timer1 (independent with Timer0) which has higher resolution. In some way to say, there are now two virtual clock working at onetime - more flexibility.

Advantages and Disadvantages
this part in progress.......

Stay Tune as we continuously innovate the sketch! 

To be Continued......



Arduino IDE - Time Function

Time Function millis()

millis() - is a time function that measures time since the microcontroller starts. It reads the time lapse every time the function was executed.

For example, the statement  timeNow=millis(); will read the time lapse since microcontroller start in milliseconds and store it in the variable timeNow.

void setup()
{Serial.begin(9600);}

void loop()
{
int timeNow=millis();

Serial.println(timeNow);
}

Another example, we want to measure how fast a microcontroller could loop.


int timeNow=0, timePrevious=0, timeLapse=0;

void setup()
{Serial.begin(9600);}
 
void loop()
{
    timeNow=millis();
    timeLapse=timeNow-timePrevious;
 
   Serial.print(timePrevious);Serial.print("\t");Serial.print(timeNow); Serial.print("\t");Serial.println(timeLapse);
   timePrevious=timeNow;
}

Here's how it works......

On first loop,   
                              statement 1: timeNow is read and recorded  timeNow=millis(); say timeNow=5;
                              statement 2: Time lapse is computed i.e. timeLapse=timeNow - timePrevious ;
                                                   where initially declared timePrevious =0. then  timeLapse=5-0=5ms
                              statement 3: Print or display the time for viewing.
                              statement 4: value of timeNow  is passed on to timePrevious by timePrevious=timeNow;
                                                   This is to record the time a loop was last executed.