ASURO Library
2.80
Hauptseite
Dateien
Auflistung der Dateien
Datei-Elemente
lib
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
}
Erzeugt am Mit Nov 27 2013 22:55:08 für ASURO Library von
1.8.2