Pages: [1]   Go Down
Author Topic: Little problem with my Stabilization Program - MSP  (Read 271 times)
0 Members and 1 Guest are viewing this topic.
Baltic states
Offline Offline
Jr. Member
**
Karma: 0
Posts: 97
new day, new try
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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..smiley and not Electronic Stabilization Program.. like on VW...smiley
« Last Edit: January 14, 2012, 04:37:23 am by Mastino » Logged

UK
Offline Offline
Shannon Member
****
Karma: 222
Posts: 12549
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

New Jersey
Offline Offline
Faraday Member
**
Karma: 65
Posts: 3638
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Cruder version of PeterH's solution, but since I had typed it up, here it is anyway:
Code:
if (currentdirection > maindirection)
  {
  if((currentdirection-maindirection) < 180)
    turnleft();
  else
    turnright();
  }
else
  {
  if((maindirection-currentdirection) < 180)
    turnright();
  else
    turnleft();
  }
Logged

Baltic states
Offline Offline
Jr. Member
**
Karma: 0
Posts: 97
new day, new try
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thank you wildbil and PeterH for great answer!  
Logged

Pages: [1]   Go Up
Jump to: