Subversion Repositories shark

Rev

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