Context Navigation

ControllerCore.nxc @ 663

Visit:

Revision 663, 28.9 KB (checked in by atorf, 4 years ago)

Bugfixing in MotorControl 2.0:

Fixed the strange "motor runs too slow with speed regulation although power seems high" bug
Added support for all "direct command like" instructions via NXC_MotorControl call from MATLAB
Overwriting a currently running motor WITHOUT tacholimit with a new speed is now possible…
For synced driving, motors have to be stopped before every time, otherwise it doesn't work!
Tweaked some timeout constants
Still one bug missing: With many motors running at the same time, a task can lock up, but where? Can't be stopped with direct command, why?

Line
1	//#!C
2	/*
3	% Part of MotorControl containing the main PD-controller function, to be
4	% included TWICE via preprocessor tricks :-)
5	%
6	% Signature
7	% Author: Linus Atorf (see AUTHORS)
8	% Date: 2009/06/28
9	% Copyright: 2007-2009, RWTH Aachen University
10	%
11	%
12	% ***********************************************************************************************
13	% * This file is part of the RWTH - Mindstorms NXT Toolbox. *
14	% * *
15	% * The RWTH - Mindstorms NXT Toolbox is free software: you can redistribute it and/or modify *
16	% * it under the terms of the GNU General Public License as published by the Free Software *
17	% * Foundation, either version 3 of the License, or (at your option) any later version. *
18	% * *
19	% * The RWTH - Mindstorms NXT Toolbox is distributed in the hope that it will be useful, *
20	% * but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS *
21	% * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. *
22	% * *
23	% * You should have received a copy of the GNU General Public License along with the *
24	% * RWTH - Mindstorms NXT Toolbox. If not, see <http://www.gnu.org/licenses/>. *
25	% ***********************************************************************************************
26	*/
27
28
29	// this function CAN and HAS TO BE INCLUDED MULTIPLE TIMES!!!
30
31
32
33	// To get functionality for TWO SYNCED MOTORS, do the following:
34	// Always take RunMotor, copy-paste it, call it RunMotor2, add another
35	// argument port2 to it, and done. BEFORE the function, define RUNMOTOR2_SYNCMODE
36
37	// note how RunMotor2 doesn't need to be inline and takes 1 more argument...
38	#ifdef RUNMOTOR2_SYNCMODE
39	void RunMotor2(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart, const byte &port2) {
40	#else
41	inline void RunMotor(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart) {
42	#endif
43
44	/*
45	The whole concept works like this:
46	There is a loop with static execution time for worst case -- if it's faster,
47	there's busy waiting at the end. Durinc decelaration / braking, we control the
48	motor with a PD-controller algorithm. The constant loop execution time is
49	important, as a varying delta T for the controller is really bad. It would
50	require a different set of KP and KD parameters, and the speed calculation the
51	way it's currently implemented would also change its sliding integration window,
52	so we stick with constant loop timing. Execution speed can vary since in the
53	worst case, all 3 motors are controlled by this function, running in 3 threads
54	at once. There is a 4th thread to listen for incoming messages, so we only have
55	1 quarter of the NXT's original CPU power left per thread. We don't want to
56	program statically, since than we'd allways have the worst case. Hence the
57	dynamic busy waiting at the end, while still intelligently checking if the
58	desired motor position has been reached (so the waiting is still useful in some
59	way).
60
61	So we've got a loop with constant execution time, and a PD-controller during
62	deceleration. The motor-power will constantly be set down, linearly, dependent
63	on its current position. This way, if it get's stuck (i.e. during braking the
64	load somehow increases or the motor blocks), the controller adjusts the power
65	accordingly (the controller has a given set-point for power, depending where
66	in relation to our position target we stand).
67
68	Since the loop is fairly slow in the worst case, we've got time to spare in the
69	best case (2 threads running, 1 motor + 1 message listener). During the waiting
70	time, we check if the motors get's close to its target -- this only matters
71	during BRAKE stage.
72
73	Yes, inside the big outer loop, there are different stages. During RAMPUP we can
74	implement a manual rampup (power constantly increasing) for a smooth start.
75	During driving, nothing happens but measuring speed and deciding when the whole
76	braking/deceleration should begin.
77
78	Then there's the BRAKING stage, with the already mentioned PD controller in
79	action. This is what's actually important. To get precision, we have to very
80	closely monitor when our desired target is reached. In order to achieve this,
81	we've got the ENDGAME mode.
82
83	ENDGAME phase will be entered a couple of degrees before the motor reaches its
84	goal. The PD controller should have succeded by now to enforce a really low
85	turning speed of the motor (just as we targeted/specified in our desired
86	"speed at motor position" deceleration curve lookup table). The most important
87	and most critical thing of the whole function is now to come to a stop at
88	exactly the right position. All CPU time we have is needed to monitor the
89	motor as often as possible and enable the brake at the target position. This
90	is whats called the endgame inner loop. To avoid the very unlikely event of a
91	lock-up (the motor gets blocked just a tiny degree or two befor its final goal),
92	there's also a timeout.
93
94
95	The idea of "stages" (or phases) during a main loop was chosen so that short
96	movements are possible with the same code/function. If we had different loops
97	after each other, we'd have to check for abort conditions all the time, and
98	include speed monitoring in each loop.
99	This way with stages in one loop, the algorithm (or command flow) can proceed
100	from stage to stage, even if the current stage has to be ended premature (i.e.
101	a motor is not fully ramped up yet, but already has to start braking in order
102	to reach its target safely).
103
104	*/
105
106
107
108
109	// **** THINGS NOT IN THIS FUNCTION
110	// - decision if NO CONTROLLED LOOP is needed (classic way?!)
111	// - defer as much decision etc as possible to MATLAB, see below
112
113	// **** More lookups needed:
114	// - Brakingdist from Speed
115	// - Absolute acceleration power during rampup (in discrete steps like 10, 20, etc)
116
117
118	// --- Some DECLARATIONS -- not all of them, this is timecritical
119	long i;
120
121	int powerSgn = sign(power);
122	long tachoTarget = powerSgn * tacholimit;
123
124
125	// --- Reset counters etc
126	// * See below, now happens implicitely in MotorCmd*Reset
127
128	// --- Decide if SIMPLE FUNCTION needed?
129	// * For small powers!
130	// * For (very?) short distances???
131
132	// --- Start driving (important that its EARLY in this function -- for direct commands)
133	// * Runstate RUNNING
134	// * power = +-1
135	// * tacholimit already...
136
137
138	//TODO why exactly do we only power up to +/-1 here? couldn't it be full speed already?
139	// maybe it's because for speed monitor initialization, we use TachoCount and need it to remain
140	// constant etc?
141	#ifdef RUNMOTOR2_SYNCMODE
142	//avoid already synced motors (that doesn't work as we know...)
143	until((MotorRegulation(port) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) {
144	// repeatedly setting this is not nice, but so
145	// we don't need a timeout...!
146	MotorOff(port);
147	MotorOff(port2);
148	// make sure VM applies our settings
149	Wait(1);
150	}//end until
151	MotorCmdDoubleReset(port, powerSgn, tacholimit, port2);
152	#else
153	MotorCmdSingleReset(port, powerSgn, tacholimit, speedreg);
154	#endif
155
156
157	#ifdef ENABLEDEBUGGING_OLDLCDTIMING
158	motorStartedTime = CurrentTick() - receivedMsgTime; // NEW DEBUG
159	#endif
160
161
162	// --- Declare and Initialize various variables
163	// * STAGE is RAMPUP
164	int curStage = STAGE_RAMPUP;
165
166	int absPower = abs(power);
167
168	int maxAbsBrakingPower;
169	maxAbsBrakingPower = absPower;
170	if (speedreg) {
171	maxAbsBrakingPower += ADDITIONALMAXSPEEDREGBRAKINGPOWER;
172	if (maxAbsBrakingPower > 100) {
173	maxAbsBrakingPower = 100;
174	}//end if
175	}//end if
176
177
178	long absBrakingDist;
179	long brakingStartSpeed;
180	long brakingStartTacho;
181
182	long loopStartTick;
183	long waitEndTick;
184
185	bool endgameSuccessful = false;
186
187	long rampupLoopCount = 0;
188	long newPower;
189
190
191
192	//PID stuff
193	long err_0 = 0; // current error (t = 0)
194	long err_1 = 0; // prev. error (t = - T)
195	long curPIDPowerUpscaled = absPower * PIDUPSCALING;
196	long newAbsPower;
197	long desiredSpeed;
198
199	// speed stuff
200	long curSpeed;
201	int SpeedLogIndex = 0;
202	long TachoCountLog[SPEEDHISTORY];
203	long TickCountLog[SPEEDHISTORY];
204
205	// init speedlog
206	for(i = 0; i < SPEEDHISTORY; i++) {
207	TachoCountLog[i] = 0; // should still be 0, was reset just a moment ago
208	TickCountLog[i] = CurrentTick(); //careful, check if this works...
209	}//end for
210
211
212	#ifdef ENABLEDEBUGGING_LCD
213	long loopCount = 0;
214	long lastTick = CurrentTick();
215	long lastLoopTime;
216	long totalStartTick = CurrentTick();
217	long totalEndTick;
218	long totalControlTime;
219	long endgameLoopCount = 0;
220	string lcdTmp;
221	string lcdTmp1;
222	string lcdTmp2;
223	#endif// - - - - - - - - - -
224
225	#ifdef ENABLEDEBUGGING_REMOTE
226	string tmp;
227	string tmp1;
228	string tmp2;
229	string tmp3;
230	bool brakingStartMarkerSent = false;
231	#endif// - - - - - - - - - -
232
233
234	#ifdef ENABLEDEBUGGING_ACOUSTIC
235	PlayTone(300,100);
236	#endif
237
238	// --- MAIN LOOP ---------------------
239	while(true) {
240
241	// * record time
242	loopStartTick = CurrentTick();
243
244	#ifdef ENABLEDEBUGGING_LCD
245	loopCount++;
246	lastLoopTime = CurrentTick() - lastTick;
247	lastTick = CurrentTick();
248	#endif// - - - - - - - - - -
249
250
251	// * check STOP CONDITIONS?
252	// - already there?
253	// - direct command stopped us?
254	// this is like an emergency-check to see if endgame should be enabled?
255	//TODO do we need this below?
256	/*
257	if ( abs(tachoTarget - MotorTachoCount(port)) >= 2 ) {
258	// we are almost already there :-)
259	// or even too far :-/
260	}//end if
261	*/
262
263	if ( MotorPower(port) == 0 ) {
264	//direct command must have stopped us!
265	//TODO is this really a clean direct command exit?
266	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
267	TextOut(0, LCD_LINE8, "ABORTED(start) ");
268	#endif
269	return;
270	}//end if
271
272
273	// * RECORD SPEED
274	// - with this long loop execution time, use speed history of 2
275	// - 2 vars, toggle between history...
276	curSpeed = ((MotorTachoCount(port) - TachoCountLog[SpeedLogIndex]) * 1000) / (CurrentTick() - TickCountLog[SpeedLogIndex]);
277
278	//TODO optimize speed logging by using only 2 history values and toggling between them
279
280	// record speedlog data to previous index...
281	i = SpeedLogIndex - 1;
282	if (i < 0) i = SPEEDHISTORY-1;
283	TachoCountLog[i] = MotorTachoCount(port);
284	TickCountLog[i] = CurrentTick();
285
286	// move current index for next reading
287	SpeedLogIndex++;
288	if (SpeedLogIndex > SPEEDHISTORY-1) SpeedLogIndex = 0;
289
290
291	// * if RAMPUP STAGE:
292	// - increase power (set according value from lookup / count loop-passes)
293	// - enter NEXT STAGE when power is reached...
294	// - maybe enable SPEED REG if not already done?
295	if (curStage == STAGE_RAMPUP) {
296	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
297	TextOut(0, LCD_LINE8, "RAMPUP ");
298	#endif
299
300	// do we need rampup at all?
301	if (smoothstart) {
302
303	rampupLoopCount++;
304	// when rampup is done
305	if (rampupLoopCount >= SMOOTHSTARTSTEPS) {
306	if (speedreg) {
307	//there's no speedreg for synced driving...
308	UpdatePowerAndEnableSpeedReg(port, power);
309	} else {
310	UpdatePower(port, power);
311	#ifdef RUNMOTOR2_SYNCMODE
312	UpdatePower(port2, power);
313	#endif
314	}//end if
315	curStage = STAGE_DRIVING;
316	} else {
317	// simple linear interpolation between 0->100
318	newPower = ((rampupLoopCount * 100)/SMOOTHSTARTSTEPS);
319	// if power < 100 we might be done sooner:
320	if (newPower > absPower) {
321	if (speedreg) {
322	//there's no speedreg for synced driving...
323	UpdatePowerAndEnableSpeedReg(port, power);
324	} else {
325	UpdatePower(port, power);
326	#ifdef RUNMOTOR2_SYNCMODE
327	UpdatePower(port2, power);
328	#endif
329	}//end if
330	curStage = STAGE_DRIVING;
331	} else {
332	// here's the actual rampup step!
333	UpdatePower(port, newPower * powerSgn);
334	#ifdef RUNMOTOR2_SYNCMODE
335	UpdatePower(port2, newPower * powerSgn);
336	#endif
337	}//end if
338	}//end if
339
340	} else { // no rampup
341
342	// full power right away
343	if (speedreg) {
344	//there's no speedreg for synced driving...
345	UpdatePowerAndEnableSpeedReg(port, power);
346	} else {
347	UpdatePower(port, power);
348	#ifdef RUNMOTOR2_SYNCMODE
349	UpdatePower(port2, power);
350	#endif
351	}//end if
352
353	curStage = STAGE_DRIVING;
354
355	}//end if
356
357	}//end if
358
359	// * if DRIVING STAGE:
360	// - look up current braking distance from current speed
361	// - decide: stage change? time to brake?
362	// - enter braking stage, set braking dist!
363	// - disable speed reg if necessary...
364	// Actually do this during either rampup or driving
365	// (that's the whole idea: braking even if already necessary during rampup)
366
367	//optimized, OLD: if ((curStage == STAGE_DRIVING) \|\| (curStage == STAGE_RAMPUP)) {
368	if (curStage <= STAGE_DRIVING) {
369	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
370	if (curStage == STAGE_DRIVING) {
371	TextOut(0, LCD_LINE8, "DRIVING ");
372	}//end if
373	#endif
374
375	// we've got two different Bremsweg-Algos (sync driving needs a bit more way ahead)
376	#ifdef RUNMOTOR2_SYNCMODE
377	absBrakingDist = GetAbsBrakingDistFromAbsSpeed2(abs(curSpeed));
378	#else
379	absBrakingDist = GetAbsBrakingDistFromAbsSpeed(abs(curSpeed));
380	#endif
381
382
383	// make the following safe for both power signs!
384	if (powerSgn > 0) {
385	// a little dirty preprocessor hack...
386	#ifdef RUNMOTOR2_SYNCMODE
387	if ((tachoTarget - MotorTachoCount(port)) <= absBrakingDist \|\| (tachoTarget - MotorTachoCount(port2)) <= absBrakingDist ) {
388	#else
389	if ((tachoTarget - MotorTachoCount(port)) <= absBrakingDist ) {
390	#endif
391	//~~~~~~~~~~~~~~~~~~ same code from here
392	// ok, we're close enough, start braking!
393	curStage = STAGE_BRAKING;
394	// record this for later down (for speed lookup)
395	brakingStartSpeed = curSpeed;
396	brakingStartTacho = powerSgn * (tacholimit - absBrakingDist);
397	// disable SPEED REG if needed
398	if (speedreg) {
399	// again no need to care about synced driving
400	DisableSpeedRegWhithMotorOn(port);
401	}//end if
402	//~~~~~~~~~~~~~~~~~~ same code until here
403	}//end if
404	} else {
405	#ifdef RUNMOTOR2_SYNCMODE
406	if ((MotorTachoCount(port) - tachoTarget) <= absBrakingDist \|\| (MotorTachoCount(port2) - tachoTarget) <= absBrakingDist) {
407	#else
408	if ((MotorTachoCount(port) - tachoTarget) <= absBrakingDist ) {
409	#endif
410	//~~~~~~~~~~~~~~~~~~ same code from here
411	// ok, we're close enough, start braking!
412	curStage = STAGE_BRAKING;
413	// record this for later down (for speed lookup)
414	brakingStartSpeed = curSpeed;
415	brakingStartTacho = powerSgn * (tacholimit - absBrakingDist);
416	// disable SPEED REG if needed
417	if (speedreg) {
418	// again no need to care about synced driving
419	DisableSpeedRegWhithMotorOn(port);
420	}//end if
421	//~~~~~~~~~~~~~~~~~~ same code until here
422	}//end if
423	}//end if
424
425
426
427	}//end if
428
429	// * if BRAKING STAGE:
430	// ( - check endbraking special mode conditions )
431	// ( . maybe skip PID if time for endgame? )
432	// - PID control
433	// . calc current error
434	// . calc new setpoint
435	// . use KP and KD constants as MACROS (#define), not VARS!
436	// . clip power and set
437	if (curStage == STAGE_BRAKING) {
438	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
439	TextOut(0, LCD_LINE8, "BRAKING ");
440	#endif
441
442	#ifdef ENABLEDEBUGGING_REMOTE
443	if(!brakingStartMarkerSent) {
444	SendMessage(OUTBOX, "1050\|0\|0");
445	brakingStartMarkerSent = true;
446	}//end if
447	#endif// - - - - - - - - - -
448
449
450	// TODO maybe replace this variable (dont use it) and do it inline?
451	desiredSpeed = GetIdealSpeedFromPos(abs(MotorTachoCount(port) - brakingStartTacho), brakingStartSpeed, absBrakingDist);
452
453	// shift/calculate all errors for current cycle
454	err_1 = err_0;
455	err_0 = curSpeed - desiredSpeed; //replace desiredSpeed with inline?
456
457
458	// use different KD, KP consts for sync:
459	#ifdef RUNMOTOR2_SYNCMODE
460	// Our PD-Controller is still dependent on turning direction...
461	if (powerSgn > 0) {
462	curPIDPowerUpscaled = curPIDPowerUpscaled
463	+ (HARDCODED_KD_SYNC * (err_0 - err_1))
464	+ (HARDCODED_KP_SYNC * err_1);
465	} else {
466	curPIDPowerUpscaled = curPIDPowerUpscaled
467	- (HARDCODED_KD_SYNC * (err_0 - err_1))
468	- (HARDCODED_KP_SYNC * err_1);
469	}//end if
470	#else
471	// Our PD-Controller is still dependent on turning direction...
472	if (powerSgn > 0) {
473	curPIDPowerUpscaled = curPIDPowerUpscaled
474	+ (HARDCODED_KD * (err_0 - err_1))
475	+ (HARDCODED_KP * err_1);
476	} else {
477	curPIDPowerUpscaled = curPIDPowerUpscaled
478	- (HARDCODED_KD * (err_0 - err_1))
479	- (HARDCODED_KP * err_1);
480	}//end if
481	#endif
482
483
484	// downscaling
485	newAbsPower = curPIDPowerUpscaled / PIDUPSCALING;
486	// clip these values!
487	if (newAbsPower > maxAbsBrakingPower) {
488	newAbsPower = maxAbsBrakingPower;
489	} else if (newAbsPower < 1) {
490	newAbsPower = 1;
491	}//end if
492
493	//finally set new power!
494	UpdatePower(port, powerSgn * newAbsPower);
495	#ifdef RUNMOTOR2_SYNCMODE
496	UpdatePower(port2, powerSgn * newAbsPower);
497	#endif
498
499
500
501	#ifdef ENABLEDEBUGGING_REMOTE
502	tmp1 = NumToStr(err_0);
503	tmp2 = NumToStr(desiredSpeed);
504	tmp3 = NumToStr(curSpeed);
505	tmp = StrCat(tmp1, "\|", tmp2, "\|", tmp3);
506	SendMessage(OUTBOX, tmp);
507	#endif// - - - - - - - - - -
508
509	}//end if
510
511	// * WAIT
512	// - burn constant CPU time by waiting for max delay...
513	// - keep on checking tachocount for ENDGAME MODE?
514	// - (MAYBE keep on checking direct command cancellation?)
515
516	waitEndTick = loopStartTick + LOOP_DURATION; //precalc loop-end
517	while (CurrentTick() < waitEndTick) {
518
519	//already there? check for endgame mode
520	if (powerSgn > 0) {
521	// this nice little line also detects if the tachotarget is way
522	// out of reach already (not a nice case though)
523	#ifdef RUNMOTOR2_SYNCMODE
524	if ((tachoTarget - MotorTachoCount(port)) <= ENDGAMEBRAKINGSTARTDIST \|\| (tachoTarget - MotorTachoCount(port2)) <= ENDGAMEBRAKINGSTARTDIST) {
525	#else
526	if ((tachoTarget - MotorTachoCount(port)) <= ENDGAMEBRAKINGSTARTDIST) {
527	#endif
528	curStage = STAGE_ENDGAME;
529	#ifdef ENABLEDEBUGGING_ACOUSTIC
530	PlayTone(1000,100);
531	#endif
532	break;
533	}//end if
534	} else {
535	// modified to work for negative stuff...
536	#ifdef RUNMOTOR2_SYNCMODE
537	if ((MotorTachoCount(port) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST \|\| (MotorTachoCount(port2) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST) {
538	#else
539	if ((MotorTachoCount(port) - tachoTarget) <= ENDGAMEBRAKINGSTARTDIST) {
540	#endif
541	curStage = STAGE_ENDGAME;
542	#ifdef ENABLEDEBUGGING_ACOUSTIC
543	PlayTone(1000,100);
544	#endif
545	break;
546	}//end if
547	}//end if
548
549	//TODO decide whether we need this?
550	//direct command stopped us?
551	if ( MotorPower(port) == 0 ) {
552	//TODO is this really a clean direct command exit?
553	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
554	TextOut(0, LCD_LINE8, "ABORTED(wait) ");
555	#endif
556	return;
557	}//end if
558
559	#ifdef SLEEP_DURING_WAIT_WHEN_DRIVING
560	if (curStage <= STAGE_DRIVING) {
561	// free some CPU time...
562	Wait(1);
563	}//end if
564	#endif
565
566	}//end while (times out when static worst case looptime is over (~10 to 20ms)
567
568	// * if SPECIAL ENDGAME BRAKING
569	// - FAST & SIMPLE INNER LOOP!!!
570	// . check position
571	// . enable HARD BRAKE if necessary, jump out of loop
572	if (curStage == STAGE_ENDGAME) {
573	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
574	TextOut(0, LCD_LINE8, "ENDGAME ");
575	#endif
576	#ifdef ENABLEDEBUGGING_REMOTE
577	SendMessage(OUTBOX, "-300\|0\|0");
578	#endif// - - - - - - - - - -
579
580
581	// fast inner loop now!
582	// don't forget TIMEOUT or something, avoid possible lock up,
583	// unfortunately it is possible...
584	waitEndTick = CurrentTick() + ENDGAME_INNERLOOP_TIMEOUT; //precalc loop-end
585	// make two loops to save execution time!
586	if (powerSgn > 0) {
587	while (CurrentTick() < waitEndTick) {
588	#ifdef RUNMOTOR2_SYNCMODE
589	if (MotorTachoCount(port) >= tachoTarget \|\| MotorTachoCount(port2) >= tachoTarget) {
590	#else
591	if (MotorTachoCount(port) >= tachoTarget) {
592	#endif
593	//WE'RE THERE!!!
594	endgameSuccessful = true;
595	break;
596	}//end if
597	#ifdef ENABLEDEBUGGING_LCD
598	endgameLoopCount++;
599	#endif
600	}//end while
601	} else { // negative power sign
602	while (CurrentTick() < waitEndTick) {
603	#ifdef RUNMOTOR2_SYNCMODE
604	if (MotorTachoCount(port) <= tachoTarget \|\| MotorTachoCount(port2) <= tachoTarget) {
605	#else
606	if (MotorTachoCount(port) <= tachoTarget) {
607	#endif
608	//WE'RE THERE!!!
609	endgameSuccessful = true;
610	break;
611	}//end if
612	#ifdef ENABLEDEBUGGING_LCD
613	endgameLoopCount++;
614	#endif
615	}//end while
616	}//end if
617
618	//if succesful or not, BRAKE (before debug)!
619	MotorBrake(port);
620	#ifdef RUNMOTOR2_SYNCMODE
621	MotorBrake(port2);
622	#endif
623
624	//now there's a little time for debug...
625	#ifdef ENABLEDEBUGGING_ACOUSTIC
626	if (!endgameSuccessful) {
627	PlayTone(4000,100);
628	}//end if
629	#endif
630
631	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
632	if (endgameSuccessful) {
633	TextOut(0, LCD_LINE8, "ENDGAME SUCCESS ");
634	} else {
635	TextOut(0, LCD_LINE8, "ENDGAME TIMEOUT ");
636	}//end if
637	#endif
638
639	// and this control process is finally over!
640	break;
641
642	}//end if (stage ENDGAME)
643
644
645	}//end while
646	// --- END MAIN LOOP ---------------------
647
648
649	#ifdef ENABLEDEBUGGING_LCD
650	totalEndTick = CurrentTick();
651	totalControlTime = CurrentTick() - totalStartTick;
652	#endif
653
654	// --- seems obsolete: if loop exited manually by direct command, exit void!
655	// (because clean exit and then return should all be above inside loop)
656
657	// --- monitor braking (as in old MotorControl)
658	// * release when necessary at full stop
659
660	//TODO what happens if during these final and "save" (esasy) stopping/braking
661	// phase, a new direct command arrives and does weired stuff? it will definitely
662	// mess up our current motor state, maybe that's not too bad...
663
664
665	WaitUntilMotorStopped(port);
666
667
668	// --- Motor off / idle, over & out?
669	if (!holdbrake) {
670	MotorOff(port);
671	#ifdef RUNMOTOR2_SYNCMODE
672	MotorOff(port2);
673	#endif
674	}//end if
675
676	// tiny wait for motorstop..., sometimes it really is better...
677	Wait(5);
678
679	#ifdef ENABLEDEBUGGING_REMOTE
680	SendMessage(OUTBOX, "-480\|0\|0");
681	#endif// - - - - - - - - - -
682
683
684
685	// --- Display Debug Info if necessary
686	#ifdef ENABLEDEBUGGING_LCD
687
688
689	lcdTmp = NumToStr(loopCount);
690	lcdTmp = StrCat("loops=", lcdTmp);
691	TextOut(0,LCD_LINE1, lcdTmp, true);
692
693	lcdTmp = NumToStr(totalControlTime / loopCount);
694	lcdTmp = StrCat("t/loop=", lcdTmp);
695	TextOut(0,LCD_LINE2, lcdTmp);
696
697	lcdTmp = NumToStr((loopCount * 1000) / totalControlTime);
698	lcdTmp = StrCat("loops/s=", lcdTmp);
699	TextOut(0,LCD_LINE3, lcdTmp);
700
701	lcdTmp = NumToStr(lastLoopTime);
702	lcdTmp = StrCat("lastloop=", lcdTmp);
703	TextOut(0,LCD_LINE4, lcdTmp);
704
705	lcdTmp1 = NumToStr(MotorTachoCount(port));
706	lcdTmp2 = NumToStr(tacholimit);
707	lcdTmp = StrCat("tacho=", lcdTmp1, "(", lcdTmp2, ")");
708	TextOut(0,LCD_LINE5, lcdTmp);
709
710	// endgame duration & count
711	lcdTmp1 = NumToStr(endgameLoopCount);
712	lcdTmp2 = NumToStr(totalEndTick - (waitEndTick - ENDGAME_INNERLOOP_TIMEOUT));
713	lcdTmp = StrCat("endg=", lcdTmp1, " in ", lcdTmp2, "ms");
714	TextOut(0,LCD_LINE6, lcdTmp);
715
716	#ifdef ENABLEDEBUGGING_WAITAFTERLCD
717	Wait(5000);
718	//until (ButtonPressed(BTNCENTER, false));
719	#endif
720
721	#endif
722
723	#ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
724	TextOut(0, LCD_LINE8, "FINISHED ");
725	#endif
726
727	}//end void
728
729
730	// the following function must be an excact copy-paste-replica of the RunMotor
731	// function from above, but with the name RunMotor2, the additional param port2,
732	// RUNMOTOR2_SYNCMODE defined, and always speedreg=false. It also doesn't need
733	// to be inline (to save space)...
734
735

Note: See TracBrowser for help on using the browser.

Context Navigation

root/branches/atorf/NXC/MotorControl2/ControllerCore.nxc @ 663

Download in other formats: