ASURO Library  2.80
encoder.c
gehe zur Dokumentation dieser Datei
1 /****************************************************************************/
38 /*****************************************************************************
39 * *
40 * This program is free software; you can redistribute it and/or modify *
41 * it under the terms of the GNU General Public License as published by *
42 * the Free Software Foundation; either version 2 of the License, or *
43 * any later version. *
44 * *
45 *****************************************************************************/
46 #include "asuro.h"
47 #include "myasuro.h"
48 
49 
50 /***************************************************************************
51 * void GoTurn(int distance, int degree, int speed)
52 *
53 * Go's a distance in mm OR
54 * Turn's given angle.
55 * Attention: EncoderInit() has to be called first.
56 *
57 * the driven distance depends a little bit from the floor friction
58 * limitations: maximum distance +-32m
59 * possible problems: in full sunlight the encoder sensors may be disturbed
60 *
61 * input
62 * distance: postiv->go forward; negativ->go backward; ZERO->use degree for TURN
63 * degree: postiv->turn right; negativ->turn left
64 * speed: sets motorspeed
65 *
66 * last modification:
67 * Ver. Date Author Comments
68 * ------- ---------- -------------- ---------------------------------
69 * sto1 29.07.2005 stochri motorfunction
70 * And1 31.07.2005 Andun added speed and Odometrie
71 * And2 07.08.2005 Andun Added Odometrie function
72 * sto2 31.10.2006 stochri distance in mm
73 * sto2 31.10.2006 stochri added comments, corrected enc_count initialisation
74 * stth 07.06.2007 Sternthaler combine Go() and Turn() into this
75 * ------- ---------- -------------- ---------------------------------
76 *
77 ***************************************************************************/
78 /****************************************************************************/
128 void GoTurn (
129  int distance,
130  int degree,
131  int speed)
132 {
133  unsigned long enc_count;
134  int tot_count = 0;
135  int diff = 0;
136  int l_speed = speed, r_speed = speed;
137 
138  /* stop the motors until the direction is set */
139  MotorSpeed (0, 0);
140 
141  /* if distance is NOT zero, then take this value to go ... */
142  if (distance != 0)
143  {
144  /* calculate tics from mm */
145  enc_count = abs (distance) * 10000L;
146  enc_count /= MY_GO_ENC_COUNT_VALUE;
147 
148  if (distance < 0)
149  MotorDir (RWD, RWD);
150  else
151  MotorDir (FWD, FWD);
152  }
153  /* ... else take the value degree for a turn */
154  else
155  {
156  /* calculate tics from degree */
157  enc_count = abs (degree) * MY_TURN_ENC_COUNT_VALUE;
158  enc_count /= 360L;
159 
160  if (degree < 0)
161  MotorDir (RWD, FWD);
162  else
163  MotorDir (FWD, RWD);
164  }
165 
166  /* reset encoder */
167  EncoderSet (0, 0);
168 
169  /* now start the machine */
170  MotorSpeed (l_speed, r_speed);
171 
172  while (tot_count < enc_count)
173  {
174  tot_count += encoder [LEFT];
175  diff = encoder [LEFT] - encoder [RIGHT];
176 
177  if (diff > 0)
178  { /* Left faster than right */
179  if ((l_speed > speed) || (r_speed > 244))
180  l_speed -= 10;
181  else
182  r_speed += 10;
183  }
184 
185  if (diff < 0)
186  { /* Right faster than left */
187  if ((r_speed > speed) || (l_speed > 244))
188  r_speed -= 10;
189  else
190  l_speed += 10;
191  }
192  /* reset encoder */
193  EncoderSet (0, 0);
194 
195  MotorSpeed (l_speed, r_speed);
196  Msleep (1);
197  }
198  MotorDir (BREAK, BREAK);
199  Msleep (200);
200 }