Subversion Repositories shark

Rev

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

Rev Author Line No. Line
1331 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
 
15
//#define DEBUG_SEND            /* Print Sent Values */
16
#define SERIAL_ON               /* Send Data using Serial Port */
17
 
18
/* Servo Tasks Constants */
19
#ifdef DUBUG_SEND
1359 giacomo 20
#define SEND_TASK_WCET 20000
1331 giacomo 21
#else
1359 giacomo 22
#define SEND_TASK_WCET 20000
1331 giacomo 23
#endif
1359 giacomo 24
#define SEND_TASK_MIT  40000
1331 giacomo 25
 
26
#ifdef DUBUG_SEND
1359 giacomo 27
#define GET_TASK_WCET   5000
1331 giacomo 28
#else
1359 giacomo 29
#define GET_TASK_WCET   5000
1331 giacomo 30
#endif
1359 giacomo 31
#define GET_TASK_MIT   40000
1331 giacomo 32
 
33
#define LEG_A 100.0
34
#define LEG_B  66.0
35
#define LEG_C  26.0
36
#define LEG_D  38.0
37
#define LEG_CD_2IPO 92.087      /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
1338 giacomo 38
#define LEG_CD_ANG  0.600       /* arctg(LEG_C/LEG_D) in radianti */
1331 giacomo 39
 
40
const float c0 = LEG_C * LEG_C;
41
const float c1 = LEG_B * LEG_B;
42
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
43
const float todeg = 180.0 / PI;
1336 giacomo 44
const float torad = PI / 180.0;
1331 giacomo 45
 
46
HEXAPOD_STATE   status;
47
 
1332 giacomo 48
extern struct leg_calibration calibration_table[];
49
 
1331 giacomo 50
void print_status(int n){
51
}
52
 
1336 giacomo 53
int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma)
1331 giacomo 54
{
55
        float px2 = px * px;
56
        float py2 = py * py;
57
        float pz2 = pz * pz;
58
 
59
        float pxz2 = px2 + pz2;
60
 
1338 giacomo 61
        float alfa1,beta1,alfa2,beta2,gamma1,gamma2;
1331 giacomo 62
        float m,dsqrt;
63
 
64
        float delta_xz = pxz2 - c0;
65
        float s,k,k2,y1,delta_xy;
66
 
67
        if (delta_xz < 0.0) return -1;
68
 
69
        if (pz >= LEG_C) {
1338 giacomo 70
                gamma2 =  acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
71
                gamma1 = gamma2 * todeg;
1331 giacomo 72
        } else {
1338 giacomo 73
                gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
74
                gamma1 = gamma2 * todeg;
1331 giacomo 75
        }
76
 
1338 giacomo 77
        m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0);
1331 giacomo 78
 
79
        s = m + py2;
80
        k = c2 + s;
81
        k2 = k * k;
82
 
83
        delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
84
 
85
        if (delta_xy >= 0.0) {
86
                dsqrt = sqrt(delta_xy);
87
                y1 = (py * k + dsqrt) / (2.0 * s);
88
                beta1 = asin(y1/LEG_B) * todeg;
89
                alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
90
                y1 = (py * k - dsqrt) / (2.0 * s);
91
                beta2 = asin(y1/LEG_B) * todeg;
92
                alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
93
 
94
                if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
1336 giacomo 95
                        *alfa = (int)(alfa1 * 3600.0);
96
                        *beta = (int)(beta1 * 3600.0);
97
                        *gamma = (int)(gamma1 * 3600.0);
1331 giacomo 98
                        return 0;
99
                } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
1336 giacomo 100
                        *alfa = (int)(alfa2 * 3600.0);
101
                        *beta = (int)(beta2 * 3600.0);
102
                        *gamma = (int)(gamma1 * 3600.0);
1331 giacomo 103
                        return 0;
104
                } else {
1336 giacomo 105
                        return -1;
1331 giacomo 106
                }
107
        } else
1336 giacomo 108
                return -1;
1331 giacomo 109
 
