View unanswered posts | View active topics It is currently Wed Jul 30, 2014 1:00 pm






Reply to topic  [ 17 posts ]  Go to page 1, 2  Next
HT Angle reading take 9ms ! 
Author Message
Rookie
User avatar

Joined: Wed Aug 25, 2010 3:59 am
Posts: 21
Location: France
Post HT Angle reading take 9ms !
Hi,

I just test these sensors for odometry with PID control.
I see that reading one sensor take 9ms with "HTANGreadAccumulatedAngle".
I read 2 sensors (left and right), so it take 18ms in my pid loop --> big fail.

I suppose I can boost time reading using two separate task for the sensors but 9ms is so long for a pid.

Someone have an idea ?

My actual code :
Code:
  ClearTimer(T1);

   do
   {
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr);

     encLeft = HTANGreadAccumulatedAngle(angleLeft) * angleLeftRevert;
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr); //9ms reading
     encRight = HTANGreadAccumulatedAngle(angleRight) * angleRightRevert;
         StringFormat(TempStr, "temp : %04d \n",time1[T1]); writeDebugStream(TempStr); //18ms reading
     //wait1Msec(1);
    if(rotation)
        error = encLeft+encRight;        // calculate the error by subtracting the offset
    else
        error = encLeft-encRight;        // calculate the error by adding the offset if robot turn

    StringFormat(TempStr2, "L %d R %d", encLeft, encRight);
    writeDebugStreamLine(TempStr2);
    integral = integral + error;        // calculate the integral
    integral=limite(-100,100,integral); // function limit value for integral between -100 and 100
    derivative = error - lastError;    // calculate the derivative
    Turn = (int)(Kp*(float)error + Ki*(float)integral + Kd*(float)derivative);  // the "P term" the "I term" and the "D term"
    powerRight = Tp + (int)Turn;                // the power level for the left motor
    powerLeft = Tp - (int)Turn;                // the power level for the right motor
....
   

_________________
Blogotechnic - Fribot 2009


Tue Mar 01, 2011 5:10 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
9 ms is what it takes to do the request (2 bytes) and get a response (4 bytes). Not a whole lot I can do about it, it's just how long it takes. Doing them in parallel wouldn't work because ROBOTC does not support re-entrant functions, so you can't call functions from two tasks simultaneously. Configuring the sensor as sensorI2CCustomFastSkipStates isn't supported but does reduce it to about 3ms. If it works for you, great, but don't complain if you get more errors.

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Mar 01, 2011 5:35 pm
Profile WWW
Rookie
User avatar

Joined: Wed Aug 25, 2010 3:59 am
Posts: 21
Location: France
Post Re: HT Angle reading take 9ms !
Thanks for these informations.

_________________
Blogotechnic - Fribot 2009


Tue Mar 01, 2011 5:37 pm
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
Hi Alban and Xander

I've done some little test to see where HTANGreadAngle function take to many time. The send and receive I2C message take less than 1ms. But the waitforI2C function take 7ms after the sendI2CMessage. I think it's beacause the sensor has not read the command. Is it possible that the slowness is due of the sensor reading command?
(sorry for my english :p)

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com


Thu Mar 10, 2011 5:40 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
Can you give me your test program so I can see what's going on?

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Fri Mar 11, 2011 2:09 am
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
hi,

I had done the following change in the readI2C method to see were the slowness is.

