block_setpoint_generator_3d.c 25.9 KB
Newer Older
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1
/*
2
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007, 2008, 2013, 2014
3
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007, 2008, 2013
Jeroen Vreeken's avatar
Jeroen Vreeken committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */
19
#define _ISOC99_SOURCE
Jeroen Vreeken's avatar
Jeroen Vreeken committed
20
21
22
23
24

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
25
#include <float.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
26

Jeroen Vreeken's avatar
Jeroen Vreeken committed
27
28
29
#include <controller/controller_block.h>
#include <controller/controller_command.h>
#include <log/log.h>
Jeroen Vreeken's avatar
Jeroen Vreeken committed
30
31
32
33
34
35
36

/*
       inputs     outputs
       nr name    nr name

     --------------------------
     |                        |
37
 ----| 0  reset_x 0  x        |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
38
     |                        |
39
 ----| 1  reset   1  v        |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
40
     |                        |
41
     |            2  a        |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
42
     |                        |
43
     |            3  j        |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
44
     |                        |
45
     |            4  setpoint |----
Jeroen Vreeken's avatar
Jeroen Vreeken committed
46
47
48
49
     |                        |
     --------------------------

	'setpoint' is the setpoint as the user requested it.
50
51
52
	'x' is the output of the setpoint generator.
	'v', 'a' and 'j' are the first, second and third
	order derivatives of 'x'.
Jeroen Vreeken's avatar
Jeroen Vreeken committed
53
54
55
56
57
58
59
60
61
62
63
64
65
66

 */

struct spg_command {
	int type;
	float setpoint;
	float speed;
	uint32_t time;
	
	int start;
	int done;
};

struct controller_block_private {
67
68
69
70
71
72
73
74
75
76
77
78
	/*
		In the setpoint generator blocks the following physics notations
		will be used.
	
		x = position	
		v = velocity (1st derivative of x)
		a = acceleration (2nd derivative of x)
		j = jerk  (3th derivative of x)
	*/


	float *reset_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
79
80
	bool  *reset;
	
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
	/* beware: 'samples' is the time unit, not 'seconds', 
	   all parameters and commands are scaled on block entry/exit */
	double cmd_x;
	float  cmd_x_out; /* float version of the internal cmd_x */
	double cmd_v;
	float max_x;
	float min_x;
	float max_v;
	float max_a;
	float inv_max_a;
	float max_j;
	float inv_max_j;
	double precision_x;
	double precision_v;
	double precision_a;

	/* conversion factor for seconds/tick and its inverse */
	float tick;	/* seconds per tick */
	float freq;	/* ticks per second */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
100
101
	float freq2;
	float freq3;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
102
103
	double t_max_a;
	double v_delta_from_max_a;
104
105
106
107
108
109
110
111
112
113

	/* parameters in real world format (time unit: second) */
	float max_v_sec;
	float max_a_sec;
	float max_j_sec;

	float precision_x_sec;
	float precision_v_sec;
	float precision_a_sec;

114
115
116
117
118
119
120
	/* state at last change */
	double start_x;
	double start_v;
	double start_a;
	double start_j;
	int start_t;

121
122
123
124
125
126
127
128
129
	/* current internal state */
	double cur_x;
	double cur_v;
	double cur_a;
	double cur_j;
	float cur_x_out; /* float version of the internal values */
	float cur_v_out;
	float cur_a_out;
	float cur_j_out;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
130
131
	
	
132
133
134
135
136
137
	struct command_entry cur_command;
	bool cur_done;
	bool cur_start;
	uint32_t cur_t_samplenr;

	struct controller_command *command;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
138
139
};

140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165

static bool almost_equal(double d1, double d2)
{
	if (fabs(d1 - d2) <= 16 * DBL_EPSILON * fmax(fabs(d1), fabs(d2)) )
		return true;
	else
		return false;
}

/* How many time ticks are needed to go from a to 0 */
static double ticks_to_a(struct controller_block_private *priv, double a_start, double a_at_t)
{
	double t;
	
	t = fabs(a_start - a_at_t) * priv->inv_max_j;

	return ceil(t);
}

/* time to a constant speed */
static double ticks_to_v(struct controller_block_private *priv, 
    double v_start, double v_at_t)
{
	double t;
	
	/* 	From a constant speed to a constant speed is done in two halves:
Jeroen Vreeken's avatar
Jeroen Vreeken committed
166
167
		First halve (untill half the speed difference is reached) is 
		done at max jerk, second half is done at -max jerk.
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
		
		1/2 v = 1/2 j t^2   ->   v = j t^2   ->  t = sqrt(v/j) 
		
		Second half takes just as long.... return 2*t
	*/
	
	t = sqrt(fabs(v_at_t - v_start) * priv->inv_max_j);
	
	return ceil(t) * 2;
}

