Subversion Repositories shark

Rev

Rev 1331 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1331 giacomo 1
 
2
/*
3
 * Project: S.Ha.R.K.
4
 *
5
 * Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
6
 *
7
 * Authors     : Paolo Gai <pj@hartik.sssup.it>
8
 * (see authors.txt for full list of hartik's authors)
9
 *
10
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
11
 *
12
 * http://www.sssup.it
13
 * http://retis.sssup.it
14
 * http://shark.sssup.it
15
 */
16
 
17
/*
18
 * Copyright (C) 2000 Paolo Gai
19
 *
20
 * This program is free software; you can redistribute it and/or modify
21
 * it under the terms of the GNU General Public License as published by
22
 * the Free Software Foundation; either version 2 of the License, or
23
 * (at your option) any later version.
24
 *
25
 * This program is distributed in the hope that it will be useful,
26
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
27
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
28
 * GNU General Public License for more details.
29
 *
30
 * You should have received a copy of the GNU General Public License
31
 * along with this program; if not, write to the Free Software
32
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
33
 *
34
 */
35
 
36
#include "chimera.h"
37
 
38
//#define DEBUG_SEND            /* Print Sent Values */
39
#define SERIAL_ON               /* Send Data using Serial Port */
40
 
41
/* Servo Tasks Constants */
42
#ifdef DUBUG_SEND
43
#define SEND_TASK_WCET  6000
44
#else
1332 giacomo 45
#define SEND_TASK_WCET  6000
1331 giacomo 46
#endif
47
#define SEND_TASK_MIT  20000
48
 
49
#ifdef DUBUG_SEND
1332 giacomo 50
#define GET_TASK_WCET   6000
1331 giacomo 51
#else
1332 giacomo 52
#define GET_TASK_WCET   6000
1331 giacomo 53
#endif
54
#define GET_TASK_MIT   20000
55
 
56
#define LEG_A 100.0
57
#define LEG_B  66.0
58
#define LEG_C  26.0
59
#define LEG_D  38.0
60
#define LEG_CD_2IPO 92.087      /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
61
#define LEG_CD_ANG  34.380      /* arctg(LEG_C/LEG_D) in gradi */
62
 
63
const float c0 = LEG_C * LEG_C;
64
const float c1 = LEG_B * LEG_B;
65
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
66
const float todeg = 180.0 / PI;
67
 
68
sem_t           mx_status, mx_servo;
69
HEXAPOD_STATE   status;
70
ANGLES_STATE    angles[6];
71
 
1332 giacomo 72
extern struct leg_calibration calibration_table[];
73
 
1331 giacomo 74
void print_status(int n){
75
        int i;
76
 
77
        if (n>=6) {
78
                for (i=0; i<6;i++)
79
                        cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.leg[i].pwm );
80
                cprintf("Power: %d", status.power);
81
                cprintf("\n");
82
        } else {
83
                cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.leg[n].pwm, status.power);
84
        }
85
}
86
 
87
int set_leg_position(int n, float px, float py, float pz)
88
{
89
        float px2 = px * px;
90
        float py2 = py * py;
91
        float pz2 = pz * pz;
92
 
93
        float pxz2 = px2 + pz2;
94
 
95
        float alfa1,beta1,alfa2,beta2,gamma1;
96
        float m,dsqrt;
97
 
98
        float delta_xz = pxz2 - c0;
99
        float s,k,k2,y1,delta_xy;
100
 
101
        if (delta_xz < 0.0) return -1;
102
 
103
        if (pz >= LEG_C) {
104
                gamma1 =  acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
105
        } else {
106
                gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
107
        }
108
 
109
        if (gamma1 < -90.0 || gamma1 > 90.0) return -1;
110
 
111
        m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG));
112
 
113
        s = m + py2;
114
        k = c2 + s;
115
        k2 = k * k;
116
 
117
        delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
118
 