1336 giacomo 110
        return -1;
111
 
1331 giacomo 112
}
113
 
1336 giacomo 114
int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) {
115
 
116
  float alfa1 = (float)(alfa)/3600.0 * torad;
117
  float beta1 = (float)(beta)/3600.0 * torad;
118
  float sin_gamma = sin((float)(gamma)/3600.0 * torad);
119
  float cos_gamma = cos((float)(gamma)/3600.0 * torad);
120
  float m;
121
 
122
  m =  LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1);
123
  *py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1);
124
 
125
  *pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma;
126
  *px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma;
127
 
128
  return 0;
129
 
130
}
131
 
1333 giacomo 132
void update_event_action(void) {
133
 
134
  struct timespec t;
135
  struct action_event *e;
136
  int i;
137
 
138
  kern_gettime(&t);
139
 
140
  while ((e = get_first_old_event(&t)) != NULL) {
141
 
142
        if (e->type == EVT_SET_MASK_LEG_ANGLE) {
143
 
144
                for (i=0;i<6;i++)
1359 giacomo 145
                        if ((e->mask >> i) & 1) {
1333 giacomo 146
 
1336 giacomo 147
                        status.ang[i].a = e->ang.a;
148
                        status.ang[i].b = e->ang.b;
149
                        status.ang[i].c = e->ang.c;
150
 
151
                        status.cfg[i].pwm = e->pwm;
152
 
1339 giacomo 153
#ifdef  DEBUG_SEND
1334 giacomo 154
                        printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i);
1339 giacomo 155
#endif
1333 giacomo 156
 
157
 
158
                        }
159
 
160
                e->status = EVT_STATUS_DONE;
161
 
162
        }
163
 
164
  }
165
 
166
}
167
 
1331 giacomo 168
TASK servo_send()
169
{
170
        HEXAPOD_STATE   old_status;
171
        register char   changes, new_pos, new_pwm, new_power;
1359 giacomo 172
        int             res,n;
1331 giacomo 173
 
174
        for (n=0; n<6;n++) {
1333 giacomo 175
                old_status.ang[n].a = 0;
176
                old_status.ang[n].b = 0;
177
                old_status.ang[n].c = 0;
178
                old_status.cfg[n].pwm = 0;
1331 giacomo 179
        }
180
        old_status.power = 0;
181
 
182
        while (1) {
183
                changes = 0;
1333 giacomo 184
 
185
                update_event_action();
1331 giacomo 186
 
187
                for (n=0; n<6; n++){
188
                        new_pos = 0;
189
                        new_pwm = 0;
190
                        new_power = 0;
191
 
1333 giacomo 192
                        if ((status.ang[n].a != old_status.ang[n].a) ||
193
                                (status.ang[n].b != old_status.ang[n].b) ||
194
                                (status.ang[n].c != old_status.ang[n].c)) {
195
 
196
                                        old_status.ang[n].a = status.ang[n].a;
197
                                        old_status.ang[n].b = status.ang[n].b;
198
                                        old_status.ang[n].c = status.ang[n].c;
199
                                        new_pos++;
200
                        }
201
 
202
                        if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
203
                                old_status.cfg[n].pwm = status.cfg[n].pwm;
1331 giacomo 204
                                new_pwm++;
205
                        }
206
                        if (status.power != old_status.power) {
207
                                old_status.power = status.power;
208
                                new_power++;
209
                        }
210
 
211
                        if (new_pos) {
212
#ifdef  SERIAL_ON
1359 giacomo 213
                                res = servo_set_angle_sec(com(n*3  ), pin(n*3  ), adjust(status.ang[n].a,n,0));
214
                                if (res != 0) cprintf("Error send data\n");
215
                                res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1));
216
                                if (res != 0) cprintf("Error send data\n");
217
                                res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2));
218
                                if (res != 0) cprintf("Error send data\n");
1331 giacomo 219
#endif
1359 giacomo 220
 
1331 giacomo 221
                        }
222
 
