Go Down

### Topic: how to quickly calculate the x, y position? (Read 2828 times)previous topic - next topic

#### peterbay

##### Sep 24, 2009, 10:17 am
Hi, I work on my project a laser projector and I need to accelerate compute the position x, y.

Slowest is compute the separation step to obtain the shift in the x and y axis.
step_x = diff_x / steps_f;
step_y = diff_y / steps_f;

You do not know someone faster method of calculating the displacement between two points?

Peter B

ps. I use MCP4922 ( 12-Bit DAC with SPI Interface )

Here is my code:

Code: [Select]
`#define LDACPIN_LOW  ( PORTB &= B11111011 ) // pin 10#define LDACPIN_HIGH ( PORTB |= B00000100 )#define SDIPIN_LOW   ( PORTB &= B11110111 ) // pin 11#define SDIPIN_HIGH  ( PORTB |= B00001000 )#define SCKPIN_LOW   ( PORTB &= B11101111 ) // pin 12#define SCKPIN_HIGH  ( PORTB |= B00010000 )#define CSPIN_LOW    ( PORTB &= B11011111 ) // pin 13#define CSPIN_HIGH   ( PORTB |= B00100000 )#define CLOCK        SCKPIN_HIGH; SCKPIN_LOW;#define SET_HIGH     SDIPIN_HIGH; CLOCK;#define SET_LOW      SDIPIN_LOW;  CLOCK;int last_x       = 0;int last_y       = 0;int move_counter = 0;void setup(){    DDRB = DDRB | B11111111;    CSPIN_HIGH;  SCKPIN_LOW;  SDIPIN_LOW;  LDACPIN_HIGH;} void set_dac_2 ( int value_a, int value_b ){  // --- output A ---  CSPIN_LOW;  SDIPIN_HIGH; CLOCK; // 15  SDIPIN_LOW;  CLOCK; // 14  SDIPIN_HIGH; CLOCK; // 13  CLOCK;              // 12    SDIPIN_LOW;  if ( 2048 & value_a ){ SDIPIN_HIGH; } CLOCK; // 11  SDIPIN_LOW;  if ( 1024 & value_a ){ SDIPIN_HIGH; } CLOCK; // 10  SDIPIN_LOW;  if (  512 & value_a ){ SDIPIN_HIGH; } CLOCK; // 09  SDIPIN_LOW;  if (  256 & value_a ){ SDIPIN_HIGH; } CLOCK; // 08  SDIPIN_LOW;  if (  128 & value_a ){ SDIPIN_HIGH; } CLOCK; // 07  SDIPIN_LOW;  if (   64 & value_a ){ SDIPIN_HIGH; } CLOCK; // 06  SDIPIN_LOW;  if (   32 & value_a ){ SDIPIN_HIGH; } CLOCK; // 05  SDIPIN_LOW;  if (   16 & value_a ){ SDIPIN_HIGH; } CLOCK; // 04  SDIPIN_LOW;  if (    8 & value_a ){ SDIPIN_HIGH; } CLOCK; // 03  SDIPIN_LOW;  if (    4 & value_a ){ SDIPIN_HIGH; } CLOCK; // 02  SDIPIN_LOW;  if (    2 & value_a ){ SDIPIN_HIGH; } CLOCK; // 01  SDIPIN_LOW;  if (    1 & value_a ){ SDIPIN_HIGH; } CLOCK; // 00  CSPIN_HIGH;  // --- output B ---  CSPIN_LOW;  SDIPIN_LOW; CLOCK;  // 15  CLOCK;              // 14  SDIPIN_HIGH; CLOCK; // 13  CLOCK;              // 12  SDIPIN_LOW;  if ( 2048 & value_b ){ SDIPIN_HIGH; } CLOCK; // 11  SDIPIN_LOW;  if ( 1024 & value_b ){ SDIPIN_HIGH; } CLOCK; // 10  SDIPIN_LOW;  if (  512 & value_b ){ SDIPIN_HIGH; } CLOCK; // 09  SDIPIN_LOW;  if (  256 & value_b ){ SDIPIN_HIGH; } CLOCK; // 08  SDIPIN_LOW;  if (  128 & value_b ){ SDIPIN_HIGH; } CLOCK; // 07  SDIPIN_LOW;  if (   64 & value_b ){ SDIPIN_HIGH; } CLOCK; // 06  SDIPIN_LOW;  if (   32 & value_b ){ SDIPIN_HIGH; } CLOCK; // 05  SDIPIN_LOW;  if (   16 & value_b ){ SDIPIN_HIGH; } CLOCK; // 04  SDIPIN_LOW;  if (    8 & value_b ){ SDIPIN_HIGH; } CLOCK; // 03  SDIPIN_LOW;  if (    4 & value_b ){ SDIPIN_HIGH; } CLOCK; // 02  SDIPIN_LOW;  if (    2 & value_b ){ SDIPIN_HIGH; } CLOCK; // 01  SDIPIN_LOW;  if (    1 & value_b ){ SDIPIN_HIGH; } CLOCK; // 00  CSPIN_HIGH;  LDACPIN_LOW;  SCKPIN_HIGH;//  SCKPIN_HIGH;  LDACPIN_HIGH;  SCKPIN_LOW;}void jump ( int x, int y ){  last_x = x;  last_y = y;  set_dac_2 ( last_x, last_y );  }void move ( int x, int y ){  byte  max_step_size = 16;  word  diff_x     = 0;  word  diff_y     = 0;  word  steps      = 0;  float steps_f    = 0;  float step_x     = 0;  float step_y     = 0;  byte  directions = 0;  float val_x     = last_x;  float val_y     = last_y;  if  ( x > last_x ){    diff_x = x - last_x;  } else {    diff_x = last_x - x;    directions |= 1;  }  if ( y > last_y ){    diff_y = y - last_y;  } else {    diff_y = last_y - y;    directions |= 2;  }  if ( diff_x < diff_y ){    steps = ( diff_y / max_step_size ) + 1;  } else {    steps = ( diff_x / max_step_size ) + 1;      }    steps_f = steps;    step_x = diff_x / steps_f;  step_y = diff_y / steps_f;  while ( steps ){    if ( directions & 1 ){      val_x  -= step_x;    } else {      val_x  += step_x;    }    if ( directions & 2 ){      val_y  -= step_y;    } else {      val_y  += step_y;    }    set_dac_2 ( ( int ) val_x, ( int ) val_y );    move_counter++;    steps--;  }  last_x = x;  last_y = y;}void loop() {  //  int start  = millis();  move_counter = 0;  jump ( 2048, 1 );  move ( 2048, 1 );  move ( 2048, 2048 );  move ( 1, 2048 );  move ( 1, 1 );/*Serial.println ( millis() - start );Serial.println ( move_counter );*/} `

