More angle wrapping related misery in code

[IanB]

But the sign of the output of the subtraction must be such that it will, including when subtracting across the wrap point, give a clear indication of whether Angle1 or Angle2 is "bigger". Bigger not meaning here an actual size, but rather working n the way that for a 0 to 360 circle you might say 30 is "bigger" than 350 because you go upward from 350 to reach 30 by the shorter route.

Since you are dealing with a circle, you need to use words like "clockwise" and "counterclockwise" instead of "bigger" and "smaller".

Let's suppose angle 1 is 30 degrees, and angle 2 is 350 degrees. You want to know the shortest path from angle 1 to angle 2. This would be 40 degrees counterclockwise, or −40 degrees. The way you do this, is you compute (angle2 − angle1), or (350 − 30), which is 320. Now, if this result is greater than 180, you subtract 360 from it. 320 − 360 = −40, which is the expected result.

The converse might be if angle 1 is 350 degrees, and angle 2 is 30 degrees. In this case, the shortest path from angle 1 to angle 2 is 40 degrees clockwise, or +40 degrees. Once more, you compute (angle2 − angle1), or (30 − 350), which is −320. Now, if this result is less than −180, you add 360 to it. −320 + 360 = +40, which is the expected result.

On the other hand, if the result of the subtraction lies between −180 and +180, then you just use the result as given, and no need to add or subtract 360.

[Siwastaja]

Let's suppose angle 1 is 30 degrees, and angle 2 is 350 degrees. You want to know the shortest path from angle 1 to angle 2. This would be 40 degrees counter-clockwise, or −40 degrees. The way you do this, is you compute (angle2 − angle1), or (350 − 30), which is 320. Now, if this result is greater than 180, you subtract 360 from it. 320 − 360 = −40, which is the expected result.

... or, you just interpret the bits as signed integer, and get the -180..+180 deg range automagically, without any if statements. This is what binary angles offer you, choice of 0..360 vs. -180..180 depending on if you cast to signed type or not.

If you use a type with "full rounds" part, taking a difference with the full data width directly gives you the shortest distance, and direction thereof.

wrap at an arbitrary value  .... wrap point

What even is this wrap point, and why do you think you need it?

If, for some weird reason, you need to represent angles to the user e.g. between 5...364 degrees instead of 0...359 or -180..+179 (which would be the usual representations), then just add an offset as a very first (in parsing) or last (in displaying) step. Internally, use the sensible format, that is, binary angle representations without any funny "arbitrary wrap points".

[IanB]

Let's suppose angle 1 is 30 degrees, and angle 2 is 350 degrees. You want to know the shortest path from angle 1 to angle 2. This would be 40 degrees counter-clockwise, or −40 degrees. The way you do this, is you compute (angle2 − angle1), or (350 − 30), which is 320. Now, if this result is greater than 180, you subtract 360 from it. 320 − 360 = −40, which is the expected result.

... or, you just interpret the bits as signed integer, and get the -180..+180 deg range automagically, without any if statements. This is what binary angles offer you, choice of 0..360 vs. -180..180 depending on if you cast to signed type or not.

This is a very helpful and useful result, but we also need to consider the human understanding part. The way I described it with decimal integers, someone can read it and follow along. Which means later on if the code is implemented this way, then someone reading the code will also be able to follow it. The additional cost of tests and adding or subtracting 360 will likely be insignificant, computationally.

On the other hand, if the work is done with binary arithmetic, then the result occurs as if by magic, and will need extensive comments in the code to explain to someone unfamiliar with such calculations.

I found it helpful to look at an example to see this this binary angle approach works.

Let's work with 8 bits, so that angles are measured clockwise from 0 to 255 (0 being 12 o'clock, and 255 being 11:59).

Let angle one be 9, and let angle two be 252. The shortest distance from 9 to 252 is 13 steps clockwise, while the shortest distance from 252 to 9 is 13 steps counterclockwise, or −13.

So, in the first case, we compute 252 − 9 = 243. Now we treat 243 as a signed integer, and it becomes −13, which is the desired result.

