OpenServo.com

[quantumechanic]

Greetings all,

I am using the code (below) to control a servo through an Arduino Duemilanove with ATMega 328 microcontroller, and an interesting thing is happening. The code is straightforward: the first instruction in the 'void loop' is to write to the servo to tell it where to go, the second instruction is to read the position of the servo, the final instruction prints the position to the screen.

When the write position instruction is the first instruction in the void loop and the read position instruction is second, the position value that is displayed on the screen is incorrect, the value is constantly 104.

When the read position instruction is the first instruction in the void loop and the write position instruction is second, the position value that is displayed on the screen is correct, however the servo does not seek the correct position.

Basically, whatever instruction comes first in the void loop is what gets executed, all subsequent instructions are ignored.

Has anyone encountered this type of problem before? Any suggestions on how to solve this?

Thanks

#include <Wire.h>

// OpenServo registers & commands
#define OPENSERVO_POSITION_HI 0x08
#define OPENSERVO_POSITION_LO 0x09
#define OPENSERVO_SEEK 0x10

#define PID_PGAIN_HI 0x22
#define PID_PGAIN_LO 0x23
#define PID_DGAIN_HI 0x24
#define PID_DGAIN_LO 0x25
#define PID_IGAIN_HI 0x26
#define PID_IGAIN_LO 0x27

#define PWM_ENABLE 0x82
#define PWM_DISABLE 0x83
#define WRITE_ENABLE 0x84
#define WRITE_DISABLE 0x85

#define SAVE_REGISTERS 0x86

// servo i2c addresses
#define SERVO_ADDRESS 0x10

unsigned int data;

void setup(){
Wire.begin();
Serial.begin(9600);
_openservo_command8(SERVO_ADDRESS,WRITE_ENABLE); delay(1);
_openservo_write16(SERVO_ADDRESS,PID_PGAIN_HI,50); delay(1);
_openservo_write16(SERVO_ADDRESS,PID_DGAIN_HI,2); delay(1);
_openservo_write16(SERVO_ADDRESS,PID_IGAIN_HI,3);
_openservo_command8(SERVO_ADDRESS,WRITE_DISABLE);
_openservo_command8(SERVO_ADDRESS,PWM_ENABLE);
_openservo_command8(SERVO_ADDRESS,SAVE_REGISTERS);
}//setup loop

void loop(){
_openservo_write16(SERVO_ADDRESS,OPENSERVO_SEEK,500);
data = _openservo_read16(SERVO_ADDRESS,OPENSERVO_POSITION_HI);

Serial.print("The byte is: "); Serial.println(data);
}//void loop

// init an i2c transmission
void _openservo_begin(int i2c, byte reg) {
Wire.beginTransmission(i2c);
Wire.send(reg);}//start

// read a 16 bit register
unsigned int _openservo_read16(int i2c, byte reg) {
_openservo_begin(i2c, reg);
unsigned int data;
Wire.requestFrom(i2c, 2);
if (Wire.available())
data = Wire.receive() << 8; // high byte
if (Wire.available())
data |= Wire.receive(); // low byte
Wire.endTransmission();
return data;}//read

// write a 16 bit register
void _openservo_write16(int i2c, byte reg, int data) {
_openservo_begin(i2c, reg);
Wire.send(data >> ; // high byte
Wire.send(data & 0xff); // low byte
Wire.endTransmission();}//write

// send an 8 bit command
void _openservo_command8(int i2c, byte reg) {
_openservo_begin(i2c, reg);
Wire.endTransmission();}//command

[ginge]

can't say I have, but I have the tools right here to take a look for you.

Let me throw something together and run your code through it.

.. looking at the code, the onyl thing that strikes me as odd is that the P value is so small. I'm surprised the servo moves.
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[quantumechanic]

Thanks Ginge, were you able to find anything?

Thanks