#### AWOL

#1
##### Sep 24, 2009, 10:25 amLast Edit: Sep 24, 2009, 10:47 am by AWOL Reason: 1
Rather than using floats, how about using 16.16 fixed point?
It may well be quicker.

The other thing is, since the value of "directions" doesn't change within the "while" loop, why not simply set the sign of "step_x" and "step_y" outside the loop, and then all you need to do is add within the loop, without the need for a conditional.
You could also simply add "steps" to "move_counter" before the loop to reduce the time spent in the loop.

Google "DDA" and "Bresenham" line drawing algorithms.
Code: [Select]
`void move ( int x, int y ){  const byte max_step_size = 16;  word  diff_x = x - last_x;  word  diff_y = y - last_y;  word  steps;  float step_x;  float step_y;    float val_x     = last_x;  float val_y     = last_y;  if ( abs(diff_x) < abs (diff_y) ){    steps = ( diff_y / max_step_size ) + 1;  } else {    steps = ( diff_x / max_step_size ) + 1;  }    step_x = diff_x / (float)steps;  step_y = diff_y / (float)steps;  move_counter += steps;  for (; steps; steps--){    val_x  += step_x;    val_y  += step_y;    set_dac_2 ( ( int ) val_x, ( int ) val_y );  }  last_x = x;  last_y = y;}`

#### peterbay

#2
##### Sep 24, 2009, 10:59 am
Thanks for your idea. Try it when I get home

#### AWOL

#3
##### Sep 24, 2009, 01:16 pmLast Edit: Sep 24, 2009, 01:20 pm by AWOL Reason: 1
Oops
Code: [Select]
`  if ( abs(diff_x) < abs (diff_y) ){    steps = ( abs(diff_y) / max_step_size ) + 1);  } else {    steps = ( abs(diff_x) / max_step_size ) + 1);  }`

#### terry3111

#4
##### Sep 28, 2009, 09:35 am
I too it cannot achieve.  :-/

#### peterbay

#5
##### Sep 28, 2009, 02:38 pm
Hi

here is the fast code without floats