In the second case, we compute 9 − 252 = 13 due to wraparound. As a signed integer this is directly the expected result.

This could well be described as "cunning logic", but it could also be described as "obfuscated code". Care is needed.

[Nominal Animal]

But the sign of the output of the subtraction must be such that it will, including when subtracting across the wrap point, give a clear indication of whether Angle1 or Angle2 is "bigger". Bigger not meaning here an actual size, but rather working n the way that for a 0 to 360 circle you might say 30 is "bigger" than 350 because you go upward from 350 to reach 30 by the shorter route.

IanB suggested "clockwise" and "counterclockwise", but I prefer "positive" and "negative".

AngleTo - AngleFrom should be positive if you need fewer iterations of incrementing AngleFrom than decrementing to get to Angle To.
AngleTo - AngleFrom should be negative if you need fewer iterations of decrementing AngleFrom than incrementing to get to Angle To.
AngleTo - AngleFrom should be zero if the two are equal.

The same logic is used in e.g. C qsort(): compar(a, b) needs to return negative if a<b, 0 if a==b, and positive if a>b.

I've got this to work easily where binary numbers are used as the limit, because subtracting them works across the limit of, for example, a uint8_t's wrap point, then going back to zero. But I'm a bit more confused when using numbers with an arbitrary wrap point, and where that wrap point goes between +MaxRevs and -MaxRevs (or +Maxrevs and -Maxrevs+1 I think more accurately, so that -MaxRevs and +MaxRevs are not duplicate values for the same point*).

Standard representable range of values is -MaxRevs .. +MaxRevs-1, i.e. including -MaxRevs but excluding +MaxRevs, based on two's complement integer range.

Let RevsRange = 2*MaxRevs (== (MaxRevs) - (-MaxRevs)), and must be representable in the type you use for revs.  Then:

    while (revs >= MaxRevs) revs -= RevsRange;
    while (revs < -MaxRevs) revs += RevsRange;

For an arbitrary value revs, that might be many times MaxRevs, you can use
    revs -= RevsRange * (revs / RevsRange);
    if (revs >= MaxRevs) revs -= RevsRange;
    if (revs < -MaxRevs) revs += RevsRange;
or, equivalently, giving the compiler more opportunities for optimization,
    revs %= RevsRange; // Result is between -RevsRange+1 and RevsRange-1, inclusive
    if (revs >= MaxRevs) revs -= RevsRange;
    if (revs < -MaxRevs) revs += RevsRange;

On architectures where conditionals are slow, multiplication/modulo is fast, but the compiler cannot eliminate them for above, you can use
    revs = (revs + MaxRevs) % RevsRange;
    revs += (revs < 0) ? MaxRevs : -MaxRevs;
which the compiler should know how to optimize.  After the first line, revs can be -RevsRange+1..RevsRange-1, but we will need to substract MaxRevs to compensate for the initial addition.  On the second line, if revs < 0, we need to add RevsRange - MaxRevs = MaxRevs; if revs >= 0, we only need to subtract MaxRev.

For arbitrary NegRevs..PosRevs-1 range, NegRevs < 0, PosRevs > 0, RevsRange = PosRevs - NegRevs, you can use
    revs = (revs - NegRevs) % RevsRange;
    revs += (revs < 0) ? -NegRevs : NegRevs;

Code: [Select]

while(count > MaxRevs){
  count=count-2*MaxRevs;
}
while(count <= -MaxRevs){
  count=count+2*MaxRevs;
}


Yes, that will limit count to range -MaxRevs+1 .. MaxRevs.  The limit with the "or equal" check will be excluded.  If both have the "or equal" check, then the later one dominates.

I do recommend you use -MaxRevs..MaxRevs-1, instead, so your limits match the "standard" ones, e.g. two's complement behaviour (and thus intN_t types' wrapping rules).

[Infraviolet]

I worked out some solutions which seem to work. For the benefit of others facing these sort of problems, I'll share them here at some point, once I've made the comments in my code tidier and such.

[Infraviolet]

