Go Down

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

#### Mastino

##### Jan 14, 2012, 10:35 amLast 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:
goforward;
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?
P.S. Why MSP and not ESP? Because: Magnetic Stabilization Program.. and not Electronic Stabilization Program.. like on VW...

#### PeterH

#1
##### Jan 14, 2012, 01:37 pm
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.

#### wildbill

#2
##### Jan 14, 2012, 01:47 pm
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

#3
##### Jan 14, 2012, 01:51 pm
Thank you wildbil and PeterH for great answer!

Go Up