Code: [Select]
`#define LDACPIN_LOW  ( PORTB &= B11111011 ) // pin 10#define LDACPIN_HIGH ( PORTB |= B00000100 )#define SDIPIN_LOW   ( PORTB &= B11110111 ) // pin 11#define SDIPIN_HIGH  ( PORTB |= B00001000 )#define SCKPIN_LOW   ( PORTB &= B11101111 ) // pin 12#define SCKPIN_HIGH  ( PORTB |= B00010000 )#define CSPIN_LOW    ( PORTB &= B11011111 ) // pin 13#define CSPIN_HIGH   ( PORTB |= B00100000 )#define CLOCK        SCKPIN_HIGH; SCKPIN_LOW;#define SET_HIGH     SDIPIN_HIGH; CLOCK;#define SET_LOW      SDIPIN_LOW;  CLOCK;long last_x       = 0;long last_y       = 0;long move_counter = 0;void setup(){    DDRB = DDRB | B11111111;    CSPIN_HIGH;  SCKPIN_LOW;  SDIPIN_LOW;  LDACPIN_HIGH;    Serial.begin( 9600 );} void set_dac_2 ( int value_a, int value_b ){  // --- output A ---  CSPIN_LOW;  SDIPIN_HIGH; CLOCK; // 15  SDIPIN_LOW;  CLOCK; // 14  SDIPIN_HIGH; CLOCK; // 13  CLOCK;              // 12    SDIPIN_LOW;  if ( 2048 & value_a ){ SDIPIN_HIGH; } CLOCK; // 11  SDIPIN_LOW;  if ( 1024 & value_a ){ SDIPIN_HIGH; } CLOCK; // 10  SDIPIN_LOW;  if (  512 & value_a ){ SDIPIN_HIGH; } CLOCK; // 09  SDIPIN_LOW;  if (  256 & value_a ){ SDIPIN_HIGH; } CLOCK; // 08  SDIPIN_LOW;  if (  128 & value_a ){ SDIPIN_HIGH; } CLOCK; // 07  SDIPIN_LOW;  if (   64 & value_a ){ SDIPIN_HIGH; } CLOCK; // 06  SDIPIN_LOW;  if (   32 & value_a ){ SDIPIN_HIGH; } CLOCK; // 05  SDIPIN_LOW;  if (   16 & value_a ){ SDIPIN_HIGH; } CLOCK; // 04  SDIPIN_LOW;  if (    8 & value_a ){ SDIPIN_HIGH; } CLOCK; // 03  SDIPIN_LOW;  if (    4 & value_a ){ SDIPIN_HIGH; } CLOCK; // 02  SDIPIN_LOW;  if (    2 & value_a ){ SDIPIN_HIGH; } CLOCK; // 01  SDIPIN_LOW;  if (    1 & value_a ){ SDIPIN_HIGH; } CLOCK; // 00  CSPIN_HIGH;  // --- output B ---  CSPIN_LOW;  SDIPIN_LOW; CLOCK;  // 15  CLOCK;              // 14  SDIPIN_HIGH; CLOCK; // 13  CLOCK;              // 12  SDIPIN_LOW;  if ( 2048 & value_b ){ SDIPIN_HIGH; } CLOCK; // 11  SDIPIN_LOW;  if ( 1024 & value_b ){ SDIPIN_HIGH; } CLOCK; // 10  SDIPIN_LOW;  if (  512 & value_b ){ SDIPIN_HIGH; } CLOCK; // 09  SDIPIN_LOW;  if (  256 & value_b ){ SDIPIN_HIGH; } CLOCK; // 08  SDIPIN_LOW;  if (  128 & value_b ){ SDIPIN_HIGH; } CLOCK; // 07  SDIPIN_LOW;  if (   64 & value_b ){ SDIPIN_HIGH; } CLOCK; // 06  SDIPIN_LOW;  if (   32 & value_b ){ SDIPIN_HIGH; } CLOCK; // 05  SDIPIN_LOW;  if (   16 & value_b ){ SDIPIN_HIGH; } CLOCK; // 04  SDIPIN_LOW;  if (    8 & value_b ){ SDIPIN_HIGH; } CLOCK; // 03  SDIPIN_LOW;  if (    4 & value_b ){ SDIPIN_HIGH; } CLOCK; // 02  SDIPIN_LOW;  if (    2 & value_b ){ SDIPIN_HIGH; } CLOCK; // 01  SDIPIN_LOW;  if (    1 & value_b ){ SDIPIN_HIGH; } CLOCK; // 00  CSPIN_HIGH;  LDACPIN_LOW;  SCKPIN_HIGH;//  SCKPIN_HIGH;  LDACPIN_HIGH;  SCKPIN_LOW;}void jump ( int x, int y ){  last_x = x;  last_y = y;  set_dac_2 ( last_x, last_y );  }void move ( int x, int y ){  const byte max_step_size = 4;  byte div = 0;  int  steps;    long diff_x = x - last_x;  long diff_y = y - last_y;  long val_x  = last_x << 6;  long val_y  = last_y << 6;  if ( abs ( diff_x ) < abs ( diff_y ) ){    steps = ( abs ( diff_y ) >> max_step_size ) + 1;  } else {    steps = ( abs ( diff_x ) >> max_step_size ) + 1;  }  if        ( steps & 0xfe00 ){ div = 8; steps = 256;  } else if ( steps & 0xff00 ){ div = 7; steps = 128;  } else if ( steps & 0xff80 ){ div = 6; steps = 64;  } else if ( steps & 0xffc0 ){ div = 5; steps = 32;  } else if ( steps & 0xffe0 ){ div = 4; steps = 16;  } else if ( steps & 0xfff0 ){ div = 3; steps = 8;  } else if ( steps & 0xfff8 ){ div = 2; steps = 4;  } else if ( steps & 0xfffc ){ div = 1; steps = 2;  } else { steps = 1; }      long step_x = ( diff_x << 6 ) >> div;  long step_y = ( diff_y << 6 ) >> div;  move_counter += steps;  for (; steps; steps--){    val_x += step_x;    val_y += step_y;    set_dac_2 ( val_x >> 6, val_y >> 6 );//    delayMicroseconds ( 20 );  }    set_dac_2 ( x, y );  last_x = x;  last_y = y;//  delayMicroseconds ( 500 );}void loop() {  //  int start  = millis();  move_counter = 0;  jump ( 2048, 1 );  move ( 2048, 1 );  move ( 2048, 2048 );  move ( 1, 2048 );  move ( 1, 1 );/*Serial.println ( millis() - start );Serial.println ( move_counter );*/}`