These are the type dfinition and functions I've found to work. The last few functions just work with angles contained as a single byte, other functions use the AngleType stored angles. The use of an int16_t for the number of revolutions before wrapping ensures that when combining with the angle within a given revolution (as done inside some functions) to make an int32_t, we never get a value there so big as to overflow the int32_t. During wrapping this code goes from +MaxRevs to -MaxRevs+1, or from -MaxRevs+1 to +MaxRevs.

Code: [Select]


const int16_t MaxRevs=25200;//maximum number of electrical revolutions before we wrap, a multiple of 252 chosen here as the gearbox gives 1 mechanical roation at the output for 252 electric rotations within the motor
//so long as we never give a single move command for a very large angle change, then the motor will never move the wrong way to reach it

struct AngleType
{
  int16_t m_revolutionsPart; //number of electrical revolutions completed, relative to an arbitrary zero
  uint16_t m_anglePart; //stored with bottom 4 bits being for incrementing, next 8 bits being the angle out of 256, uppermost 4 bits unused

  //default constructor initialises it at zero
  AngleType() : m_revolutionsPart(0), m_anglePart(0) {}

  void AddToAngle(int16_t BinaryDelta){
    int32_t IntAngle=m_anglePart;
    IntAngle=(int32_t)IntAngle+BinaryDelta;
    while(IntAngle>=4096L){//(256<<4)){
      IntAngle=IntAngle - 4096L;//(256<<4);
      m_revolutionsPart=m_revolutionsPart+1L;
    }
    while(IntAngle<0L){
      IntAngle=IntAngle + 4096L;//(256<<4);
      m_revolutionsPart=m_revolutionsPart-1L;
    }

    while(m_revolutionsPart > (int32_t)MaxRevs){
      m_revolutionsPart=m_revolutionsPart-2L*MaxRevs;
    }
    while(m_revolutionsPart <= (int32_t)(0L-MaxRevs) ){
      m_revolutionsPart=m_revolutionsPart+2L*MaxRevs;
    }
    m_anglePart=(uint16_t) ( (int32_t)IntAngle & (int32_t)0xfff );//or m_anglePart=(uint16_t) ( (int32_t)IntAngle)); //???
  }

  void AddRevs(int16_t RevsDelta){
    m_revolutionsPart=m_revolutionsPart+RevsDelta;
    while(m_revolutionsPart > MaxRevs){
      m_revolutionsPart=m_revolutionsPart-2*MaxRevs;
    }
    while(m_revolutionsPart <= (0-MaxRevs) ){
      m_revolutionsPart=m_revolutionsPart+2*MaxRevs;
    }
  }

  void SubtractFromAngle(int16_t BinaryDelta){
    AddToAngle(-BinaryDelta);
  }
  void SubtractRevs(int16_t RevsDelta){
    AddRevs(-RevsDelta);
  }

  void UpdateAnglePart(uint16_t BinaryNewAnglePart){//give a new absolute value for the angle part, and this will automatically add to or decrement the revs, for situations where you measure the angle itself (at fast enough intervals not to have the motor having rotated >360 elec degrees between measurements) from an absolute "encoder" rather than constantly monitoring an incremental "encoder" type system
      uint16_t m_anglePartOld=m_anglePart  & 0x0fff;
      m_anglePart=BinaryNewAnglePart & 0x0fff;
     
      if(m_anglePart < (uint16_t)85<<4 && m_anglePartOld > (uint16_t)171<<4){
        m_revolutionsPart=m_revolutionsPart+1;
      }else if(m_anglePartOld < (uint16_t)85<<4 && m_anglePart > (uint16_t)171<<4){
        m_revolutionsPart=m_revolutionsPart-1;
      }

      while(m_revolutionsPart > MaxRevs){
        m_revolutionsPart=m_revolutionsPart-2*MaxRevs;
      }
      while(m_revolutionsPart <= (0-MaxRevs) ){
        m_revolutionsPart=m_revolutionsPart+2*MaxRevs;
      }
  }
 
};

