summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/simu.host.c
blob: 1035af10d060a689acff8a8b51f76d026a3f570b (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
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
/* simu.host.c */
/*  {{{
 *
 * Copyright (C) 2006 Nicolas Schodet
 *
 * 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.
 *
 * Contact :
 *        Web: http://perso.efrei.fr/~schodet/
 *      Email: <contact@ni.fr.eu.org>
 * }}} */
#include "common.h"
#include "modules/host/host.h"
#include "simu.host.h"
#include "motor_model.host.h"
#include "models.host.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

/** Simulate some AVR regs. */
uint8_t DDRD, PORTD, PORTA, PORTC, PINA;

/** Overall counter values. */
uint16_t counter_left, counter_right;
/** Counter differences since last update.
 * Maximum of 9 significant bits, sign included. */
int16_t counter_left_diff, counter_right_diff;

/** PWM values, this is an error if absolute value is greater than the
 * maximum. */
int16_t pwm_left, pwm_right;
/** PWM reverse direction, only set pwm dir bits or you will get weird results
 * on port B. */
uint8_t pwm_dir;

struct motor_t simu_left_model, simu_right_model;

/** Initialise simulation. */
static void
simu_init (void)
{
    int argc;
    char **argv;
    const struct motor_t *m;
    host_get_program_arguments (&argc, &argv);
    if (argc != 1)
      {
	fprintf (stderr, "need model name as first argument\n");
	exit (1);
      }
    m = models_get (argv[0]);
    if (!m)
      {
	fprintf (stderr, "unknown model name: %s\n", argv[0]);
	exit (1);
      }
    simu_left_model = *m;
    simu_right_model = *m;
}

/** Do a simulation step. */
static void
simu_step (void)
{
    double old_left_th, old_right_th;
    /* Convert pwm value into voltage. */
    assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX);
    assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX);
    simu_left_model.u = (double) (pwm_left + 1) / (PWM_MAX + 1);
    simu_right_model.u = (double) (pwm_right + 1) / (PWM_MAX + 1);
    /* Make one step. */
    old_left_th = simu_left_model.th;
    old_right_th = simu_right_model.th;
    motor_model_step (&simu_left_model);
    motor_model_step (&simu_right_model);
    /* Modifie counters. */
    counter_left_diff = (simu_left_model.th - old_left_th) / M_2_PI * 500 *
	simu_left_model.i_G;
    counter_left += counter_left_diff;
    counter_right_diff = (simu_right_model.th - old_right_th) / M_2_PI * 500 *
	simu_right_model.i_G;
    counter_right += counter_right_diff;
}

/** Initialise the timer. */
void
timer_init (void)
{
    simu_init ();
}

/** Wait for timer overflow. */
void
timer_wait (void)
{
    simu_step ();
}

/** Read timer value. Used for performance analysis. */
uint8_t
timer_read (void)
{
    return 0;
}

/** Initialize the counters. */
void
counter_init (void)
{
}

/** Update overall counter values and compute diffs. */
void
counter_update (void)
{
}

/** Restart counting. */
void
counter_restart (void)
{
}

/** Initialise PWM generator. */
void
pwm_init (void)
{
}

/** Update the hardware PWM values. */
void
pwm_update (void)
{
}