Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1624 giacomo 1
/*
2
 * Project: S.Ha.R.K.
3
 *
4
 * Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
5
 *
6
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
7
 *
8
 * http://www.sssup.it
9
 * http://retis.sssup.it
10
 * http://shark.sssup.it
11
 */
12
 
13
#include "chimera.h"
14
#include "tracer.h"
15
#include "unistd.h"
16
 
17
//#define DEBUG_SEND            /* Print Sent Values */
18
#define SERIAL_ON               /* Send Data using Serial Port */
19
 
20
/* Servo Tasks Constants */
21
#ifdef DUBUG_SEND
22
#define SEND_TASK_WCET 26000
23
#else
24
#define SEND_TASK_WCET 26000
25
#endif
26
#define SEND_TASK_MIT  30000
27
 
28
#define LEG_A 100.0
29
#define LEG_B  66.0
30
#define LEG_C  26.0
31
#define LEG_D  38.0
32
#define LEG_CD_2IPO 92.087      /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
33
#define LEG_CD_ANG  0.600       /* arctg(LEG_C/LEG_D) in radianti */
34
 
35
const float c0 = LEG_C * LEG_C;
36
const float c1 = LEG_B * LEG_B;
37
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
38
const float todeg = 180.0 / PI;
39
const float torad = PI / 180.0;
40
 
41
HEXAPOD_STATE   status;
42
 
43
void *start_tracer = NULL;
44
void *end_tracer = NULL;
45
void *trace_pointer = NULL;
46
 
47
extern struct leg_calibration calibration_table[];
48
 
49
void print_status(int n){
50
}
51
 
52
int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma)
53
{
54
        float px2 = px * px;
55
        float py2 = py * py;
56
        float pz2 = pz * pz;
57
 
58
        float pxz2 = px2 + pz2;
59
 
60
        float alfa1,beta1,alfa2,beta2,gamma1,gamma2;
61
        float m,dsqrt;
62
 
63
        float delta_xz = pxz2 - c0;
64
        float s,k,k2,y1,delta_xy;
65
 
66
        if (delta_xz < 0.0) return -1;
67
 
68
        if (pz >= LEG_C) {
69
                gamma2 =  acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
70
                gamma1 = gamma2 * todeg;
71
        } else {
72
                gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
73
                gamma1 = gamma2 * todeg;
74
        }
75
 
76
        m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0);
77
 
78
        s = m + py2;
79
        k = c2 + s;
80
        k2 = k * k;
81
 
82
        delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
83
 
84
        if (delta_xy >= 0.0) {
85
                dsqrt = sqrt(delta_xy);
86
                y1 = (py * k + dsqrt) / (2.0 * s);
87
                beta1 = asin(y1/LEG_B) * todeg;
88
                alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
89
                y1 = (py * k - dsqrt) / (2.0 * s);
90
                beta2 = asin(y1/LEG_B) * todeg;
91
                alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
92
 
93
                if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
94
                        *alfa = (int)(alfa1 * 3600.0);
95
                        *beta = (int)(beta1 * 3600.0);
96
                        *gamma = (int)(gamma1 * 3600.0);
97
                        return 0;
98
                } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
99
                        *alfa = (int)(alfa2 * 3600.0);
100
                        *beta = (int)(beta2 * 3600.0);
101
                        *gamma = (int)(gamma1 * 3600.0);
102
                        return 0;
103
                } else {
104
                        return -1;
105
                }
106
        } else
107
                return -1;
108
 
109
        return -1;
110
 
111
}
112
 
113
int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) {
114
 
115
  float alfa1 = (float)(alfa)/3600.0 * torad;
116
  float beta1 = (float)(beta)/3600.0 * torad;
117
  float sin_gamma = sin((float)(gamma)/3600.0 * torad);
118
  float cos_gamma = cos((float)(gamma)/3600.0 * torad);
119
  float m;
120
 
121
  m =  LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1);
122
  *py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1);
123
 
124
  *pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma;
125
  *px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma;
126
 
127
  return 0;
128
 
129
}
130
 
131
void update_event_action(void) {
132
 
133
  struct timespec t;
134
  struct action_event *e;
135
  int i;
136
 
137
  kern_gettime(&t);
138
 
139
  while ((e = get_first_old_event(&t)) != NULL) {
140
 
141
        if (e->type == EVT_SET_MASK_LEG_ANGLE) {
142
 
143
                for (i=0;i<6;i++)
144
                        if ((e->mask >> i) & 1) {
145
 
146
                        status.ang[i].a = e->ang.a;
147
                        status.ang[i].b = e->ang.b;
148
                        status.ang[i].c = e->ang.c;
149
 
150
                        status.cfg[i].pwm = e->pwm;
151
 
152
#ifdef  DEBUG_SEND
153
                        printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i);
154
#endif
155
 
156
 
157
                        }
158
 
159
                e->status = EVT_STATUS_DONE;
160
 
161
        }
162
 
163
  }