Peter

#### Grumpy_Mike

#6
##### Sep 28, 2009, 03:01 pm
I may be missing something her but the code:-
Code: [Select]
`SDIPIN_LOW;  if ( 2048 & value_a ){ SDIPIN_HIGH; } CLOCK; // 11  SDIPIN_LOW;  if ( 1024 & value_a ){ SDIPIN_HIGH; } CLOCK; // 10  SDIPIN_LOW;  if (  512 & value_a ){ SDIPIN_HIGH; } CLOCK; // 09  SDIPIN_LOW;  if (  256 & value_a ){ SDIPIN_HIGH; } CLOCK; // 08  SDIPIN_LOW;  if (  128 & value_a ){ SDIPIN_HIGH; } CLOCK; // 07  SDIPIN_LOW;  if (   64 & value_a ){ SDIPIN_HIGH; } CLOCK; // 06  SDIPIN_LOW;  if (   32 & value_a ){ SDIPIN_HIGH; } CLOCK; // 05  SDIPIN_LOW;  if (   16 & value_a ){ SDIPIN_HIGH; } CLOCK; // 04  SDIPIN_LOW;  if (    8 & value_a ){ SDIPIN_HIGH; } CLOCK; // 03  SDIPIN_LOW;  if (    4 & value_a ){ SDIPIN_HIGH; } CLOCK; // 02  SDIPIN_LOW;  if (    2 & value_a ){ SDIPIN_HIGH; } CLOCK; // 01  SDIPIN_LOW;  if (    1 & value_a ){ SDIPIN_HIGH; } CLOCK; // 00`

Just boils down into:-
Code: [Select]
`SDIPIN_LOW;  if ( value_a  & 0xFFF){ SDIPIN_HIGH; } CLOCK; // 00`

#### peterbay

#7
##### Sep 28, 2009, 03:23 pm
My code is ok.

I check 12 bits in value, setting output and sending clock tick for EACH bit.

2048 = 1000 0000 0000 (binary) and it means bit 11 in value
1024 = 0100 0000 0000 - bit 10
512 = 0010 0000 0000 - bit 9
256 = 0001 0000 0000 - bit 8
128 = 0000 1000 0000 - bit 7
64 = 0000 0100 0000 - bit 6
32 = 0000 0010 0000 - bit 5
16 = 0000 0001 0000 - bit 4
8 = 0000 0000 1000 - bit 3
4 = 0000 0000 0100 - bit 2
2 = 0000 0000 0010 - bit 1
1 = 0000 0000 0001 - bit 0

Your code only check if some of 12 bits is set to 1 and not sending 12 bits to output.

#### AWOL

#8
##### Sep 28, 2009, 03:44 pmLast Edit: Sep 28, 2009, 03:45 pm by AWOL Reason: 1
@Mike,
the formatting is unusual, but think of it as:
Code: [Select]
`SDIPIN_LOW;  if ( 2048 & value_a ) {   SDIPIN_HIGH; } CLOCK; // 11SDIPIN_LOW;  if ( 1024 & value_a ) {  SDIPIN_HIGH; } CLOCK; // 10SDIPIN_LOW;  if (  512 & value_a ) {  SDIPIN_HIGH; } CLOCK; // 09...`
just a hand-optimised loop, sending serial data.

#### Grumpy_Mike

#9
##### Sep 28, 2009, 03:51 pm
Yes sorry the formatting did have me fooled, I didn't spot CLOCK was outside the if scope.

Go Up