amiro-os / devices / DiWheelDrive / userthread.cpp @ 25388c2f
History | View | Annotate | Download (17.1 KB)
1 |
#include "userthread.hpp" |
---|---|
2 |
|
3 |
#include "global.hpp" |
4 |
|
5 |
#include "linefollow2.hpp" |
6 |
|
7 |
using namespace amiro; |
8 |
|
9 |
extern Global global;
|
10 |
|
11 |
// State machine states
|
12 |
enum states : uint8_t {
|
13 |
IDLE, |
14 |
GO_RIGHT, |
15 |
GO_STRAIGHT, |
16 |
PARKING, |
17 |
PARKING_RIGHT, |
18 |
PARKING_LEFT, |
19 |
GO_LEFT, |
20 |
SPINNING_PARKING, |
21 |
SPINNING |
22 |
}; |
23 |
|
24 |
// Policy
|
25 |
states policy[] = { |
26 |
GO_STRAIGHT, |
27 |
GO_RIGHT, |
28 |
GO_RIGHT, |
29 |
GO_STRAIGHT, |
30 |
GO_RIGHT, |
31 |
GO_STRAIGHT, |
32 |
GO_RIGHT, |
33 |
GO_STRAIGHT, |
34 |
GO_STRAIGHT, |
35 |
GO_RIGHT, |
36 |
GO_STRAIGHT, |
37 |
GO_RIGHT, |
38 |
GO_STRAIGHT |
39 |
}; |
40 |
|
41 |
// The different classes (or members) of color discrimination
|
42 |
// BLACK is the line itselfe
|
43 |
// GREY is the boarder between the line and the surface
|
44 |
// WHITE is the common surface
|
45 |
enum colorMember : uint8_t {
|
46 |
BLACK=0,
|
47 |
GREY=1,
|
48 |
WHITE=2
|
49 |
}; |
50 |
|
51 |
// a buffer for the z-value of the accelerometer
|
52 |
int16_t accel_z; |
53 |
bool running;
|
54 |
|
55 |
// Get some information about the policy
|
56 |
const int sizeOfPolicy = sizeof(policy) / sizeof(states); |
57 |
int policyCounter = 0; // Do not change this, it points to the beginning of the policy |
58 |
|
59 |
// Different speed settings (all values in "rounds per minute")
|
60 |
const int rpmTurnLeft[2] = {-10, 10}; |
61 |
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]}; |
62 |
const int rpmHalt[2] = {0, 0}; |
63 |
|
64 |
// Definition of the fuzzyfication function
|
65 |
// | Membership
|
66 |
// 1|_B__ G __W__
|
67 |
// | \ /\ /
|
68 |
// | \/ \/
|
69 |
// |_____/\__/\______ Sensor values
|
70 |
// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
|
71 |
// All values are "raw sensor values"
|
72 |
/* Use these values for white ground surface (e.g. paper) */
|
73 |
|
74 |
const int blackStartFalling = 0x1000; // Where the black curve starts falling |
75 |
const int blackOff = 0x1800; // Where no more black is detected |
76 |
const int whiteStartRising = 0x2800; // Where the white curve starts rising |
77 |
const int whiteOn = 0x6000; // Where the white curve has reached the maximum value |
78 |
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum |
79 |
const int greyStartRising = blackStartFalling; // Where grey starts rising |
80 |
const int greyOff = whiteOn; // Where grey is completely off again |
81 |
|
82 |
/* Use these values for gray ground surfaces */
|
83 |
/*
|
84 |
const int blackStartFalling = 0x1000; // Where the black curve starts falling
|
85 |
const int blackOff = 0x2800; // Where no more black is detected
|
86 |
const int whiteStartRising = 0x4000; // Where the white curve starts rising
|
87 |
const int whiteOn = 0x5000; // Where the white curve starts rising
|
88 |
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
|
89 |
const int greyStartRising = blackStartFalling; // Where grey starts rising
|
90 |
const int greyOff = whiteOn; // Where grey is completely off again
|
91 |
*/
|
92 |
|
93 |
int vcnl4020AmbientLight[4] = {0}; |
94 |
int vcnl4020Proximity[4] = {0}; |
95 |
|
96 |
// Border for the discrimination between black and white
|
97 |
const int discrBlackWhite = 16000; // border in "raw sensor values" |
98 |
// Discrimination between black and white (returns BLACK or WHITE)
|
99 |
// The border was calculated by a MAP-decider
|
100 |
colorMember discrimination(int value) {
|
101 |
if (value < discrBlackWhite)
|
102 |
return BLACK;
|
103 |
else
|
104 |
return WHITE;
|
105 |
} |
106 |
|
107 |
// Copy the speed from the source to the target array
|
108 |
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) { |
109 |
target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
110 |
target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
111 |
// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
|
112 |
} |
113 |
|
114 |
// Fuzzyfication of the sensor values
|
115 |
void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) { |
116 |
if (sensorValue < blackStartFalling ) {
|
117 |
// Only black value
|
118 |
fuzziedValue[BLACK] = 1.0f; |
119 |
fuzziedValue[GREY] = 0.0f; |
120 |
fuzziedValue[WHITE] = 0.0f; |
121 |
} else if (sensorValue > whiteOn ) { |
122 |
// Only white value
|
123 |
fuzziedValue[BLACK] = 0.0f; |
124 |
fuzziedValue[GREY] = 0.0f; |
125 |
fuzziedValue[WHITE] = 1.0f; |
126 |
} else if ( sensorValue < greyMax) { |
127 |
// Some greyisch value between black and grey
|
128 |
|
129 |
// Black is going down
|
130 |
if ( sensorValue > blackOff) {
|
131 |
fuzziedValue[BLACK] = 0.0f; |
132 |
} else {
|
133 |
fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff); |
134 |
} |
135 |
|
136 |
// Grey is going up
|
137 |
if ( sensorValue < greyStartRising) {
|
138 |
fuzziedValue[GREY] = 0.0f; |
139 |
} else {
|
140 |
fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising); |
141 |
} |
142 |
|
143 |
// White is absent
|
144 |
fuzziedValue[WHITE] = 0.0f; |
145 |
|
146 |
} else if ( sensorValue >= greyMax) { |
147 |
// Some greyisch value between grey white
|
148 |
|
149 |
// Black is absent
|
150 |
fuzziedValue[BLACK] = 0.0f; |
151 |
|
152 |
// Grey is going down
|
153 |
if ( sensorValue < greyOff) {
|
154 |
fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff); |
155 |
} else {
|
156 |
fuzziedValue[GREY] = 0.0f; |
157 |
} |
158 |
|
159 |
// White is going up
|
160 |
if ( sensorValue < whiteStartRising) {
|
161 |
fuzziedValue[WHITE] = 0.0f; |
162 |
} else {
|
163 |
fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising); |
164 |
} |
165 |
} |
166 |
} |
167 |
|
168 |
// Return the color, which has the highest fuzzy value
|
169 |
colorMember getMember(float (&fuzzyValue)[3]) { |
170 |
colorMember member; |
171 |
|
172 |
if (fuzzyValue[BLACK] > fuzzyValue[GREY])
|
173 |
if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
|
174 |
member = BLACK; |
175 |
else
|
176 |
member = WHITE; |
177 |
else
|
178 |
if (fuzzyValue[GREY] > fuzzyValue[WHITE])
|
179 |
member = GREY; |
180 |
else
|
181 |
member = WHITE; |
182 |
|
183 |
return member;
|
184 |
} |
185 |
|
186 |
// Get a crisp output for the steering commands
|
187 |
void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) { |
188 |
|
189 |
// all sensors are equal
|
190 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
|
191 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] && |
192 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) { |
193 |
// something is wrong -> stop
|
194 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
195 |
// both front sensor detect a line
|
196 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
197 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
198 |
// straight
|
199 |
copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl); |
200 |
// exact one front sensor detects a line
|
201 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
202 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
203 |
// soft correction
|
204 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
205 |
// soft right
|
206 |
copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl); |
207 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) { |
208 |
// hard right
|
209 |
copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl); |
210 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
211 |
// soft left
|
212 |
copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl); |
213 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) { |
214 |
// hard left
|
215 |
copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl); |
216 |
} |
217 |
// both wheel sensors detect a line
|
218 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK && |
219 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
220 |
// something is wrong -> stop
|
221 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
222 |
// exactly one wheel sensor detects a line
|
223 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK || |
224 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
225 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
|
226 |
// turn left
|
227 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
228 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
229 |
// turn right
|
230 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
231 |
} |
232 |
// both front sensors may detect a line
|
233 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY && |
234 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
235 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
236 |
// turn left
|
237 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
238 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
239 |
// turn right
|
240 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
241 |
} |
242 |
// exactly one front sensor may detect a line
|
243 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
244 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
245 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
246 |
// turn left
|
247 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
248 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
249 |
// turn right
|
250 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
251 |
} |
252 |
// both wheel sensors may detect a line
|
253 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY && |
254 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
255 |
// something is wrong -> stop
|
256 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
257 |
// exactly one wheel sensor may detect a line
|
258 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY || |
259 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
260 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
261 |
// turn left
|
262 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
263 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
264 |
// turn right
|
265 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
266 |
} |
267 |
// no sensor detects anything
|
268 |
} else {
|
269 |
// line is lost -> stop
|
270 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
271 |
} |
272 |
|
273 |
return;
|
274 |
} |
275 |
|
276 |
Color memberToLed(colorMember member) { |
277 |
switch (member) {
|
278 |
case BLACK:
|
279 |
return Color(Color::GREEN);
|
280 |
case GREY:
|
281 |
return Color(Color::YELLOW);
|
282 |
case WHITE:
|
283 |
return Color(Color::RED);
|
284 |
default:
|
285 |
return Color(Color::WHITE);
|
286 |
} |
287 |
} |
288 |
|
289 |
//void lineFollowing_new(xyz) {}
|
290 |
|
291 |
void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]){ |
292 |
// all sensors are equal
|
293 |
// if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
|
294 |
// member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
|
295 |
// member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
|
296 |
// // something is wrong -> stop
|
297 |
// copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
|
298 |
// // both front sensor detect a line
|
299 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
|
300 |
(member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)) { |
301 |
// straight
|
302 |
copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl); |
303 |
// Deviation to right
|
304 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK |
305 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){ |
306 |
copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl); |
307 |
// Deviation to left
|
308 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK |
309 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){ |
310 |
copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl); |
311 |
// Hard deviatio to right
|
312 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
313 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){ |
314 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
315 |
// Hard deviation to left
|
316 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
317 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){ |
318 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
319 |
// stop if white
|
320 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE |
321 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){ |
322 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
323 |
} |
324 |
} |
325 |
|
326 |
// Line following by a fuzzy controler
|
327 |
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) { |
328 |
// FUZZYFICATION
|
329 |
// First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
|
330 |
float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3]; |
331 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues); |
332 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues); |
333 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues); |
334 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues); |
335 |
|
336 |
// INFERENCE RULE DEFINITION
|
337 |
// Get the member for each sensor
|
338 |
colorMember member[4];
|
339 |
member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues); |
340 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues); |
341 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues); |
342 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues); |
343 |
|
344 |
// visualize sensors via LEDs
|
345 |
global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT])); |
346 |
global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT])); |
347 |
global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
348 |
global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT])); |
349 |
|
350 |
// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
|
351 |
// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
352 |
|
353 |
// DEFUZZYFICATION
|
354 |
// defuzzyfication(member, rpmFuzzyCtrl);
|
355 |
defuzz(member, rpmFuzzyCtrl); |
356 |
} |
357 |
|
358 |
|
359 |
|
360 |
|
361 |
// Set the speed by the array
|
362 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
363 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
364 |
} |
365 |
|
366 |
// Get the next policy rule
|
367 |
states getNextPolicy() { |
368 |
// If the policy is over, start again
|
369 |
if (policyCounter >= sizeOfPolicy)
|
370 |
policyCounter = 3;
|
371 |
|
372 |
return policy[policyCounter++];
|
373 |
} |
374 |
|
375 |
|
376 |
|
377 |
UserThread::UserThread() : |
378 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
379 |
{ |
380 |
} |
381 |
|
382 |
UserThread::~UserThread() |
383 |
{ |
384 |
} |
385 |
|
386 |
msg_t |
387 |
UserThread::main() |
388 |
{ |
389 |
/*
|
390 |
* SETUP
|
391 |
*/
|
392 |
int rpmFuzzyCtrl[2] = {0}; |
393 |
for (uint8_t led = 0; led < 8; ++led) { |
394 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
395 |
} |
396 |
running = false;
|
397 |
LineFollow lf; |
398 |
/*
|
399 |
* LOOP
|
400 |
*/
|
401 |
while (!this->shouldTerminate()) |
402 |
{ |
403 |
/*
|
404 |
* read accelerometer z-value
|
405 |
*/
|
406 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
407 |
|
408 |
/*
|
409 |
* evaluate the accelerometer
|
410 |
*/
|
411 |
|
412 |
//Features over can: (Line Following)
|
413 |
//SetKP
|
414 |
//SetKI
|
415 |
//SetKd ?
|
416 |
//SetSleepTime
|
417 |
//SeForwardSpeed? or SetMaxSpeed
|
418 |
//DriveOnLeftLine
|
419 |
//DriveOnRightLine
|
420 |
|
421 |
//if accel_z<-900
|
422 |
//send can event (one time)
|
423 |
// Line following is started if amiro roteted
|
424 |
if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature |
425 |
if (running) {
|
426 |
// stop the robot
|
427 |
running = false;
|
428 |
global.motorcontrol.setTargetRPM(0, 0); |
429 |
} else {
|
430 |
// start the robot
|
431 |
running = true;
|
432 |
} |
433 |
// set the front LEDs to blue for one second
|
434 |
global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK)); |
435 |
global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK)); |
436 |
global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE)); |
437 |
global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE)); |
438 |
global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE)); |
439 |
global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE)); |
440 |
global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK)); |
441 |
global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK)); |
442 |
this->sleep(MS2ST(1000)); |
443 |
global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK)); |
444 |
global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK)); |
445 |
global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK)); |
446 |
global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK)); |
447 |
} |
448 |
|
449 |
if (running) {
|
450 |
// Read the proximity values
|
451 |
for (int i = 0; i < 4; i++) { |
452 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
453 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
454 |
} |
455 |
lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global); |
456 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
|
457 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
|
458 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
|
459 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
|
460 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
|
461 |
//if (configLineFollowing==2)
|
462 |
// lineFollownew
|
463 |
//else
|
464 |
// lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
|
465 |
setRpmSpeed(rpmFuzzyCtrl); |
466 |
} |
467 |
|
468 |
// this->sleep(US2ST(5));
|
469 |
this->sleep(CAN::UPDATE_PERIOD);
|
470 |
} |
471 |
|
472 |
return RDY_OK;
|
473 |
} |