164
 
165
}
166
 
167
int trace_init(int buffer_size)
168
{
169
 
170
        start_tracer = trace_pointer = (void *)malloc(buffer_size);
171
        end_tracer = start_tracer + buffer_size;
172
        memset(trace_pointer,0,buffer_size);
173
 
174
        if (trace_pointer == NULL) return -1;
175
 
176
        return 0;
177
}
178
 
179
/* Init the network stack */
180
int init_network(char *local_ip)
181
{
182
 
183
        struct net_model m = net_base;
184
 
185
        net_setudpip(m, local_ip, "255.255.255.255");
186
 
187
        if (net_init(&m) != 1) {
188
                cprintf("Network: Init Error.\n");
189
                return -1;
190
        }
191
 
192
        return 0;
193
 
194
}
195
 
196
int trace_consumption(int sensor, int value)
197
{
198
 
199
        struct timespec t;
200
        SYS_FLAGS f;
201
 
202
        if (trace_pointer == NULL) return -1;
203
 
204
        if (trace_pointer  >= (end_tracer-16)) return -1;
205
 
206
        f = kern_fsave();
207
 
208
        sys_gettime(&t);
209
 
210
        *(unsigned int *)trace_pointer = (sensor & 0xFF) | 0xAABBFF00;
211
        *(unsigned int *)(trace_pointer + 4) = value;
212
        *(unsigned int *)(trace_pointer + 8) = t.tv_sec;
213
        *(unsigned int *)(trace_pointer + 12) = t.tv_nsec;     
214
 
215
        trace_pointer += 16;
216
 
217
        kern_frestore(f);
218
 
219
        return 0;
220
 
221
}
222
 
223
int trace_send() {
224
 
225
        static char pkg_buffer[1100];
226
        int actual = 0;
227
 
228
        UDP_ADDR target, local;
229
        char local_ip[20], target_ip[20];
230
        int socket;
231
        IP_ADDR bindlist[5];
232
 
233
        strcpy(local_ip,  "192.168.0.99");
234
        strcpy(target_ip, "192.168.0.104");
235
 
236
        if (init_network(local_ip)) sys_end();
237
 
238
        /* local IP string to addr */
239
        ip_str2addr(local_ip,&(local.s_addr));
240
        /* set the source port */
241
        local.s_port = 20000;
242
 
243
        /* target IP string to addr */
244
        ip_str2addr(target_ip,&(bindlist[0]));
245
        memset(&(bindlist[1]), 0, sizeof(IP_ADDR));
246
        /* bind */
247
        socket = udp_bind(&local, NULL);
248
 
249
        /* target IP string to addr */
250
        ip_str2addr(target_ip,&(target.s_addr));
251
        /* target port */
252
        target.s_port = 20000;
253
 
254
        trace_pointer = start_tracer;
255
        actual = 0;
256
 
257
        while(trace_pointer < end_tracer) {
258
 
259
                if (((*(int *)(trace_pointer) >> 8) & 0xAABBFF) == 0xAABBFF) {
260
 
261
                        cprintf(".");
262
 
263
                        memcpy(&(pkg_buffer[actual]),trace_pointer,16);
264
 
265
                        actual += 16;
266
 
267
                        if (actual > 800) {
268
                                pkg_buffer[actual] = 0;
269
                                cprintf("X");
270
                                udp_sendto(socket, pkg_buffer, actual, &target);
271
                                usleep(100000);
272
                                actual = 0;
273
                        }
274
 
275
                }
276
 
277
                trace_pointer += 16;
278
 
279
        }
280
 
281
        pkg_buffer[actual] = 0;
282
        cprintf("X");
283
        udp_sendto(socket, pkg_buffer, actual+1, &target);
284
        usleep(10000);
285
        actual = 0;
286
 
287
        return 0;
288
 
289
}
290
 