119
        if (delta_xy >= 0.0) {
120
                dsqrt = sqrt(delta_xy);
121
                y1 = (py * k + dsqrt) / (2.0 * s);
122
                beta1 = asin(y1/LEG_B) * todeg;
123
                alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
124
                y1 = (py * k - dsqrt) / (2.0 * s);
125
                beta2 = asin(y1/LEG_B) * todeg;
126
                alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
127
 
128
                if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
129
                        angles[n].a = (int)(alfa1 * 3600.0);
130
                        angles[n].b = (int)(beta1 * 3600.0);
131
                        angles[n].c = (int)(gamma1 * 3600.0);
132
#ifdef  DEBUG_SEND
133
                        cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
134
#endif
135
                        return 0;
136
                } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
137
                        angles[n].a = (int)(alfa2 * 3600.0);
138
                        angles[n].b = (int)(beta2 * 3600.0);
139
                        angles[n].c = (int)(gamma1 * 3600.0);
140
#ifdef  DEBUG_SEND
141
                        cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
142
#endif
143
                        return 0;
144
                } else {
145
#ifdef  DEBUG_SEND
146
                        cprintf("No possible !\n");
147
#endif
148
                        return 1;
149
                }
150
        } else
151
                return 1;
152
 
153
        return 0;
154
}
155
 
156
TASK servo_send()
157
{
158
        HEXAPOD_STATE   old_status;
159
        register char   changes, new_pos, new_pwm, new_power;
160
        int             n;
161
 
162
        for (n=0; n<6;n++) {
163
                old_status.leg[n].x = 150;
164
                old_status.leg[n].y =   0;
165
                old_status.leg[n].z =   0;
166
                old_status.leg[n].pwm = 0;
167
        }
168
        old_status.power = 0;
169
 
170
        while (1) {
171
                changes = 0;
172
 
173
                for (n=0; n<6; n++){
174
                        new_pos = 0;
175
                        new_pwm = 0;
176
                        new_power = 0;
177
 
178
                        //sem_wait(&mx_status);
179
                        if ((status.leg[n].x != old_status.leg[n].x) || (status.leg[n].y != old_status.leg[n].y) || (status.leg[n].z != old_status.leg[n].z)) {
180
                                if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) {
181
                                        old_status.leg[n].x = status.leg[n].x;
182
                                        old_status.leg[n].y = status.leg[n].y;
183
                                        old_status.leg[n].z = status.leg[n].z;
184
                                        new_pos++;
185
                                } else {
186
#ifdef  DEBUG_SEND
187
                                        cprintf("Position not reachable.\n");
188
#endif
189
                                        status.leg[n].x = old_status.leg[n].x;
190
                                        status.leg[n].y = old_status.leg[n].y;
191
                                        status.leg[n].z = old_status.leg[n].z;
192
                                }
193
                        }
194
                        if (status.leg[n].pwm != old_status.leg[n].pwm) {
195
                                old_status.leg[n].pwm = status.leg[n].pwm;
196
                                new_pwm++;
197
                        }
198
                        if (status.power != old_status.power) {
199
                                old_status.power = status.power;
200
                                new_power++;
201
                        }
202
                        //sem_post(&mx_status);
203
 
204
                        if (new_pos) {
205
#ifdef  SERIAL_ON
206
                                sem_wait(&mx_servo);
1332 giacomo 207
                                servo_set_angle_sec(COM_PORT, n*3+2, adjust(angles[n].a,n,0));
208
                                servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1));
209
                                servo_set_angle_sec(COM_PORT, n*3  , adjust(angles[n].c,n,2));
1331 giacomo 210
                                sem_post(&mx_servo);
1332 giacomo 211
 
212
                                printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x)
213
                                                                                ,(int)(status.leg[n].y)
214
                                                                                ,(int)(status.leg[n].z));
215
 
216
                                printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a)
217
                                                                                ,(int)(angles[n].b)
218
                                                                                ,(int)(angles[n].c));
219
 
220
                                printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0),adjust(angles[n].b,n,1),adjust(angles[n].c,n,2));
221
 
1331 giacomo 222
#endif
223
                        }
224
 