Code:
bool writeI2C(tSensors link, tByteArray &data, int replylen) {

#if __COMMON_H_SENSOR_CHECK__ == 1
  TSensorTypes type = SensorType[link];
  if ((type != sensorI2CCustom) &&
      (type != sensorI2CCustom9V) &&
      (type != sensorI2CCustomFast) &&
      (type != sensorI2CCustomFast9V) &&
      (type != sensorI2CCustomFastSkipStates9V) &&
      (type != sensorI2CCustomFastSkipStates)) {
    hogCPU();
    PlaySound(soundException);
    eraseDisplay();
    nxtDisplayCenteredTextLine(0, "3rd Party Driver");
    nxtDisplayCenteredTextLine(1, "ERROR");
    nxtDisplayCenteredTextLine(2, "You have not");
    nxtDisplayCenteredTextLine(3, "setup the sensor");
    nxtDisplayCenteredTextLine(4, "port correctly. ");
    nxtDisplayCenteredTextLine(5, "Please refer to");
    nxtDisplayCenteredTextLine(6, "one of the");
    nxtDisplayCenteredTextLine(7, "examples.");
    wait1Msec(10000);
    StopAllTasks();
  }
#endif
writeDebugStreamLine("write");
ClearTimer(T2);
  if (!waitForI2CBus(link)) {
    clearI2CError(link, data.arr[1]);

    // Let's try the bus again, see if the above packets flushed it out
    // clearI2CBus(link);
    if (!waitForI2CBus(link))
      return false;
  }
writeDebugStreamLine("%d",time1[T2]);

  sendI2CMsg(link, data.arr[0], replylen);
writeDebugStreamLine("%d",time1[T2]);

  if (!waitForI2CBus(link)) {
    writeDebugStreamLine("error send");
    clearI2CError(link, data.arr[1]);
    sendI2CMsg(link, data.arr[0], replylen);
    if (!waitForI2CBus(link))
      return false;
  }
  writeDebugStreamLine("%d",time1[T2]);
  writeDebugStreamLine("end write");
  return true;
}


and it appears that, that the thing that take the most time is not the send I2CMsg but the waitForI2CBus.

If we use the HTAngreadangle with HTMux (first read 12ms , second read and others 7ms) or directly we have the same response time.
I think that it's not a question of response packet size from the HTAng. The HTAng is to long to read the command and respond to it.

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com


Mon Mar 21, 2011 8:05 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
I will take a look at it and see if there's something I can do to make this more robust without taking so much time.

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Mar 22, 2011 2:01 am
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
Thanks xander :)

We need to access to 2 SMUX with a HTAngle on each, do you think that if we modify your driver to

-sendI2CMsg to SMUX 1
-sendI2CMsg to SMUX 2
-wait until I2CPort 1 AND I2Port 2 is ready

could improve the total response time?

I will test this solution to night

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com


Tue Mar 22, 2011 4:54 am
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
I am not sure I will go that far but I will take a look at the waitForI2CBus() stuff.

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Mar 22, 2011 5:07 am
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
Thibaud,

Can you please send me the complete program you wrote to perform these test with the single HTAC and SMUX?

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Tue Mar 22, 2011 5:50 am
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
No problem Xander, i will send you it at the beginning of the evening . I haven't the program with me yet.

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com