291
TASK servo_send()
292
{
293
        HEXAPOD_STATE   old_status;
294
        register char   new_pos, new_pwm, new_power;
295
        int             res,n;
296
        struct timespec t;
297
        int actual_leg = 0;
298
 
299
        for (n=0; n<6;n++) {
300
                old_status.ang[n].a = 0;
301
                old_status.ang[n].b = 0;
302
                old_status.ang[n].c = 0;
303
                old_status.cfg[n].pwm = 0;
304
        }
305
        old_status.power = 0;
306
 
307
        while (1) {
308
                new_pos = 0;
309
                new_pwm = 0;
310
                new_power = 0;
311
 
312
                update_event_action();
313
 
314
                if (status.power != old_status.power) {
315
#ifdef  SERIAL_ON
316
                        task_nopreempt();
317
                        if (old_status.power) {
318
                                servo_set_RC5_switch(COM2, 1);
319
                        } else {
320
                                servo_set_RC5_switch(COM2, 0);
321
                        }
322
                        task_preempt();
323
                        old_status.power = status.power;
324
#endif
325
                }
326
 
327
                for (n=0; n<6; n++){
328
#ifdef  SERIAL_ON
329
                        task_nopreempt();
330
                        status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
331
                        trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);   
332
                        task_preempt();
333
#endif
334
                        sys_gettime(&t);
335
                        printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d)                ",
336
                                status.cfg[0].adc_in,
337
                                status.cfg[1].adc_in,
338
                                status.cfg[2].adc_in,
339
                                status.cfg[3].adc_in,
340
                                status.cfg[4].adc_in,
341
                                status.cfg[5].adc_in);
342
                        actual_leg = (actual_leg+1)%6;
343
 
344
                        if ((status.ang[n].a != old_status.ang[n].a) ||
345
                                (status.ang[n].b != old_status.ang[n].b) ||
346
                                (status.ang[n].c != old_status.ang[n].c)) {
347
 
348
                                        old_status.ang[n].a = status.ang[n].a;
349
                                        old_status.ang[n].b = status.ang[n].b;
350
                                        old_status.ang[n].c = status.ang[n].c;
351
                                        new_pos += 1 << n;
352
                        }
353
 
354
                        if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
355
                                old_status.cfg[n].pwm = status.cfg[n].pwm;
356
                                new_pwm += 1 << n;
357
                        }
358
 
359
                        if (new_pos && (1<<n)) {
360
#ifdef  SERIAL_ON
361
                                task_nopreempt();
362
                                res = servo_set_angle_sec(com(n*3  ), pin(n*3  ), adjust(status.ang[n].a,n,0));
363
                                if (res != 0) cprintf("Error send data\n");
364
                                res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1));
365
                                if (res != 0) cprintf("Error send data\n");
366
                                res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2));
367
                                if (res != 0) cprintf("Error send data\n");
368
                                task_preempt();
369
#endif
370
 
371
                        }
372
 
373
                        if (new_pwm && (1<<n)) {
374
#ifdef  SERIAL_ON
375
                                task_nopreempt();
376
                                (old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3  ), pin(n*3  )) : servo_turn_off(com(n*3  ), pin(n*3  ));
377
                                (old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n*3+1), pin(n*3+1)) : servo_turn_off(com(n*3+1), pin(n*3+1));
378
                                (old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n*3+2), pin(n*3+2)) : servo_turn_off(com(n*3+2), pin(n*3+2));
379
                                task_preempt();
380
#endif
381
                        }
382
                }
383
 
384
                task_testcancel();
385
                task_endcycle();
386
        }
387
 
388
        return 0;
389
}
390
 
391
int init_serial()
392
{
393
        int err;
394
        err = servo_open(COM1, COM_SPEED);
395
        if (!err)
396
                err = servo_open(COM2, COM_SPEED);
397
 
398
        return err;
399
}
400
 
401
void end_serial()
402
{
403
        servo_close(COM1);
404
        servo_close(COM2);
405
}
406
 
407
PID servo_pid;
408
 
409
void init_send_task()
410
{
411
        HARD_TASK_MODEL ms;
412
 
413
        hard_task_default_model(ms);
414
        hard_task_def_ctrl_jet(ms);
415
        hard_task_def_wcet(ms, SEND_TASK_WCET);
416
        hard_task_def_mit(ms, SEND_TASK_MIT);
417
        hard_task_def_usemath(ms);
418
        servo_pid = task_create("Servo_Task", servo_send, &ms, NULL);
419
        if (servo_pid == NIL) {
420
                perror("Could not create task <Send_Task>");
421
                sys_end();
422
        } else
423
                task_activate(servo_pid);
424
 
425
}
426
 
427
void init_send()
428
{
429
        int i;
430
 
431
        if (init_serial()) {
432
                perror("Could not initialize serial port.");
433
                sys_end();
434
        }
435
 
436
        for (i=0; i<6;i++) {
437
                status.ang[i].a =   0;
438
                status.ang[i].b =   0;
439
                status.ang[i].c =   0;
440
                status.cfg[i].pwm = 0;
441
        }
442
        status.power = 0;
443
 
444
        init_send_task();
445
 
446
}
447
 
448
void end_send()
449
{
450
        task_kill(servo_pid);
451
 
452
        end_serial();
453
}