225
                        if (new_pwm) {
226
#ifdef  SERIAL_ON
227
                                sem_wait(&mx_servo);
228
                                (old_status.leg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2);
229
                                (old_status.leg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1);
230
                                (old_status.leg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3  ) : servo_turn_off(COM_PORT, n*3  );
231
                                sem_post(&mx_servo);
232
#endif
233
                        }
234
                        if (new_power) {
235
#ifdef  SERIAL_ON
236
                                if (old_status.power) {
237
                                        sem_wait(&mx_servo);
238
                                        servo_set_RC5_switch(COM_PORT, 1);
239
                                        sem_post(&mx_servo);
240
                                } else {
241
                                        sem_wait(&mx_servo);
242
                                        servo_set_RC5_switch(COM_PORT, 0);
243
                                        sem_post(&mx_servo);
244
                                }
245
#endif
246
                        }
247
 
248
                        if (new_pos || new_pwm || new_power) {
249
                                changes++;
250
#ifdef  DEBUG_SEND
251
                                print_status(n);
252
#endif
253
                        }
254
                }
255
                task_endcycle();
256
        }
257
 
258
        return 0;
259
}
260
 
261
TASK servo_get()
262
{
263
        int     i = 0;
264
 
265
        while (1) {
266
#ifdef  SERIAL_ON
267
                //sem_wait(&mx_status);
268
                sem_wait(&mx_servo);
269
                status.leg[i  ].adc_in = servo_get_analog(COM_PORT, i  );
270
                status.leg[i+1].adc_in = servo_get_analog(COM_PORT, i+1);
271
                sem_post(&mx_servo);
272
#ifdef  DEBUG_SEND
273
                cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in);
274
#endif
275
                i = (i+2)%6;
276
                //sem_post(&mx_status);
277
#endif
278
                task_endcycle();
279
        }
280
 
281
        return 0;
282
}
283
 
284
int init_serial()
285
{
286
        int err;
1332 giacomo 287
        err = servo_open(COM_PORT, 19200);
1331 giacomo 288
 
289
        return err;
290
}
291
 
292
void end_serial()
293
{
294
        servo_close(COM_PORT);
295
}
296
 
297
void init_send_task()
298
{
1332 giacomo 299
        SOFT_TASK_MODEL ms;
1331 giacomo 300
        PID pid;
301
 
1332 giacomo 302
        soft_task_default_model(ms);
303
        soft_task_def_ctrl_jet(ms);
304
        soft_task_def_met(ms, SEND_TASK_WCET);
305
        soft_task_def_period(ms, SEND_TASK_MIT);
306
        soft_task_def_usemath(ms);
1331 giacomo 307
        pid = task_create("Send_Task", servo_send, &ms, NULL);
308
        if (pid == NIL) {
309
                perror("Could not create task <Send_Task>");
310
                sys_end();
311
        } else
312
                task_activate(pid);
313
 
1332 giacomo 314
        soft_task_default_model(ms);
315
        soft_task_def_ctrl_jet(ms);
316
        soft_task_def_met(ms, GET_TASK_WCET);
317
        soft_task_def_period(ms, GET_TASK_MIT);
318
        soft_task_def_usemath(ms);
1331 giacomo 319
        pid = task_create("Get_Task", servo_get, &ms, NULL);
320
        if (pid == NIL) {
321
                perror("Could not create task <Get_Task>");
322
                sys_end();
323
        } else
324
                task_activate(pid);
325
}
326
 
327
void init_send()
328
{
329
        int i;
330
 
331
        if (init_serial()) {
332
                perror("Could not initialize serial port.");
333
                sys_end();
334
        }
335
 
336
        //sem_init(&mx_status,0,1);
337
        sem_init(&mx_servo,0,1);
338
 
339
        for (i=0; i<6;i++) {
340
                status.leg[i].x = 150;
341
                status.leg[i].y =   0;
342
                status.leg[i].z =   0;
343
                status.leg[i].pwm = 0;
344
        }
345
        status.power = 0;
346
 
347
        init_send_task();
348
}
349
 
350
void end_send()
351
{
352
        end_serial();
353
}