static double a_after_ticks(struct controller_block_private *priv, 
    double a, double j, double t)
{
	double a_at_ticks;
	
	a_at_ticks = 
	   a +
	   j * t;

	return a_at_ticks;
}

static double v_after_ticks(struct controller_block_private *priv, 
    double v, double a, double j, double t)
{
	double v_at_ticks;
	
	v_at_ticks = 
	    v +
	    a * t + 
	    1.0/2.0 * j * t * t;

	return v_at_ticks;
}

static double x_after_ticks(struct controller_block_private *priv, 
    double x, double v, double a, double j, double t)
{
	double x_at_t;
	
	x_at_t =
	    x +
	    v * t +
	    1.0/2.0 * a * t * t +
	    1.0/6.0 * j * t * t * t;

	return x_at_t;
}

218
static void setpoint_generator_3d_calculate(struct controller_block *spg)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
219
220
{
	struct controller_block_private *priv = spg->private;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
221
	double cur_x, cur_v;
222
	double t_max_a;
223
	bool ignore_x = false;
224
	bool must_brake = false;
225
226
	bool good_x;
	bool good_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
227

Jeroen Vreeken's avatar
Jeroen Vreeken committed
228
229
230
	cur_x = priv->cur_x;
	cur_v = priv->cur_v;

231
232
	t_max_a = priv->t_max_a;

Jeroen Vreeken's avatar
Jeroen Vreeken committed
233
	if (*priv->reset) {
234
		priv->cmd_x = *priv->reset_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
235
		cur_x = priv->cmd_x;
236
237
		priv->cur_done = true;
		priv->cur_command.type = COMMAND_PTYPE_SETPOINT;
238
		priv->cmd_v = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
239
		cur_v = 0.0;
240
241
		priv->cur_a = 0.0;
		priv->cur_j = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
242
		priv->start_x = cur_x;
243
244
245
246
		priv->start_v = 0.0;
		priv->start_a = 0.0;
		priv->start_j = 0.0;
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
247
248
	}

249
250
251
252
253
254
255
	if (priv->cur_done) {
		int r;
		
		r = controller_command_queue_read(priv->command, &priv->cur_command);
		if (r == 0) {
			priv->cur_done = false;
			priv->cur_start = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
256
		} else {
257
258
			if (priv->cur_command.type ==
			    COMMAND_PTYPE_SETPOINT_TIME) {
259
				priv->cmd_v = 0.0;
260
				priv->cmd_x = priv->cur_command.value.f;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
261
262
263
			}
		}
	}
264
	if (!priv->cur_done) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
265
266
		double t;
			
267
268
269
		switch (priv->cur_command.type) {
			case COMMAND_PTYPE_SETPOINT:
				priv->cmd_x = priv->cur_command.value.f;
270
				priv->cmd_v = 0.0;
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SPEED:
				priv->cmd_v = priv->cur_command.value.f * priv->tick;
				priv->cur_done = true;
				break;
			case COMMAND_PTYPE_SETPOINT_TIME:
				if (!priv->cur_start) {
					double t_samplenr;

					t_samplenr = floor(
					    ((double)priv->cur_command.t.tv_nsec * priv->freq) /
					    1000000.0);
					priv->cur_t_samplenr = t_samplenr;

					t = (double)(priv->cur_command.t.tv_sec
					    - controller_time_seconds) * priv->freq +
					    t_samplenr -
289
					    (double)controller_time_samplenr;
290
291

					if (t <= 0.0) {
Jeroen Vreeken's avatar
Jeroen Vreeken committed
292
293
						/* Command is to old, pretend it
						   is a setpoint without time */
294
295
296
						log_send(LOG_T_WARNING, "%s: setpoint's time is in the past, clock skew? (%lld < %d)",
						    spg->name, (long long)priv->cur_command.t.tv_sec, controller_time_seconds);
						priv->cmd_x = priv->cur_command.value.f;
297
						priv->cmd_v = 0.0;
298
						priv->cur_done = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
299
					} else {
300
						priv->cmd_v =
301
						    (priv->cur_command.value.f -
302
						    priv->cmd_x) / (t+1);
303
						priv->cur_start = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
304
305
306
					}
				}
				
307
				if (priv->cur_command.t.tv_sec <=
Jeroen Vreeken's avatar
Jeroen Vreeken committed
308
				    controller_time_seconds) {
309
					priv->cur_done = true;
310
					priv->cmd_x =
311
					   priv->cur_command.value.f -
312
					   priv->cmd_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
313
				}
314
315
316
				break;
			default:
				break;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
317
318
319
		}
	}

