Go Down

Topic: Little problem with my Stabilization Program - MSP (Read 301 times) previous topic - next topic

Mastino

Jan 14, 2012, 10:35 am Last Edit: Jan 14, 2012, 10:37 am by Mastino Reason: 1
Hello everybody,
I have I little problem with this code. The idea is that the robot before to go straight he reads data from compass. And when he go straight, he reads every second compass data again compares it, and if is it different, he corrects his drive direction.
So the code looks something like this:
maindirection = readcompas;    
goforward;
currentdirection =  readcompas;
if (currentdirection > maindirection) {
turnleft();
}
if(currentdirection < maindirection)  {
turnright();
}

This code is working well if main direction is from about 50 to 310 degrees. Because if main direction is 360 or 1 he makes one turn to correct drive direction. Because if main direction = 358 and current direction = 2, he need only to turn left for 4 degrees and he will be on the right way.., but with my code he will turn right till 2 becomes 358(2,3,4,5,6,7,8,9..) so he will make one turn..
Maybe someone knows the way how to avoid this problem?
Thank you very much for your answers!
P.S. Why MSP and not ESP? Because: Magnetic Stabilization Program..:) and not Electronic Stabilization Program.. like on VW...:)

PeterH

Subtract desired heading from current heading to get the angle you want to turn.
Use modulus to convert this to a number between -180 ... 180.
Turn by the angle you have calculated.
I only provide help via the forum - please do not contact me for private consultancy.

wildbill

Cruder version of PeterH's solution, but since I had typed it up, here it is anyway:
Code: [Select]

if (currentdirection > maindirection)
  {
  if((currentdirection-maindirection) < 180)
    turnleft();
  else
    turnright();
  }
else
  {
  if((maindirection-currentdirection) < 180)
    turnright();
  else
    turnleft();
  }

Mastino

Thank you wildbil and PeterH for great answer!  

Go Up