amiro-os / devices / DiWheelDrive / userthread.cpp @ c9fa414d
History | View | Annotate | Download (14.124 KB)
1 | 58fe0e0b | Thomas Schöpping | #include "userthread.hpp" |
---|---|---|---|
2 | #include "global.hpp" |
||
3 | 05d54823 | galberding | #include "linefollow.hpp" |
4 | c76baf23 | Georg Alberding | |
5 | 58fe0e0b | Thomas Schöpping | using namespace amiro; |
6 | |||
7 | extern Global global;
|
||
8 | |||
9 | // a buffer for the z-value of the accelerometer
|
||
10 | int16_t accel_z; |
||
11 | 5d138bca | galberding | bool running = false; |
12 | 58fe0e0b | Thomas Schöpping | |
13 | |||
14 | 181f2892 | galberding | /**
|
15 | * Set speed.
|
||
16 | *
|
||
17 | * @param rpmSpeed speed for left and right wheel in rounds/min
|
||
18 | */
|
||
19 | c9fa414d | galberding | void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) { |
20 | 5d138bca | galberding | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
21 | 58fe0e0b | Thomas Schöpping | } |
22 | |||
23 | c9fa414d | galberding | void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) { |
24 | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
||
25 | } |
||
26 | |||
27 | 181f2892 | galberding | void UserThread::lightOneLed(Color color, int idx){ |
28 | 5d138bca | galberding | global.robot.setLightColor(idx, Color(color)); |
29 | } |
||
30 | 58fe0e0b | Thomas Schöpping | |
31 | 181f2892 | galberding | void UserThread::lightAllLeds(Color color){
|
32 | 5d138bca | galberding | int led = 0; |
33 | for(led=0; led<8; led++){ |
||
34 | lightOneLed(color, led); |
||
35 | } |
||
36 | 58fe0e0b | Thomas Schöpping | } |
37 | |||
38 | 181f2892 | galberding | void UserThread::showChargingState(){
|
39 | uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
||
40 | Color color = Color::GREEN; |
||
41 | if (numLeds <= 2){ |
||
42 | color = Color::RED; |
||
43 | }else if(numLeds <= 6){ |
||
44 | color = Color::YELLOW; |
||
45 | } |
||
46 | for (int i=0; i<numLeds; i++){ |
||
47 | lightOneLed(color, i); |
||
48 | this->sleep(300); |
||
49 | 9c46b728 | galberding | } |
50 | 181f2892 | galberding | this->sleep(1000); |
51 | lightAllLeds(Color::BLACK); |
||
52 | 9c46b728 | galberding | } |
53 | |||
54 | /**
|
||
55 | * Blocks as long as the position changes.
|
||
56 | */
|
||
57 | 181f2892 | galberding | void UserThread::checkForMotion(){
|
58 | 9c46b728 | galberding | int motion = 1; |
59 | int led = 0; |
||
60 | types::position oldPos = global.odometry.getPosition(); |
||
61 | while(motion){
|
||
62 | 181f2892 | galberding | this->sleep(500); |
63 | 9c46b728 | galberding | types::position tmp = global.odometry.getPosition(); |
64 | motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z); |
||
65 | oldPos = tmp; |
||
66 | global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
||
67 | global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
||
68 | led++; |
||
69 | } |
||
70 | } |
||
71 | |||
72 | 181f2892 | galberding | bool UserThread::checkPinVoltage(){
|
73 | return global.ltc4412.isPluggedIn();
|
||
74 | } |
||
75 | 9c46b728 | galberding | |
76 | 181f2892 | galberding | bool UserThread::checkPinEnabled(){
|
77 | return global.ltc4412.isEnabled();
|
||
78 | } |
||
79 | |||
80 | int UserThread::checkDockingSuccess(){
|
||
81 | // setRpmSpeed(stop);
|
||
82 | checkForMotion(); |
||
83 | 9c46b728 | galberding | int success = 0; |
84 | global.odometry.resetPosition(); |
||
85 | types::position start = global.startPos = global.odometry.getPosition(); |
||
86 | global.motorcontrol.toggleMotorEnable(); |
||
87 | 61544eee | galberding | this->sleep(500); |
88 | 181f2892 | galberding | types::position stop_ = global.endPos = global.odometry.getPosition(); |
89 | |||
90 | 9c46b728 | galberding | // Amiro moved, docking was not successful
|
91 | 181f2892 | galberding | if ((start.x + stop_.x) || (start.y + stop_.y)){
|
92 | lightAllLeds(Color::RED); |
||
93 | // Enable Motor again if docking was not successful
|
||
94 | global.motorcontrol.toggleMotorEnable(); |
||
95 | 9c46b728 | galberding | success = 0;
|
96 | }else{
|
||
97 | 181f2892 | galberding | lightAllLeds(Color::GREEN); |
98 | 9c46b728 | galberding | success = 1;
|
99 | } |
||
100 | |||
101 | 61544eee | galberding | // this->sleep(500);
|
102 | 181f2892 | galberding | lightAllLeds(Color::BLACK); |
103 | 9c46b728 | galberding | return success;
|
104 | } |
||
105 | |||
106 | c9fa414d | galberding | int UserThread::getProxyRingSum(){
|
107 | int prox_sum = 0; |
||
108 | e2002d0e | galberding | for(int i=0; i<8;i++){ |
109 | prox_sum += global.robot.getProximityRingValue(i);; |
||
110 | } |
||
111 | return prox_sum;
|
||
112 | } |
||
113 | |||
114 | |||
115 | 58fe0e0b | Thomas Schöpping | UserThread::UserThread() : |
116 | chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
||
117 | { |
||
118 | } |
||
119 | |||
120 | UserThread::~UserThread() |
||
121 | { |
||
122 | } |
||
123 | |||
124 | msg_t |
||
125 | UserThread::main() |
||
126 | { |
||
127 | 5d138bca | galberding | /*
|
128 | * SETUP
|
||
129 | */
|
||
130 | 181f2892 | galberding | // User thread state:
|
131 | states utState = states::IDLE; |
||
132 | states newState; |
||
133 | |||
134 | int whiteBuf = 0; |
||
135 | e2002d0e | galberding | int proxyBuf = 0; |
136 | 61544eee | galberding | int releaseBuf = 0; |
137 | 181f2892 | galberding | int correctionStep = 0; |
138 | 61544eee | galberding | int dist_count = 0; |
139 | bool needDistance = false; |
||
140 | |||
141 | 019224ff | galberding | uint16_t rProx[8]; // buffer for ring proxy values |
142 | 9c46b728 | galberding | int rpmSpeed[2] = {0}; |
143 | int stop[2] = {0}; |
||
144 | 61544eee | galberding | int turn[2] = {5,-5}; |
145 | c9fa414d | galberding | LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
146 | 5d138bca | galberding | for (uint8_t led = 0; led < 8; ++led) { |
147 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
148 | } |
||
149 | running = false;
|
||
150 | LineFollow lf(&global); |
||
151 | /*
|
||
152 | * LOOP
|
||
153 | */
|
||
154 | while (!this->shouldTerminate()) |
||
155 | { |
||
156 | 181f2892 | galberding | /*
|
157 | * read accelerometer z-value
|
||
158 | */
|
||
159 | accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
||
160 | |||
161 | if (accel_z < -900 /*-0.9g*/) { |
||
162 | // Start line following when AMiRo is rotated
|
||
163 | if(utState == states::IDLE){
|
||
164 | utState = states::FOLLOW_LINE; |
||
165 | }else{
|
||
166 | utState = states::IDLE; |
||
167 | } |
||
168 | lightAllLeds(Color::GREEN); |
||
169 | this->sleep(1000); |
||
170 | lightAllLeds(Color::BLACK); |
||
171 | c76baf23 | Georg Alberding | |
172 | d607fcef | galberding | // If message was received handle it here:
|
173 | } else if(global.msgReceived){ |
||
174 | global.msgReceived = false;
|
||
175 | 181f2892 | galberding | // running = true;
|
176 | d607fcef | galberding | switch(global.lfStrategy){
|
177 | case msg_content::START:
|
||
178 | 181f2892 | galberding | utState = states::FOLLOW_LINE; |
179 | d607fcef | galberding | break;
|
180 | case msg_content::STOP:
|
||
181 | 181f2892 | galberding | utState = states::IDLE; |
182 | d607fcef | galberding | break;
|
183 | case msg_content::EDGE_RIGHT:
|
||
184 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
185 | d607fcef | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
186 | break;
|
||
187 | case msg_content::EDGE_LEFT:
|
||
188 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
189 | d607fcef | galberding | lStrategy = LineFollowStrategy::EDGE_LEFT; |
190 | break;
|
||
191 | case msg_content::FUZZY:
|
||
192 | 181f2892 | galberding | // utState = states::FOLLOW_LINE;
|
193 | d607fcef | galberding | lStrategy = LineFollowStrategy::FUZZY; |
194 | break;
|
||
195 | 181f2892 | galberding | case msg_content::DOCK:
|
196 | utState = states::DETECT_STATION; |
||
197 | break;
|
||
198 | case msg_content::UNDOCK:
|
||
199 | utState = states::RELEASE; |
||
200 | break;
|
||
201 | case msg_content::CHARGE:
|
||
202 | utState = states::CHARGING; |
||
203 | d607fcef | galberding | break;
|
204 | default:
|
||
205 | 181f2892 | galberding | utState = states::IDLE; |
206 | d607fcef | galberding | break;
|
207 | } |
||
208 | 5d138bca | galberding | } |
209 | 181f2892 | galberding | newState = utState; |
210 | d607fcef | galberding | |
211 | 181f2892 | galberding | // Get sensor data
|
212 | 019224ff | galberding | uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
213 | uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
||
214 | for(int i=0; i<8;i++){ |
||
215 | rProx[i] = global.robot.getProximityRingValue(i); |
||
216 | } |
||
217 | 181f2892 | galberding | // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
218 | // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
219 | switch(utState){
|
||
220 | case states::IDLE:
|
||
221 | if (!global.motorcontrol.getMotorEnable()){
|
||
222 | global.motorcontrol.toggleMotorEnable(); |
||
223 | } |
||
224 | setRpmSpeed(stop); |
||
225 | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
||
226 | global.robot.requestCharging(0);
|
||
227 | } |
||
228 | 61544eee | galberding | whiteBuf = 0;
|
229 | proxyBuf = 0;
|
||
230 | 181f2892 | galberding | break;
|
231 | // ---------------------------------------
|
||
232 | case states::FOLLOW_LINE:
|
||
233 | // Set correct forward speed to every strategy
|
||
234 | if (global.forwardSpeed != global.rpmForward[0]){ |
||
235 | global.forwardSpeed = global.rpmForward[0];
|
||
236 | } |
||
237 | |||
238 | if(lf.getStrategy() != lStrategy){
|
||
239 | 9c46b728 | galberding | lf.setStrategy(lStrategy); |
240 | } |
||
241 | 5d138bca | galberding | |
242 | 181f2892 | galberding | //TODO: Check if white is detected and stop threshold is reached
|
243 | if(lf.followLine(rpmSpeed)){
|
||
244 | 61544eee | galberding | whiteBuf++; |
245 | 181f2892 | galberding | if(whiteBuf >= WHITE_COUNT_THRESH){
|
246 | 61544eee | galberding | rpmSpeed[0] = stop[0]; |
247 | rpmSpeed[1] = stop[1]; |
||
248 | 181f2892 | galberding | newState = states::IDLE; |
249 | 9c46b728 | galberding | } |
250 | 181f2892 | galberding | }else{
|
251 | whiteBuf = 0;
|
||
252 | 61544eee | galberding | // setRpmSpeed(rpmSpeed);
|
253 | 9c46b728 | galberding | } |
254 | e2002d0e | galberding | |
255 | if(getProxyRingSum() > PROXY_RING_THRESH){
|
||
256 | proxyBuf++; |
||
257 | if(proxyBuf > WHITE_COUNT_THRESH){
|
||
258 | newState = states::IDLE; |
||
259 | 61544eee | galberding | rpmSpeed[0] = stop[0]; |
260 | rpmSpeed[1] = stop[1]; |
||
261 | e2002d0e | galberding | } |
262 | }else{
|
||
263 | proxyBuf = 0;
|
||
264 | 61544eee | galberding | } |
265 | |||
266 | if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){ |
||
267 | rpmSpeed[0] = stop[0]; |
||
268 | rpmSpeed[1] = stop[1]; |
||
269 | } |
||
270 | |||
271 | // if (needDistance){
|
||
272 | // whiteBuf = 0;
|
||
273 | // proxyBuf = 0;
|
||
274 | // dist_count++;
|
||
275 | // if(dist_count > DIST_THRESH){
|
||
276 | // dist_count = 0;
|
||
277 | // needDistance = false;
|
||
278 | // }
|
||
279 | // }
|
||
280 | // whiteBuf = 0;
|
||
281 | // proxyBuf = 0;
|
||
282 | 181f2892 | galberding | // lf.followLine(rpmSpeed);
|
283 | c9fa414d | galberding | if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
284 | setRpmSpeedFuzzy(rpmSpeed); |
||
285 | }else{
|
||
286 | |||
287 | setRpmSpeed(rpmSpeed); |
||
288 | } |
||
289 | 181f2892 | galberding | |
290 | break;
|
||
291 | // ---------------------------------------
|
||
292 | case states::DETECT_STATION:
|
||
293 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
294 | global.forwardSpeed = DETECTION_SPEED; |
||
295 | } |
||
296 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
297 | lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
||
298 | } |
||
299 | |||
300 | lf.followLine(rpmSpeed); |
||
301 | setRpmSpeed(rpmSpeed); |
||
302 | // // Detect marker before docking station
|
||
303 | 019224ff | galberding | // if ((WL+WR) < PROXY_WHEEL_THRESH){
|
304 | // Use proxy ring
|
||
305 | if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
||
306 | |||
307 | d607fcef | galberding | setRpmSpeed(stop); |
308 | 181f2892 | galberding | checkForMotion(); |
309 | // 180° Rotation
|
||
310 | global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
||
311 | // BaseThread::sleep(8000);
|
||
312 | checkForMotion(); |
||
313 | newState = states::CORRECT_POSITIONING; |
||
314 | 9c46b728 | galberding | } |
315 | 181f2892 | galberding | break;
|
316 | // ---------------------------------------
|
||
317 | case states::CORRECT_POSITIONING:
|
||
318 | 019224ff | galberding | if (global.forwardSpeed != CHARGING_SPEED){
|
319 | global.forwardSpeed = CHARGING_SPEED; |
||
320 | } |
||
321 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
322 | lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
||
323 | 9c46b728 | galberding | } |
324 | 181f2892 | galberding | lf.followLine(rpmSpeed); |
325 | setRpmSpeed(rpmSpeed); |
||
326 | 9c46b728 | galberding | |
327 | 181f2892 | galberding | correctionStep++; |
328 | if (correctionStep >= MAX_CORRECTION_STEPS){
|
||
329 | correctionStep = 0;
|
||
330 | newState = states::REVERSE; |
||
331 | setRpmSpeed(stop); |
||
332 | checkForMotion(); |
||
333 | } |
||
334 | break;
|
||
335 | // ---------------------------------------
|
||
336 | case states::REVERSE:
|
||
337 | if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
||
338 | lf.setStrategy(LineFollowStrategy::REVERSE); |
||
339 | } |
||
340 | lf.followLine(rpmSpeed); |
||
341 | setRpmSpeed(rpmSpeed); |
||
342 | 58fe0e0b | Thomas Schöpping | |
343 | 019224ff | galberding | // if ((WL+WR) < PROXY_WHEEL_THRESH){
|
344 | // Is of those sensors at it max val means that the AMiRo cant drive back
|
||
345 | // so check if correctly positioned
|
||
346 | //definitely wrong positioned -> correct position directly without rotation
|
||
347 | // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate
|
||
348 | 61544eee | galberding | // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
349 | // setRpmSpeed(stop);
|
||
350 | // checkForMotion();
|
||
351 | // newState = states::CHECK_POSITIONING;
|
||
352 | // } else
|
||
353 | 019224ff | galberding | if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
354 | 61544eee | galberding | // setRpmSpeed(stop);
|
355 | // checkForMotion();
|
||
356 | 181f2892 | galberding | newState = states::CHECK_POSITIONING; |
357 | } |
||
358 | break;
|
||
359 | // ---------------------------------------
|
||
360 | case states::CHECK_POSITIONING:
|
||
361 | 61544eee | galberding | setRpmSpeed(stop); |
362 | checkForMotion(); |
||
363 | // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
|
||
364 | if(checkDockingSuccess()){
|
||
365 | newState = states::CHECK_VOLTAGE; |
||
366 | }else{
|
||
367 | newState = states::CORRECT_POSITIONING; |
||
368 | } |
||
369 | // }
|
||
370 | // else{
|
||
371 | // newState = CORRECT_POSITIONING;
|
||
372 | // }
|
||
373 | 181f2892 | galberding | break;
|
374 | // ---------------------------------------
|
||
375 | ba75ee1d | galberding | case states::CHECK_VOLTAGE:
|
376 | if(!checkPinEnabled()){
|
||
377 | global.robot.requestCharging(1);
|
||
378 | } else {
|
||
379 | if(checkPinVoltage()){
|
||
380 | // Pins are under voltage -> correctly docked
|
||
381 | newState = states::CHARGING; |
||
382 | }else{
|
||
383 | // No voltage on pins -> falsely docked
|
||
384 | // deactivate pins
|
||
385 | c9fa414d | galberding | if (!global.motorcontrol.getMotorEnable()){
|
386 | global.motorcontrol.toggleMotorEnable(); |
||
387 | } |
||
388 | ba75ee1d | galberding | global.robot.requestCharging(0);
|
389 | 61544eee | galberding | if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
390 | newState = states::RELEASE_TO_CORRECT; |
||
391 | } else {
|
||
392 | newState = states::CORRECT_POSITIONING; |
||
393 | } |
||
394 | ba75ee1d | galberding | } |
395 | } |
||
396 | 019224ff | galberding | break;
|
397 | // ---------------------------------------
|
||
398 | case states::RELEASE_TO_CORRECT:
|
||
399 | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
||
400 | checkForMotion(); |
||
401 | // move 1cm forward
|
||
402 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
403 | checkForMotion(); |
||
404 | // rotate back
|
||
405 | global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
||
406 | checkForMotion(); |
||
407 | |||
408 | global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION); |
||
409 | checkForMotion(); |
||
410 | newState = states::CORRECT_POSITIONING; |
||
411 | break;
|
||
412 | ba75ee1d | galberding | // ---------------------------------------
|
413 | 181f2892 | galberding | case states::CHARGING:
|
414 | if (global.motorcontrol.getMotorEnable()){
|
||
415 | global.motorcontrol.toggleMotorEnable(); |
||
416 | } |
||
417 | // Formulate Request to enable charging
|
||
418 | if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
||
419 | global.robot.requestCharging(1);
|
||
420 | } |
||
421 | if(checkPinEnabled()){
|
||
422 | showChargingState(); |
||
423 | } |
||
424 | break;
|
||
425 | |||
426 | // ---------------------------------------
|
||
427 | case states::RELEASE:
|
||
428 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
429 | global.rpmForward[0] = DETECTION_SPEED;
|
||
430 | } |
||
431 | 181f2892 | galberding | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
432 | global.robot.requestCharging(0);
|
||
433 | }else{
|
||
434 | if (!global.motorcontrol.getMotorEnable()){
|
||
435 | global.motorcontrol.toggleMotorEnable(); |
||
436 | } |
||
437 | 61544eee | galberding | // if (releaseBuf > RELEASE_COUNT){
|
438 | // releaseBuf = 0;
|
||
439 | // setRpmSpeed(stop);
|
||
440 | // needDistance = true;
|
||
441 | // newState = states::FOLLOW_LINE;
|
||
442 | // }else{
|
||
443 | // releaseBuf++;
|
||
444 | // setRpmSpeed(turn);
|
||
445 | // }
|
||
446 | 181f2892 | galberding | //Rotate -20° to free from magnet
|
447 | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
||
448 | 61544eee | galberding | checkForMotion(); |
449 | // move 1cm forward
|
||
450 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
451 | checkForMotion(); |
||
452 | // rotate back
|
||
453 | global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
||
454 | checkForMotion(); |
||
455 | 019224ff | galberding | |
456 | 61544eee | galberding | // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
457 | // checkForMotion();
|
||
458 | 181f2892 | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
459 | newState = states::FOLLOW_LINE; |
||
460 | 61544eee | galberding | // whiteBuf = -100;
|
461 | // lf.followLine(rpmSpeed);
|
||
462 | // setRpmSpeed(rpmSpeed);
|
||
463 | 181f2892 | galberding | } |
464 | 61544eee | galberding | // lightAllLeds(Color::BLACK);
|
465 | 181f2892 | galberding | break;
|
466 | |||
467 | default:
|
||
468 | break;
|
||
469 | } |
||
470 | utState = newState; |
||
471 | 5d138bca | galberding | this->sleep(CAN::UPDATE_PERIOD);
|
472 | } |
||
473 | 58fe0e0b | Thomas Schöpping | |
474 | return RDY_OK;
|
||
475 | } |