summaryrefslogtreecommitdiffhomepage
path: root/digital/mimot/src/dirty/models.host.c
blob: efea35bc3e2f6204369054486e82f768153ee07a (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
/* models.host.c */
/* asserv - Position & speed motor control on AVR. {{{
 *
 * Copyright (C) 2006 Nicolas Schodet
 *
 * APBTeam:
 *        Web: http://apbteam.org/
 *      Email: team AT apbteam DOT org
 *
 * 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 2 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, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * }}} */
#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */
#include "common.h"

#include "motor_model.host.h"
#include "models.host.h"
#include "simu.host.h"

#include <math.h>
#include <string.h>

/* Marcel clamp, with a Faulhaber 2342 and 23/1 3.71:1 gearbox model. */
static const struct motor_def_t marcel_clamp_f2342_model =
{
    /* Motor characteristics. */
    366 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
    26.10 / 1000,	/* Torque constant (N.m/A). */
    0,			/* Bearing friction (N.m/(rad/s)). */
    7.10,		/* Terminal resistance (Ohm). */
    0.265 / 1000,	/* Terminal inductance (H). */
    24.0,		/* Maximum voltage (V). */
    /* Gearbox characteristics. */
    3.71,		/* Gearbox ratio. */
    0.88,		/* Gearbox efficiency. */
    /* Load characteristics. */
    0.100 * 0.005 * 0.005,/* Load (kg.m^2). */
    /* This is a pifometric estimation. */
    /* Hardware limits. */
    0.0, +INFINITY,
};

/* Marcel, APBTeam 2010. */
static const struct robot_t marcel_robot =
{
    /** Auxiliary motors, NULL if not present. */
    { &marcel_clamp_f2342_model, &marcel_clamp_f2342_model },
    /** Number of steps for each auxiliary motor encoder. */
    { 256, 256 },
    /** Sensor update function. */
    simu_sensor_update_marcel,
};

/* Table of models. */
static const struct
{
    const char *name;
    const struct robot_t *robot;
} models[] = {
      { "marcel", &marcel_robot },
      { 0, 0 }
};

/** Get a pointer to a model by name, or return 0. */
const struct robot_t *
models_get (const char *name)
{
    int i;
    for (i = 0; models[i].name; i++)
      {
	if (strcmp (models[i].name, name) == 0)
	    return models[i].robot;
      }
    return 0;
}

/** Initialise simulation models. */
void
models_init (const struct robot_t *robot, struct motor_t aux_motor[])
{
    int i;
    if (aux_motor)
      {
	for (i = 0; i < AC_ASSERV_AUX_NB; i++)
	  {
	    if (robot->aux_motor[i])
	      {
		aux_motor[i].m = *robot->aux_motor[i];
		aux_motor[i].h = ECHANT_PERIOD;
		aux_motor[i].d = 1000;
	      }
	  }
      }
}