320
	if (priv->cur_command.type == COMMAND_PTYPE_SPEED) {
321
		ignore_x = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
322
		priv->cmd_x = cur_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
323
324
	}
	
325
326
327
328
	if (priv->cmd_x > priv->max_x)
		priv->cmd_x = priv->max_x;
	if (priv->cmd_x < priv->min_x)
		priv->cmd_x = priv->min_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
329
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
330
331
	good_x = almost_equal(cur_x, priv->cmd_x);
	good_v = almost_equal(cur_v, priv->cmd_v);
332
333
334
335
336
337
338

	if (!good_v || !good_x) {
		double error_x;
		double req_x_1;
		double error_x_jpos;
		double error_x_j0;
		double error_x_jneg;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
339
		double error_v = priv->cmd_v - cur_v;
340
		double x, v, a, j, t;
341
		double req_x, req_v, v_delta_from_max_a;
342
		double error_v_after_a, error_x_at_v;
343
		double j_from_pos;
344
345
346
347
348
349
350
		bool state_at_max_a = false;
		bool state_to_max_a = false;
		
		/* procedure: where would we end up if we go to
		   requested speed. 
		 */
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
351
352
		x = cur_x;
		v = cur_v;
353
354
355
356
		a = priv->cur_a;
		req_x = priv->cmd_x;
		req_v = priv->cmd_v;
		
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
		req_x_1 = req_x + req_v;
		
		error_x_jpos = req_x_1 - x_after_ticks(priv, x, v, a, priv->max_j, 1);
		error_x_j0 = req_x_1 - x_after_ticks(priv, x, v, a, 0, 1);
		error_x_jneg = req_x_1 - x_after_ticks(priv, x, v, a, -priv->max_j, 1);
		if (fabs(error_x_jpos) < fabs(error_x_jneg)) {
			error_x = error_x_jpos;
			j_from_pos = priv->max_j;
		} else {
			error_x = error_x_jneg;
			j_from_pos = -priv->max_j;
		}
		if (fabs(error_x_j0) < fabs(error_x)) {
			error_x = error_x_j0;
			j_from_pos = 0;
		}
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
374
		v_delta_from_max_a = priv->v_delta_from_max_a;
375
376

		j = copysign(priv->max_j, error_v);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
377
			
378
		if (fabs(a) >= priv->max_j) {
379
380
381
382
383
384
			/* Not at constant velocity */
		
			if (signbit(a) != signbit(error_v)) {
				/* We are not accelerating towards cmd speed.
				   Go to constant velocity first
				 */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
385
			
386
387
388
389
390
				t = ticks_to_a(priv, a, 0);
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
				a = a_after_ticks(priv, a, j, t);
				req_x = req_x + req_v * t;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
391

392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
				state_to_max_a = true;
			} else {
				/* We are accelerating towards cmd speed. */
				double a_peak;
				double t_to_max_a, t_a, t_at_max_a, t_a_to_0;
				double v_end_max_a, v_start_max_a, v_started_a, v_delta_a;

				/* speed we had when starting acceleration */
				t_a = ticks_to_a(priv, 0, a);
				v_delta_a = v_after_ticks(priv, 0, 0, priv->max_j, t_a);
				v_started_a = v - copysign(v_delta_a, a);

				/* What is the highest a we get? */
				t_a_to_0 = ticks_to_v(priv, v_started_a, req_v) * 0.5;
				a_peak = a_after_ticks(priv, 0, j, t_a_to_0);
				
				if (fabs(a) > fabs(a_peak)) {
					/* We are accelerating much faster than needed acc down first */
					t_a_to_0 = ticks_to_a(priv, a, 0);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
411
				
412
				} else if (fabs(a_peak) > priv->max_a) {
413
					/* We are going to hit maximum acc */
414
					a_peak = copysign(priv->max_a, a_peak);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
415
					
416
417
					t = ticks_to_a(priv, 0, a_peak);
					v_start_max_a = v_after_ticks(priv, v_started_a, 0, j, t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
418
					
419
					v_end_max_a = req_v - copysign(v_delta_from_max_a, error_v);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
420
					
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
					t_at_max_a = fabs(v_end_max_a - v_start_max_a) * priv->inv_max_a;
					t_to_max_a = t - t_a;
					
					if (t_to_max_a >= 1.0) {
						state_to_max_a = true;
						
						x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
						v = v_after_ticks(priv, v, a, j, t_to_max_a);
						a = a_after_ticks(priv, a, j, t_to_max_a);
						req_x = req_x + req_v * t_to_max_a;
					} else {
						state_at_max_a = true;
					}
					x = x_after_ticks(priv, x, v, a, 0, t_at_max_a);
					v = v_after_ticks(priv, v, a, 0, t_at_max_a);
					a = a_after_ticks(priv, a, 0, t_at_max_a);
					req_x = req_x + req_v * t_at_max_a;
					
					t_a_to_0 = t_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
440
				} else {
441
442
443
444
445
					t_to_max_a = t_a_to_0 - t_a;
					
					if (t_to_max_a >= 1.0) {
						state_to_max_a = true;
						
446
						/* accelerate to max a */
447
448
449
450
451
452
453
						x = x_after_ticks(priv, x, v, a, j, t_to_max_a);
						v = v_after_ticks(priv, v, a, j, t_to_max_a);
						a = a_after_ticks(priv, a, j, t_to_max_a);
						req_x = req_x + req_v * t_to_max_a;
					} else {
						state_at_max_a = true;
					}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
454
				}
455
				/* decellerate to a==0 */
456
457
458
459
				x = x_after_ticks(priv, x, v, a, -j, t_a_to_0);
				v = v_after_ticks(priv, v, a, -j, t_a_to_0);
				req_x = req_x + req_v * t_a_to_0;
				a = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
460
			}
461
462
		} else {
			state_to_max_a = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
463
		}
464
465
466
467
468
469
470
471
472
473
474
475
476

		/* We are at constant velocity and need to go to another
		   constant velocity
		 */
		
		error_v_after_a = priv->cmd_v - v;
		
		if (fabs(error_v_after_a) > v_delta_from_max_a * 2) {
			/* Going to hit max a: trapeze 
			        ___
			       /   \
			 */
			double t_at_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
477
			
478
479
			x = x_after_ticks(priv, x, v, 0, j, t_max_a);
			v = v_after_ticks(priv, v, 0, j, t_max_a);
480
			a = copysign(priv->max_a, j);
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
			req_x = req_x + req_v * t_max_a;

			t_at_max_a = (fabs(error_v_after_a) - v_delta_from_max_a*2) * priv->inv_max_a;
			x = x_after_ticks(priv, x, v, a, 0, t_at_max_a);
			v = v_after_ticks(priv, v, a, 0, t_at_max_a);
			a = a_after_ticks(priv, a, 0, t_at_max_a);
			req_x = req_x + req_v * t_at_max_a;

			x = x_after_ticks(priv, x, v, a, -j, t_max_a);
			v = v_after_ticks(priv, v, a, -j, t_max_a);
			req_x = req_x + req_v * t_max_a;
			a = 0;
		} else {
			/* Simple profile: triangle 
			 
			       /\
			 */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
498
			
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
			t = ticks_to_v(priv, 0, error_v_after_a) * 0.5;

			x = x_after_ticks(priv, x, v, 0, j, t);
			v = v_after_ticks(priv, v, 0, j, t);
			a = a_after_ticks(priv, 0, j, t);
			req_x = req_x + req_v * t;

			x = x_after_ticks(priv, x, v, a, -j, t);
			v = v_after_ticks(priv, v, a, -j, t);
			a = a_after_ticks(priv, a, -j, t);
			req_x = req_x + req_v * t;
			a = 0;
		}
		
		error_x_at_v = req_x - x;
		
515
516
517
518
519
		
		if (fabs(error_x_at_v) < fabs(error_x) || 
		    (signbit(error_x) != signbit(error_x_at_v) && !state_to_max_a && !state_at_max_a) ||
		    ignore_x) {
			if (!ignore_x &&
520
			    signbit(error_x_at_v) == signbit(error_x) ) {
521
				priv->cur_j = j_from_pos;
522
523
524
525
526
527
528
			} else {
				if (state_to_max_a) {
					priv->cur_j = j;
				} else if (state_at_max_a) {
					priv->cur_j = 0;
				} else {
					priv->cur_j = -j;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
529
530
531
				}
			}
		} else {
532
533
534
			/* going to requested speed would make position error
			   bigger, first go to position.
			 */
535
			priv->cur_j = j_from_pos;
536
537
538
		}
	}
	
539
	
540
	/* When moving can we brake before the position limits? */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
541
	if (fabs(cur_v) > 0.0) {
542
543
544
		double t, x, v, a, j;
		bool done = false;
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
545
546
		x = cur_x;
		v = cur_v;
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
		a = priv->cur_a;

		/* add 1 tick, we want to know if we are still safe untill the next
		   tick, not only this one 
		 */
		x = x_after_ticks(priv, x, v, a, priv->cur_j, 1);
		v = v_after_ticks(priv, v, a, priv->cur_j, 1);
		a = a_after_ticks(priv, a, priv->cur_j, 1);
		
		j = -copysign(priv->max_j, v);
		
		/* accelerating? */
		if (fabs(priv->cur_a) > 0.0) {
			/* Is our speed still increasing? */
			if (signbit(a) == signbit(v)) {
				/* Reduce a to zero first,
				   then use normal constant v to zero checks */
			
				t = ticks_to_a(priv, a, 0);
			
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
				a = 0;
			} else {
				/* Speed already going down, 
				   should we try to go down faster? */
				if (almost_equal(priv->cur_j, j)) {
					/* We can't do any better... */
					done = true;
				} else if (fabs(a) > priv->max_a - priv->max_j) {
					/* Already at max a, can't do better */
					done = true;
				} else {
					/* reducing a */
					t = ticks_to_a(priv, 0, a);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
582

583
584
585
					x = x_after_ticks(priv, x, v, a, -j, t);
					v = v_after_ticks(priv, v, a, -j, t);
					a = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
586

587
					/* Now at constant v */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
588
589
590
591
				}
			}
		}

592
593
594
595
		if (!done) {
			/* Assuming we have a constant v,
			   check if we have to start deceleration now. */

Jeroen Vreeken's avatar
Jeroen Vreeken committed
596
			t = ticks_to_v(priv, cur_v, 0);
597
598
599
600
		
			/* will we hit max a? */
			if (fabs(a_after_ticks(priv, a, j, t/2)) > priv->max_a) {
				/* 3-part a profile: /-\ */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
601
			
602
				double v_start_3, t_2;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
603
			
Jeroen Vreeken's avatar
Jeroen Vreeken committed
604
				t = t_max_a;
605
606
607
			
				x = x_after_ticks(priv, x, v, a, j, t);
				v = v_after_ticks(priv, v, a, j, t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
608
				
609
610
611
612
613
614
615
616
617
618
619
620
621
622
				/* v at start of 3th period */
				v_start_3 = v_after_ticks(priv, 0, a, j, t);
				
				/* length of 2nd period */
				t_2 = fabs(v - v_start_3) * priv->inv_max_a;
			
				/* period 2 */
				x = x_after_ticks(priv, x, v, a, 0, t_2);
				v = v_after_ticks(priv, v, a, 0, t_2);
			
				/* period 3 */
				x = x_after_ticks(priv, x, v, a, -j, t);
			} else {
				/* 2-part profile: /\ */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
623

624
625
626
				/* first half */
				x = x_after_ticks(priv, x, v, a, j, t/2);
				v = v_after_ticks(priv, v, a, j, t/2);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
627
		
628
629
630
				/* second half, swap j to reduce a and v to 0 */
				x = x_after_ticks(priv, x, v, a, -j, t/2);
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
631
		
632
633
634
			/* once we are still, would we still be within limits? */
			if ((x > priv->max_x || x < priv->min_x) && priv->cur_j != j) {
				priv->cur_j = j;
635
				must_brake = true;
636
			}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
637
638
639
		}
	}

640
	/* If accelerating, can we decelerate before going beyond our max vel */
641
642
	if (fabs(priv->cur_a) > 0.0) {
		double t, v, a, j;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
643
		
Jeroen Vreeken's avatar
Jeroen Vreeken committed
644
		v = cur_v;
645
646
647
648
649
650
651
		a = priv->cur_a;

		/* add 1 tick, we want to know if we are still safe untill the next
		   tick, not only this one 
		 */
		v = v_after_ticks(priv, v, a, priv->cur_j, 1);
		a = a_after_ticks(priv, a, priv->cur_j, 1);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
652
		
653
654
655
656
657
658
659
660
661
662
663
664
665
666
		j = -copysign(priv->max_j, a);
		
		t = ticks_to_a(priv, a, 0);
		
		v = v_after_ticks(priv, v, a, j, t);
		
		/* If not already at the right jerk, set it */
		if (fabs(v) > priv->max_v && signbit(priv->cur_j) != signbit(j)) {
			if (t <= 1) {
				priv->cur_j = 0;
				priv->cur_a = 0;
			} else {
				priv->cur_j = j;
			}
667
			must_brake = true;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
668
669
670
		}
	}

671
672
673
674
675

	if (!ignore_x) {
		priv->cmd_x += priv->cmd_v;
	}

676
677
	/* Is the difference between spg and command small enough?
	   If so, make outputs equal to command */
678
	if (!must_brake &&
Jeroen Vreeken's avatar
Jeroen Vreeken committed
679
680
	    fabs(priv->cmd_x - cur_x) < priv->precision_x &&
	    fabs(priv->cmd_v - cur_v) < priv->precision_v &&
681
682
683
	    fabs(priv->cur_a) < priv->precision_a) {
		priv->cur_j = 0.0;
		priv->cur_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
684
685
		cur_v = priv->cmd_v;
		cur_x = priv->cmd_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
686
687
	}

688
689
690
691
	/* new jerk? */
	if (!almost_equal(priv->cur_j, priv->start_j)) {
		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
692
693
		priv->start_v = cur_v;
		priv->start_x = cur_x;
694
695
696
697
698
699
		priv->start_t = 0;
	}
	priv->start_t++;
	
	priv->cur_a = a_after_ticks(priv, 
	    priv->start_a, priv->start_j, priv->start_t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
700
	cur_v = v_after_ticks(priv, 
701
	    priv->start_v, priv->start_a, priv->start_j, priv->start_t);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
702
	cur_x = x_after_ticks(priv, 
703
704
705
	    priv->start_x, priv->start_v, priv->start_a, priv->start_j, 
	    priv->start_t);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
706

707
708
709
	if (fabs(priv->cur_a) > priv->max_a) {
		priv->cur_a = copysign(priv->max_a, priv->cur_a);
		priv->cur_j = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
710

711
712
		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
713
714
		priv->start_v = cur_v;
		priv->start_x = cur_x;
715
716
		priv->start_t = 0;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
717

Jeroen Vreeken's avatar
Jeroen Vreeken committed
718
	if (fabs(cur_v) >= priv->max_v) {
719
		/* prevent further acceleration beyond max v */
Jeroen Vreeken's avatar
Jeroen Vreeken committed
720
		if (signbit(cur_v) == signbit(priv->cur_a)) {
721
			priv->cur_a = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
722
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
723
		if (signbit(cur_v) == signbit(priv->cur_j)) {
724
			priv->cur_j = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
725
		}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
726
		cur_v = copysign(priv->max_v, cur_v);
727
728
729

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
730
731
		priv->start_v = cur_v;
		priv->start_x = cur_x;
732
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
733
734
	}
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
735
736
737
	if (cur_x > priv->max_x) {
		cur_x = priv->max_x;
		cur_v = 0;
738
739
		priv->cur_a = 0;
		priv->cur_j = 0;
740
741
742

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
743
744
		priv->start_v = cur_v;
		priv->start_x = cur_x;
745
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
746
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
747
748
749
	if (cur_x < priv->min_x) {
		cur_x = priv->min_x;
		cur_v = 0;
750
751
		priv->cur_a = 0;
		priv->cur_j = 0;
752
753
754

		priv->start_j = priv->cur_j;
		priv->start_a = priv->cur_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
755
756
		priv->start_v = cur_v;
		priv->start_x = cur_x;
757
		priv->start_t = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
758
759
	}

Jeroen Vreeken's avatar
Jeroen Vreeken committed
760
761
762
763
	priv->cur_x_out = cur_x;
	priv->cur_v_out = cur_v * priv->freq;
	priv->cur_a_out = priv->cur_a * priv->freq2;
	priv->cur_j_out = priv->cur_j * priv->freq3;
764
	priv->cmd_x_out = priv->cmd_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
765
766
	priv->cur_x = cur_x;
	priv->cur_v = cur_v;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
767
768
}

769
770
static int block_setpoint_generator_command_filter(struct controller_command *command,
    struct command_entry  *entry)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
771
{
772
773
	struct controller_block_private *priv = command->block->private;
	int r = 0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
774
	
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
	switch (entry->type) {
		case COMMAND_PTYPE_SETPOINT_TIME:
		case COMMAND_PTYPE_SETPOINT: {
			float value = entry->value.f;
			
			if (value > priv->max_x)
				value = priv->max_x;
			if (value < priv->min_x)
				value = priv->min_x;
				
			entry->value.f = value;
			break;
		}
		case COMMAND_PTYPE_SPEED: {
			float value = entry->value.f;
			
			if (fabs(value) > priv->max_v_sec)
				value = copysign(priv->max_v_sec, value);
			
			entry->value.f = value;
			
			break;
		}
		default:
			r = -1;
			break;
	}
Jeroen Vreeken's avatar
Jeroen Vreeken committed
802

803
	return r;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
804
805
806
}

static struct controller_block_param_list params[] = {
807
808
809
810
811
812
813
814
815
816
	{ "setpoint",    true  },
	{ "t",           true  },
	{ "max_x",       true  },
	{ "min_x",       true  },
	{ "max_v",       true  },
	{ "max_a",       true  },
	{ "max_j",       true  },
	{ "precision_x", true  },
	{ "precision_v", true  },
	{ "precision_a", true  },
Jeroen Vreeken's avatar
Jeroen Vreeken committed
817
818
819
820
821
822
823
824
	{ NULL },
};

static void param_get(struct controller_block *spg, int param, void *val)
{
	switch (param) {
		case 0:
			*(float*)val =
825
			    spg->private->cmd_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
826
827
828
			break;
		case 1:
			*(float*)val =
829
			    spg->private->tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
830
831
832
			break;
		case 2:
			*(float*)val =
833
			    spg->private->max_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
834
835
836
			break;
		case 3:
			*(float*)val =
837
			    spg->private->min_x;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
838
839
840
			break;
		case 4:
			*(float*)val =
841
			    spg->private->max_v_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
842
843
844
			break;
		case 5:
			*(float*)val =
845
			    spg->private->max_a_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
846
847
848
			break;
		case 6:
			*(float*)val =
849
			    spg->private->max_j_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
850
851
852
			break;
		case 7:
			*(float*)val =
853
			    spg->private->precision_x_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
854
855
856
			break;
		case 8:
			*(float*)val =
857
			    spg->private->precision_v_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
858
859
860
			break;
		case 9:
			*(float*)val =
861
			    spg->private->precision_a_sec;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
862
863
864
865
866
867
			break;
	}
}

static void param_set(struct controller_block *spg, int param, va_list val)
{
Jeroen Vreeken's avatar
Jeroen Vreeken committed
868
869
	double t_max_a;
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
870
871
	switch (param) {
		case 0:
872
873
			spg->private->cmd_x = va_arg(val, double);
			spg->private->cur_x = spg->private->cmd_x;
874
			spg->private->cur_done = true;
875
876
877
878
			spg->private->cmd_v = 0.0;
			spg->private->cur_v = 0.0;
			spg->private->cur_a = 0.0;
			spg->private->cur_j = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
879
880
			break;
		case 1:
881
			spg->private->tick = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
882
883
			break;
		case 2:
884
			spg->private->max_x = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
885
886
			break;
		case 3:
887
			spg->private->min_x = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
888
889
			break;
		case 4:
890
			spg->private->max_v_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
891
892
			break;
		case 5:
893
			spg->private->max_a_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
894
895
			break;
		case 6:
896
			spg->private->max_j_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
897
898
			break;
		case 7:
899
			spg->private->precision_x_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
900
901
			break;
		case 8:
902
			spg->private->precision_v_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
903
904
			break;
		case 9:
905
			spg->private->precision_a_sec = va_arg(val, double);
Jeroen Vreeken's avatar
Jeroen Vreeken committed
906
907
			break;
	}
908
909
910
911
912
913
914
915
916
917

	/* Scale all settings to sample time unit */
	spg->private->max_v = spg->private->max_v_sec * spg->private->tick;
	spg->private->max_a = spg->private->max_a_sec * spg->private->tick * spg->private->tick;
	spg->private->max_j = spg->private->max_j_sec * spg->private->tick * spg->private->tick * spg->private->tick;
	spg->private->precision_x = spg->private->precision_x_sec;
	spg->private->precision_v = spg->private->precision_v_sec * spg->private->tick;
	spg->private->precision_a = spg->private->precision_a_sec * spg->private->tick * spg->private->tick;

	spg->private->freq = 1.0 / spg->private->tick;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
918
919
	spg->private->freq2 = spg->private->freq * spg->private->freq;
	spg->private->freq3 = spg->private->freq2 * spg->private->freq;
920
921
	spg->private->inv_max_j = 1.0 / spg->private->max_j;
	spg->private->inv_max_a = 1.0 / spg->private->max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
922
923
924
925
926
927

	/* Calculate delta v when comming from max_a */
	t_max_a = ticks_to_a(spg->private, 0, spg->private->max_a);
	spg->private->v_delta_from_max_a = 
	    v_after_ticks(spg->private, 0, 0, spg->private->max_j, t_max_a);
	spg->private->t_max_a = t_max_a;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
928
929
}

930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
static struct controller_block_interm_list interms[] = {
	{ "reset_x", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, reset_x) },
	{ "reset",   CONTROLLER_BLOCK_TERM_BOOL,  offsetof(struct controller_block_private, reset)   },
	{ NULL }
};

static struct controller_block_outterm_list outterms[] = {
	{ "x",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_x_out) },
	{ "v",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_v_out) },
	{ "a",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_a_out) },
	{ "j",        CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cur_j_out) },
	{ "setpoint", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, cmd_x_out) },
	{ NULL }
};


