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