223
                        if (new_pwm) {
224
#ifdef  SERIAL_ON
1359 giacomo 225
                                (old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3  ), pin(n*3  )) : servo_turn_off(com(n*3  ), pin(n*3  ));
226
                                (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));
227
                                (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));
1331 giacomo 228
#endif
229
                        }
230
                        if (new_power) {
231
#ifdef  SERIAL_ON
232
                                if (old_status.power) {
1359 giacomo 233
                                        servo_set_RC5_switch(COM2, 1);
1331 giacomo 234
                                } else {
1359 giacomo 235
                                        servo_set_RC5_switch(COM2, 0);
1331 giacomo 236
                                }
237
#endif
238
                        }
239
 
240
                        if (new_pos || new_pwm || new_power) {
241
                                changes++;
242
#ifdef  DEBUG_SEND
243
                                print_status(n);
244
#endif
245
                        }
246
                }
247
                task_endcycle();
248
        }
249
 
250
        return 0;
251
}
252
 
253
TASK servo_get()
254
{
255
        int     i = 0;
256
 
257
        while (1) {
258
#ifdef  SERIAL_ON
1359 giacomo 259
                status.cfg[i  ].adc_in = servo_get_analog(COM1, i  );
260
                status.cfg[i+1].adc_in = servo_get_analog(COM1, i+1);
1331 giacomo 261
#ifdef  DEBUG_SEND
262
                cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in);
263
#endif
264
                i = (i+2)%6;
265
#endif
266
                task_endcycle();
267
        }
268
 
269
        return 0;
270
}
271
 
272
int init_serial()
273
{
274
        int err;
1359 giacomo 275
        err = servo_open(COM1, COM_SPEED);
276
        if (!err)
277
                err = servo_open(COM2, COM_SPEED);
1331 giacomo 278
 
279
        return err;
280
}
281
 
282
void end_serial()
283
{
1359 giacomo 284
        servo_close(COM1);
285
        servo_close(COM2);
1331 giacomo 286
}
287
 
288
void init_send_task()
289
{
1359 giacomo 290
        HARD_TASK_MODEL ms;
1331 giacomo 291
        PID pid;
292
 
1359 giacomo 293
        hard_task_default_model(ms);
294
        hard_task_def_ctrl_jet(ms);
295
        hard_task_def_wcet(ms, SEND_TASK_WCET);
296
        hard_task_def_mit(ms, SEND_TASK_MIT);
297
        hard_task_def_usemath(ms);
1331 giacomo 298
        pid = task_create("Send_Task", servo_send, &ms, NULL);
299
        if (pid == NIL) {
300
                perror("Could not create task <Send_Task>");
301
                sys_end();
302
        } else
303
                task_activate(pid);
304
 
1359 giacomo 305
        hard_task_default_model(ms);
306
        hard_task_def_ctrl_jet(ms);
307
        hard_task_def_wcet(ms, GET_TASK_WCET);
308
        hard_task_def_mit(ms, GET_TASK_MIT);
309
        hard_task_def_usemath(ms);
1331 giacomo 310
        pid = task_create("Get_Task", servo_get, &ms, NULL);
311
        if (pid == NIL) {
312
                perror("Could not create task <Get_Task>");
313
                sys_end();
1359 giacomo 314
        } //else
315
                //task_activate(pid);
1331 giacomo 316
}
317
 
318
void init_send()
319
{
320
        int i;
321
 
322
        if (init_serial()) {
323
                perror("Could not initialize serial port.");
324
                sys_end();
325
        }
326
 
327
        for (i=0; i<6;i++) {
1333 giacomo 328
                status.ang[i].a =   0;
329
                status.ang[i].b =   0;
330
                status.ang[i].c =   0;
331
                status.cfg[i].pwm = 0;
1331 giacomo 332
        }
333
        status.power = 0;
334
 
335
        init_send_task();
1336 giacomo 336
 
1331 giacomo 337
}
338
 
339
void end_send()
340
{
341
        end_serial();
342
}