NXC: problem with reading out the servo position

[noob405]

Hi,

I have several Hitec Servos connected to a Mindsensors NXTServo-v3 Servo module
and tried to add some code to the original NXC library so that I can read out the current position of
a servo.

For testing purpose I just coded a routine routine for port 1 on the controller board.
Here´s the function I added to the library:

Code: Select all

void ShowServo1Position(byte prt, byte adr, byte reg)
{
  SetSensorLowspeed(prt);
  int result = -1;                                                              
  int cnt = 2;
  byte outbuf[];                                                               
  byte cmdbuf[];                                                              
  byte nByteReady = 0;
  string pos;
  
  ArrayBuild(cmdbuf, adr, reg);

  while (I2CStatus(prt, nByteReady) ==  STAT_COMM_PENDING)                      
  {
      Wait(10);
  }

  if(I2CBytes(prt, cmdbuf, cnt, outbuf))                                       
  {
    result = outbuf[0];                                                        

    if(cnt==2)
      result = result + outbuf[1]*256;                                        
  }
  pos = NumToStr(result);
  TextOut(0, LCD_LINE4, pos, true);
}

It seems that this is working. SOMEHOW......
......because I have to call this function twice each time I want to read the position.......

Code: Select all

#include "180813_lib.nxc"
#define SensorPort S1
const byte ServoAddr = 0xb0;                                                    

task main ()
{
   SetSensorLowspeed(SensorPort);
   SetServoSpeed(SensorPort, ServoAddr, 1, 100);
   
   while(true)
   {
      QuickServoSetup(SensorPort, ServoAddr, 1, 50);
      Wait (2000);
      ShowServo1Position(SensorPort, ServoAddr, 0x42);
      ShowServo1Position(SensorPort, ServoAddr, 0x42);
      
      QuickServoSetup(SensorPort, ServoAddr, 1, 200);
      Wait (2000);
      ShowServo1Position(SensorPort, ServoAddr, 0x42);
      ShowServo1Position(SensorPort, ServoAddr, 0x42);
   }
}

So, what´s wrong with my function? Any ideas?

I read in the general I2C documentation, that you have to start the reading with a write command.
As I2CBytes seems to be the higher-level wrapper for this You shouldn´t need it.
Or am I missing something?

Any help/hint is wellcom.

Thanks in advance
noob

[noob405]

Hi again,

is anyone in here using the NXTServo-v3. Would like to solve this
problem and get a deeper understanding.

THX
noob

[HaWe]

noob,
I'm 200% sure that Xander Soldaat ("mightor") should know -
maybe was too busy to read your post while writing a daisy chain lib for EV3/C

write him an email or a PM!
https://sourceforge.net/apps/phpbb/mind ... ofile&u=68

[mattallen37]

Servos (i.e. typical RC style servos) don't have an external position feedback system. You connect them to e.g. 5v and Gnd, and then give them a signal to tell them where you want them to be. Then they attempt to move to that position, but there's no way for the external controller to know the actual position of the servo. This is by design of pretty much every RC style servo. The only exceptions I'm aware of are some special servos sold by adafruit, and the NXTServo-v3 doesn't support that feature anyhow.

[HaWe]

I read in the general I2C documentation, that you have to start the reading with a write command.

what exactly did you read? can you copy it?

[mattallen37]

Typicall in order to do an I2C read, you first need to do an I2C write to set the register pointer.

The following is a simple library I wrote for doing basic I2C read and write operations. The read function takes care of writing the register pointer.

Code: Select all

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  http://mattallen37.wordpress.com/
*  Initial date: pre 2012
*  Last updated: Aug. 29, 2013
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  This is a library of functions to aid in reading from I2C slaves, and writing to them.
*/

#define I2C_COM_ERROR false
#define I2C_COM_SUCCESS true

safecall bool MyI2CRead(byte port, byte addr, byte reg, byte cnt, byte& outbuf[])             // NXT sensor port, I2C address, I2C register, byte count, buffer[]
{
  byte cmdbuf[];                                                                              // The buffer of data that will be sent
  byte nByteReady;                                                                            // required for getting the status

  ArrayBuild(cmdbuf, addr, reg);                                                              // Build the output buffer

  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING){Yield();}                         // Wait until bus is free

  if(I2CBytes(port, cmdbuf, cnt, outbuf)){                                                    // Write the register pointer, and read the registers
    return I2C_COM_SUCCESS;
  }
  return I2C_COM_ERROR;
}

safecall bool MyI2CWrite(byte port, byte addr, byte reg, byte data[])                         // NXT sensor port, I2C device address, I2C register, data[]
{
  byte cmdbuf[];                                                                              // The buffer of data that will be sent
  byte nByteReady;                                                                            // required for getting the status

  ArrayBuild(cmdbuf, addr, reg, data);                                                        // Build the output buffer

  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING){Yield();}                         // Wait until bus is free

  if(I2CWrite(port, 0, cmdbuf) == NO_ERR){                                                    // Send the message
    return I2C_COM_SUCCESS;
  }
  return I2C_COM_ERROR;
}

[HaWe]

I actually supposed "noob" to have taken the servo-get-position-information from a Mindsensor's servo manual, not any general NXC i2c guide, that's why I asked

[noob405]

Hey!
First no response and now 6!
Thanks a lot so far.

I did´t do anything fancy. Yes, doc-helmut, You´re right. I just got my idea to read out the values from the Mindsensor's servo
manual, not any general NXC i2c guide.

My code seems to work - somehow. As You can see in my test program, I´m setting the servo position to 50, wait a while,and
then I read out the register. Or let´s say, I think I´m reading out values..... And it gives me 50 back. Fine.
Same goes for the position 200: I get 200 back. Fine too..

What I don´t understand is, why I have to start my my function "ShowServo1Position(SensorPort, ServoAddr, 0x42);" (see library)
twice to get values back. If I just call it once, I allways get "0"s back.

Noob

[mattallen37]

The value you're reading is just the target position. The actual position could be significantly different.

I'm not sure why it requires two calls. You could try adding a wait between the write (set register pointer) and the read. Here is an example:

Code: Select all

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  http://mattallen37.wordpress.com/
*  Initial date: pre 2012
*  Last updated: Aug. 29, 2013
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  This is a library of functions to aid in reading from I2C slaves, and writing to them.
*/

#define I2C_COM_ERROR false
#define I2C_COM_SUCCESS true

safecall bool MyI2CRead2(byte port, byte addr, byte reg, byte cnt, byte & outbuf[])           // NXT sensor port, I2C address, I2C register, byte count, buffer[]
{
  byte cmdbuf[];                                                                              // The buffer of data that will be sent
  byte nByteReady;                                                                            // required for getting the status

  ArrayBuild(cmdbuf, addr, reg);                                                              // Build the output buffer.

  while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING){Yield();}                         // Wait until bus is free

  if(I2CWrite(port, 2, cmdbuf) == NO_ERR){                                                    // Write the register pointer
    while (I2CStatus(port, nByteReady) ==  STAT_COMM_PENDING){Yield();}                       // Wait until bus is free
    Wait(10);                                                                                 // Give the slave time to act
    if(I2CRead (port, cnt, outbuf) == NO_ERR){                                                // Read the registers
      return I2C_COM_SUCCESS;
    }
  }
  return I2C_COM_ERROR;
}

Note that the code has been modified since being tested.

[noob405]

Thanks Matt
Will try later and report results.

Cheers
noob