amiro-os / devices / DiWheelDrive / userthread.cpp @ 58fe0e0b
History | View | Annotate | Download (12.9 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 = 0x4000; // Where the white curve starts rising |
80 |
const int whiteOn = 0x8000; // 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 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
|
192 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
193 |
// straight
|
194 |
copyRpmSpeed(rpmForward, rpmFuzzyCtrl); |
195 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
196 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
197 |
// soft correction
|
198 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY)
|
199 |
// soft right
|
200 |
copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl); |
201 |
else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) |
202 |
// hard right
|
203 |
copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl); |
204 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) |
205 |
// soft left
|
206 |
copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl); |
207 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) |
208 |
// hard left
|
209 |
copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl); |
210 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
211 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
212 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)
|
213 |
// turn right
|
214 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
215 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) |
216 |
// turn left
|
217 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
218 |
else
|
219 |
// go straight
|
220 |
copyRpmSpeed(rpmForward, rpmFuzzyCtrl); |
221 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE && |
222 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) { |
223 |
// go straight and check wheel sensors
|
224 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] != WHITE)
|
225 |
// turn left
|
226 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
227 |
else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] != WHITE) |
228 |
// turn right
|
229 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
230 |
else
|
231 |
// line is lost -> stop
|
232 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
233 |
} |
234 |
|
235 |
return;
|
236 |
} |
237 |
|
238 |
Color memberToLed(colorMember member) { |
239 |
switch (member) {
|
240 |
case BLACK:
|
241 |
return Color(Color::GREEN);
|
242 |
case GREY:
|
243 |
return Color(Color::YELLOW);
|
244 |
case WHITE:
|
245 |
return Color(Color::RED);
|
246 |
default:
|
247 |
return Color(Color::WHITE);
|
248 |
} |
249 |
} |
250 |
|
251 |
// Line following by a fuzzy controler
|
252 |
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) { |
253 |
// FUZZYFICATION
|
254 |
// First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
|
255 |
float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3]; |
256 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues); |
257 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues); |
258 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues); |
259 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues); |
260 |
|
261 |
// INFERENCE RULE DEFINITION
|
262 |
// Get the member for each sensor
|
263 |
colorMember member[4];
|
264 |
member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues); |
265 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues); |
266 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues); |
267 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues); |
268 |
|
269 |
// visualize sensors via LEDs
|
270 |
global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT])); |
271 |
global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT])); |
272 |
global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
273 |
global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT])); |
274 |
|
275 |
// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);
|
276 |
// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
277 |
|
278 |
// DEFUZZYFICATION
|
279 |
defuzzyfication(member, rpmFuzzyCtrl); |
280 |
} |
281 |
|
282 |
// Set the speed by the array
|
283 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
284 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
285 |
} |
286 |
|
287 |
// Get the next policy rule
|
288 |
states getNextPolicy() { |
289 |
// If the policy is over, start again
|
290 |
if (policyCounter >= sizeOfPolicy)
|
291 |
policyCounter = 3;
|
292 |
|
293 |
return policy[policyCounter++];
|
294 |
} |
295 |
|
296 |
|
297 |
|
298 |
UserThread::UserThread() : |
299 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
300 |
{ |
301 |
} |
302 |
|
303 |
UserThread::~UserThread() |
304 |
{ |
305 |
} |
306 |
|
307 |
msg_t |
308 |
UserThread::main() |
309 |
{ |
310 |
/*
|
311 |
* SETUP
|
312 |
*/
|
313 |
int rpmFuzzyCtrl[2] = {0}; |
314 |
for (uint8_t led = 0; led < 8; ++led) { |
315 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
316 |
} |
317 |
running = false;
|
318 |
|
319 |
/*
|
320 |
* LOOP
|
321 |
*/
|
322 |
while (!this->shouldTerminate()) |
323 |
{ |
324 |
/*
|
325 |
* read accelerometer z-value
|
326 |
*/
|
327 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
328 |
|
329 |
/*
|
330 |
* evaluate the accelerometer
|
331 |
*/
|
332 |
if (accel_z < -900 /*-0.9g*/) { |
333 |
if (running) {
|
334 |
// stop the robot
|
335 |
running = false;
|
336 |
global.motorcontrol.setTargetRPM(0, 0); |
337 |
} else {
|
338 |
// start the robot
|
339 |
running = true;
|
340 |
} |
341 |
// set the front LEDs to blue for one second
|
342 |
global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK)); |
343 |
global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK)); |
344 |
global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE)); |
345 |
global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE)); |
346 |
global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE)); |
347 |
global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE)); |
348 |
global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK)); |
349 |
global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK)); |
350 |
this->sleep(MS2ST(1000)); |
351 |
global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK)); |
352 |
global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK)); |
353 |
global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK)); |
354 |
global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK)); |
355 |
} |
356 |
|
357 |
if (running) {
|
358 |
// Read the proximity values
|
359 |
for (int i = 0; i < 4; i++) { |
360 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
361 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
362 |
} |
363 |
|
364 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
|
365 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
|
366 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
|
367 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
|
368 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
|
369 |
|
370 |
lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl); |
371 |
setRpmSpeed(rpmFuzzyCtrl); |
372 |
} |
373 |
|
374 |
this->sleep(MS2ST(100)); |
375 |
} |
376 |
|
377 |
return RDY_OK;
|
378 |
} |
379 |
|