Tue Mar 22, 2011 5:54 am
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
Here is the result of the code in attachment
Port 1 HTANG
Port 2 SMUX + HTANG
Code:
[color=#00BF00]--temp : 0000
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
write
T2 : 0000
T2 : 0000
T2 :  0006
end write
T4 : 0000
T4 : 0000
--temp : 0632 [/color]
write
T2 : 0000
T2 : 0000
T2 :  0004
end write
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0012
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0008
write
T2 : 0000
T2 : 0001
T2 :  0007
end write
T3 : 0008
T4 : 0000
T4 : 0000
T3 : 0000
--temp : 0008
write
T2 : 0001
T2 : 0000
T2 :  0007
end write
T3 : 0008
T4 : 0000
T4 : 0000
T3 : 0000
--temp : 0008
write
T2 : 0000
T2 : 0000
T2 :  0007
end write
T4 : 0000
T4 : 0000
--temp : 0008
[color=#FF0000]value : 0308,0308
0012,0012,0240[/color]

in green it's the SMUX initialisation
in red the read results, with a strange result , the last read of the HTangSensor on the mux is not the same.


Attachments:
common_test.h [29.15 KiB]
Downloaded 395 times
HTANG-driver_test.h [7.8 KiB]
Downloaded 361 times
TestHTAngle.c [1.3 KiB]
Downloaded 359 times

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com
Tue Mar 22, 2011 6:05 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
I am afraid there's not a whole lot I can do about the 8ms I2C request time. I've done many tests in the past and it just takes that much time to do a request for a 4 byte read and get the response.

You can't do multiple requests at once due to the nature of ROBOTC; it is not possible to call the same function twice from two different tasks, so there is no way to parallelise them.

Here's a program to show you that the delay is not because of my driver suite code:

Code:
#pragma config(Sensor, S1,     ANGLE,               sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

ubyte xmit_msg[] = {2, 0x02, 0x42};
ubyte recv_msg[4];

task main () {
  long time = 0;
  time1[T1] = 0;
  for (int i = 0; i < 100; i++) {
    while(nI2CStatus(ANGLE) != NO_ERR) EndTimeSlice();
    sendI2CMsg(ANGLE, xmit_msg[0], 4);
    while(nI2CStatus(ANGLE) != NO_ERR) EndTimeSlice();
    readI2CReply(ANGLE, recv_msg[0], 4);
  }
  time = time1[T1];
  writeDebugStreamLine("Each call: %d", time/100);
  writeDebugStreamLine("Total time: %d", time);
}


The reason the readI2CReply() does not take any time to complete is because the actual I2C read is already done by the sendI2CMsg call. If you hook this up to a protocol analyser (which I happened to have) and remove the readI2CReply(ANGLE, recv_msg[0], 4); line, the I2C read is still done. This is why you need to specify the size of the I2C response of the slave when doing a sendI2CMsg(). All the readI2CReply() call does is copy an internal response buffer to the specified array.

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Thu Mar 24, 2011 6:16 am
Profile WWW
Novice

Joined: Sun Feb 24, 2008 6:15 pm
Posts: 50
Post Re: HT Angle reading take 9ms !
Ok, thanks for your answer

I don't understand why the sendI2Cmessage take no time if it does the readI2Cmessage? when i use your dirver suite i can see that the thing which take time is the wait for the I2C port availability.
is the sendI2Cmessage use an internal task?

Do you think that if i use 2 HTAngle on to separate port, could i do the following :

sendI2CMsg to port 1
sendI2CMsg to port 2
check I2C status on the two ports
read Value on port 1
read Value on port 2

_________________
My Eurobot team website :
http://blog.mlrobotic.com/


My MOCs :
http://nxtgen.6mablog.com


Thu Mar 24, 2011 7:02 am
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: HT Angle reading take 9ms !
sendI2Cmessage is an internal function.

This program seems to take the same amount of time:
Code:
#pragma config(Sensor, S1,     ANGLE1,              sensorI2CCustom)
#pragma config(Sensor, S2,     ANGLE2,              sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

ubyte xmit_msg[] = {2, 0x02, 0x42};
ubyte recv_msg[4];

task main () {
  long time = 0;
  time1[T1] = 0;
  for (int i = 0; i < 100; i++) {
    while((nI2CStatus(ANGLE1) != NO_ERR) && (nI2CStatus(ANGLE2) != NO_ERR)) EndTimeSlice();
    sendI2CMsg(ANGLE1, xmit_msg[0], 4);
    sendI2CMsg(ANGLE2, xmit_msg[0], 4);
    while((nI2CStatus(ANGLE1) != NO_ERR) && (nI2CStatus(ANGLE2) != NO_ERR)) EndTimeSlice();
    readI2CReply(ANGLE1, recv_msg[0], 4);
    readI2CReply(ANGLE2, recv_msg[0], 4);
  }
  time = time1[T1];
  writeDebugStreamLine("Each call: %d", time/100);
  writeDebugStreamLine("Total time: %d", time);
}


as the previous one. So you could run two I2C requests simultaneously if they are on two different sensor ports. Unfortunately, making that work transparently with my driver suite would require a complete rewrite using tasks and dispatching functions. If I do end up making that, it won't be anytime between now and next month :)

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Thu Mar 24, 2011 7:16 am
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 17 posts ]  Go to page 1, 2  Next

Who is online

Users browsing this forum: No registered users and 1 guest


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 post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.