946
struct controller_block * block_setpoint_generator_3d_create(char *name, va_list ap)
Jeroen Vreeken's avatar
Jeroen Vreeken committed
947
948
{
	struct controller_block *spg;
949
950
951
952
953
954
	char *server_name;
	char *spg_unit;
	
	server_name = va_arg(ap, char *);
	spg_unit = va_arg(ap, char *);

Jeroen Vreeken's avatar
Jeroen Vreeken committed
955
956
957
958
	spg = malloc(sizeof(struct controller_block));
	if (!spg)
		return NULL;

959
	spg->type = "setpoint_generator_3d";
Jeroen Vreeken's avatar
Jeroen Vreeken committed
960
961
962
963
964
965
966
967
	spg->name = malloc(strlen(name)+1);
	if (!spg->name)
		goto err_spg;
	strcpy(spg->name, name);

	spg->private = malloc(sizeof(struct controller_block_private));
	if (!spg->private)
		goto err_name;
968
969
970
971
972
973
974
975
976
977
978
979
980
981
	spg->private->cmd_x = 0.0;
	spg->private->cmd_v = 0.0;
	spg->private->tick = 1.0;
	spg->private->max_x = 0.0;
	spg->private->min_x = 0.0;
	spg->private->max_v = 0.0;
	spg->private->max_a = 0.0;
	spg->private->inv_max_a = 1.0;
	spg->private->max_j = 0.0;
	spg->private->inv_max_j = 1.0;
	spg->private->max_v_sec = 0.0;
	spg->private->max_a_sec = 0.0;
	spg->private->max_j_sec = 0.0;
	spg->private->freq = 1.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
982
983
	spg->private->freq2 = 1.0;
	spg->private->freq3 = 1.0;
984
985
986
987
988
989
990
991
992
993
994
	spg->private->precision_x = 0.0;
	spg->private->precision_v = 0.0;
	spg->private->precision_a = 0.0;
	spg->private->precision_x_sec = 0.0;
	spg->private->precision_v_sec = 0.0;
	spg->private->precision_a_sec = 0.0;

	spg->private->cur_x = 0.0;
	spg->private->cur_v = 0.0;
	spg->private->cur_a = 0.0;
	spg->private->cur_j = 0.0;
995
996
997
998
999
	spg->private->start_x = 0.0;
	spg->private->start_v = 0.0;
	spg->private->start_a = 0.0;
	spg->private->start_j = 0.0;
	spg->private->start_t = 0;
1000
1001
1002
1003
1004
	spg->private->cur_x_out = 0.0;
	spg->private->cur_v_out = 0.0;
	spg->private->cur_a_out = 0.0;
	spg->private->cur_j_out = 0.0;
	spg->private->cmd_x_out = 0.0;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1005

1006
	if (controller_block_interm_list_init(spg, interms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1007
		goto err_private;
1008
1009

	if (controller_block_outterm_list_init(spg, outterms))
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1010
1011
		goto err_input;

1012
	spg->calculate = setpoint_generator_3d_calculate;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1013
1014
1015
1016
1017
1018
1019

	if (controller_block_param_list_init(spg, params))
		goto err_output;

	spg->param_get = param_get;
	spg->param_set = param_set;

1020
1021
	spg->private->cur_done = true;
	spg->private->cur_start = false;
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1022
1023

	controller_block_add(spg);
1024

1025
1026
1027
1028
1029
1030
1031
1032
	spg->private->command = controller_command_create(spg, 
	    server_name, spg_unit);
	spg->private->command->value_type = COMMAND_VALUE_TYPE_FLOAT;
	spg->private->command->command_types[0] = COMMAND_PTYPE_SETPOINT;
	spg->private->command->command_types[1] = COMMAND_PTYPE_SPEED;
	spg->private->command->command_types[2] = COMMAND_PTYPE_SETPOINT_TIME;
	spg->private->command->filter =
	    block_setpoint_generator_command_filter;
1033
	
Jeroen Vreeken's avatar
Jeroen Vreeken committed
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
	return spg;

err_output:
	free(spg->output);
err_input:
	free(spg->input);
err_private:
	free(spg->private);
err_name:
	free(spg->name);
err_spg:
	free(spg);
	return NULL;
}