int16_t MultiTurnDiffOfAngles(AngleType Angle1, AngleType Angle2){//returns a difference, or a very large or very small number if multiple revs apart
  //output of this is in actual 256ths, the 4 bits of incrementers have been abandoned
 
  int32_t Angle1Big=(int32_t)((Angle1.m_anglePart>>4) & 0xff)+(int32_t)Angle1.m_revolutionsPart * (int32_t)256L;
  int32_t Angle2Big=(int32_t)((Angle2.m_anglePart>>4) & 0xff)+(int32_t)Angle2.m_revolutionsPart * (int32_t)256L;

  //because m_revolutionsPart is between 32767 and -32768 , AngleXBig will never get near the limits of an int32_t

  int32_t DiffBig=(int32_t)Angle1Big-(int32_t)Angle2Big;
  if(DiffBig > -(MaxRevs*256L) && DiffBig <=(MaxRevs*256L)){//within 1*MaxRevs of each other, 256s because DiffBig is in 256ths rather than in revs itself
    //return (int16_t)DiffBig;
  }else{
    while(DiffBig > (MaxRevs*256L)){
      DiffBig=DiffBig-2L*(MaxRevs*256L);
    }
    while(DiffBig <= (0L-(MaxRevs*256L)) ){
      DiffBig=DiffBig+2L*(MaxRevs*256L);
    }   
    //return (int16_t)DiffBig;
  }

  //now we clamp the value of DiffBig to +/-256, then return it as an int16_t
  if(DiffBig>256L){
    DiffBig=256;
  }

  if(DiffBig< -256L){
    DiffBig=-256;
  }

  int16_t DiffSmall=DiffBig;
  return DiffSmall;
}

void printAngleType(AngleType AngleToPrint){
  Serial.print(F(" Revs: "));
  Serial.print(AngleToPrint.m_revolutionsPart);
  Serial.print(F(" Angle: "));
  Serial.print((AngleToPrint.m_anglePart>>4));
  Serial.print(F("/256 plus "));
  Serial.print(AngleToPrint.m_anglePart & 0xf);
  Serial.print(" increments ");
}


int8_t AngleDiff(uint8_t In2, uint8_t In1){//expects angle in 256ths, with the 4 bits of incrementing already removed
  int8_t Diff= ( ( In1 < In2 ) ? (int8_t)(In2 - In1) : -(int8_t)(In1-In2));
  return Diff; //returns in 256ths, no 4 bits of incrementing are present
}

const uint8_t Sine_table256[] PROGMEM = {generate as needed};


uint8_t FasterSine(int32_t AngleIn){//expects angle in 256ths, with the 4 bits of incrementing already removed
  uint8_t AngleInSmall=AngleIn & 0xff;
  uint8_t ValueOut=pgm_read_byte_near(Sine_table256 + AngleInSmall);
 
  return ValueOut;
}

uint8_t AngleAverage(uint8_t AngleIn1, uint8_t AngleIn2){//works, but results may be jumpy when the two input anles are "180 degrees", or nearly 180, apart (128/256)
  int8_t Difference = AngleDiff(AngleIn1,AngleIn2);
  uint8_t Out=0;
  int8_t HalfDiff=abs(Difference >> 1);
  if(Difference>0){
    Out=AngleIn2 + HalfDiff; //to the smaller angle add half the difference
  }else{
    Out=AngleIn1 + HalfDiff; //to the smaller angle add half the difference
  }
  return (uint8_t) (Out & 0xff);
}


bool CheckBetween(uint8_t testingVal, uint8_t LowerLimit, uint8_t UpperLimit){//will not work for LowerLimit==UpperLimit
  if(LowerLimit == UpperLimit){
    return 0;
  }
 
  if(AngleDiff(UpperLimit,LowerLimit) > 0 ){//LowerLimit < UpperLimit){
    if(AngleDiff(testingVal,LowerLimit) >=0 && AngleDiff(UpperLimit,testingVal)>=0){
      return 1;
    }else{
      return 0;
    }
  }else{
    if(AngleDiff(testingVal,UpperLimit) >=0 && AngleDiff(LowerLimit,testingVal)>=0){
      return 1;
    }else{
      return 0;
    }
  }
}

Hopefully these can help someone in future.