Zdetier
Joined: 28 Nov 2013
Posts: 28
Location: Stillwater, OK
|
| Controlling 2 Servos with IMU |
 Posted: Sun Dec 01, 2013 2:14 pm |
 |
|
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.
Hardware:
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.
| Code: | #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
#int_timer0
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).
roll-=360;
roll=roll-88;
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).
pitch-=720;
}
printf("%f",pitch); //Printing the Pitch
printf(",");
printf("%f",roll); //Printing the Roll
printf("\n\r");
}
} |
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. |
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
