CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug Reports on this forum. Send them to

Controlling 2 Servos with IMU

Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message

Joined: 28 Nov 2013
Posts: 28
Location: Stillwater, OK

View user's profile Send private message

Controlling 2 Servos with IMU
PostPosted: Sun Dec 01, 2013 2:14 pm     Reply with quote

Hey guys,

For my Mechatronics Final Project, I wanted to use an IMU for orientation recognition, and then 2 servo motors to counter the rotation on 2 axis to stabilize an object.


PIC 18F4520 (Controller)
IMU 3000 ADXL 345 Fusion board by Sparkfun
2 Futaba S3004 Servo Motors
4 AA Batteries
Logic Level Converter by Sparkfun
317T Voltage regulator
Various resistors/capacitors/wires

Note, in the code I end up not using the gyro because I think my IMU is broke as the IMU gyros will not respond, the device communicates over I2C but the data registers are always 0x00.

#include <18f4520.h> //Library for the Pic
#include <stdio.h>   //Library for erros
#include <string.h>  //Library for strings
#include <stdlib.h>  //Returns Library
#include <math.h>    //Math Library for Atan2 Functions

#fuses HS,NOLVP,NOWDT,PUT     //Pic18f4520 Fuses
#use delay(clock=20000000)    //20000000 Hz Core Clock

#use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7)  //Serial Comms
#use i2c(master, sda=pin_C4, scl=pin_C3)                 //I2C Comms

#define INTS_PER_ISR 1                 //Setting the Timer Mode
#define Accel_W 0xA6                   //ADXL 345 Write Addr.
#define Accel_R 0xA7                   //ADXL 345 Read Addr.
#define Accel_D 0x32                   //ADXL 345 Data Addr.
#define Data_Mode 0x31                 //Mode to Config the Accel
#define PWR_Ctrl 0x2D

signed int16 accelResult[3];  //Accelerometer Readings
float Gx, Gy, Gz,pitch,roll;  //G Readings and Pitch/Roll Conversions
long Pdelay,Rdelay;           //Puse Length for the Servos
int8 int_count;               //Variable to Make to enable ISR

void clock_isr()                       //Our interrupt function that goes off every 20 ms.
   if(--int_count==0)                  //IF we are somewhere in the main
      int_count=INTS_PER_ISR;          //Resetting the switch
         Pdelay=-pitch*6+1500;         //Calculating the Pulse Width for the Angle of Pitch
         output_high(PIN_A3);          //Rise of the pulse
         delay_us(Pdelay);             //Width of the pulse
         output_low(PIN_A3);           //End of the pulse
         Rdelay=roll*9+1500;           //Calculating the Pulse Width for the Angle of Roll
         output_high(PIN_A4);          //Rise of the pulse
         delay_us(Rdelay);             //Width of the pulse
         output_low(PIN_A4);           //End of the pulse
   set_timer0(53036);             //Offset to make sure we have an interrupt ever 20ms

void writeTo(int device, int toAddress, int val) { //Write Function
   i2c_start();                                    //Starting Comms
      i2c_write(device);                           //Device Register Address
      i2c_write(toAddress);                        //Param Register Address
      i2c_write(val);                              //Value to Set Register Too
   i2c_stop();                                     //Stop Comms

void readFrom(int device, int fromAddress, int result[]) {  //Read Data Func
   i2c_start();                                             //Starting Comms
      i2c_write(device);                                    //Device Register Add
      i2c_write(fromAddress);                               //Device Data Reg                                     
   i2c_start();                                             //Restart the Line
      i2c_write(Accel_R);                                   //Read Address
            result[0]=i2c_read();                           //X LB
            result[1]=i2c_read();                           //X HB
            result[2]=i2c_read();                           //Y LB
            result[3]=i2c_read();                           //Y HB
            result[4]=i2c_read();                           //Z LB
            result[5]=i2c_read(0);                          //Z HB
      i2c_stop();                                          //Stop Comms

void getAccelerometerReadings(int16 gyroResult[]) {        //Accel Result Func
   int buffer[6];                                          //Temp Var.
   readFrom(Accel_W,Accel_D,buffer);                             //Call readFrom Func
      accelResult[0]=make16(buffer[1],buffer[0])+15;       //X value + offset
      accelResult[1]=make16(buffer[3],buffer[2])+15;       //Y Value + offset
      accelResult[2]=make16(buffer[5],buffer[4])+18;       //Z Value + offset

void AccelInit() {                     //Setting up the Accel.
   writeTo(Accel_W,Data_Mode,0x09);   //Setting the Accel to 11 bit +/-4g
   delay_ms(150);                     //Delay just for safe Practice
   writeTo(Accel_W,PWR_Ctrl,0x08);   //Wake Accel Up and Set to Measure Mode.
   delay_ms(150);                    //Delay just for safe Practice
void main() {

   AccelInit();            //Set up the Gyros
   int_count = INTS_PER_ISR;     //ISR switch on
   set_timer0(53036);                   //Offset to make sure we have an interrupt ever 20ms 
   setup_timer_0(RTCC_INTERNAL | RTCC_DIV_8); //Setting up the internal timer, but with a prescale of /8
   enable_interrupts(INT_TIMER0);            //using Timer 0
   enable_interrupts(GLOBAL);                //Global Interrupt

   while(true) {
      getAccelerometerReadings(accelResult);    //Calling the Func. To read Accel
      Gx=((float)(accelResult[0]));             //Converting to float
      Gy=((float)(accelResult[1]));             //Converting to float
      Gz=((float)(accelResult[2]));             //Converting to float
      roll=(atan2((Gy)/256,(Gz-256)/256)+PI)*360/(2*PI)*2;  //Calculating roll from accel.
         if( roll > 360)                        //Changing the Orientation (you may not need this).
      pitch=(atan2((Gx)/256,(Gz-256)/256)+PI)*360/(2*PI)*2; //Calculating Pitch from accel.
         if( pitch > 360)  {                    //Changing the Orientation (you may not need this).

      printf("%f",pitch);     //Printing the Pitch
      printf("%f",roll);      //Printing the Roll


Again not sure if I did it the best way, but it works and it's fairly decent. If anyone has any questions please let me know, and I can try to answer them to the best of my ability.
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library All times are GMT - 6 Hours
Page 1 of 1

Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum

Powered by phpBB © 2001, 2005 phpBB Group