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
 
15
#define POS_B_REF        0
16
#define POS_B_UP        50
17
 
18
int vel_dx = 15;
19
int vel_sx = 15;
20
 
21
void program_key_end(KEY_EVT* e)
22
{
23
 
24
        int i;
25
        TIME tmp;
26
 
27
        end_send();
28
 
29
        for (i=3; i<MAX_PROC; i++){
30
                if (!jet_getstat(i, NULL, &tmp, NULL, NULL))
31
                        kern_printf("Task Name : %s - Max Time  : %d\n", proc_table[i].name, (int)tmp);
32
        }
33
 
34
        trace_send();
35
 
36
        sys_end();
37
 
38
}
39
 
40
int beta = 30;
41
 
42
void action_stand_up(void) {
43
 
44
  struct action_event e;
45
  int event,alfa;
46
 
47
  status.power = 1;
48
 
49
  kern_gettime(&(e.time));
50
 
51
  for (alfa=0;alfa<=90;alfa+=15) {
52
 
53
  ADDUSEC2TIMESPEC(5000000,&(e.time));
54
 
55
  e.type = EVT_SET_MASK_LEG_ANGLE;
56
  e.mask = 0x3F;
57
  e.ang.a = alfa * 3600;
58
  e.ang.b = beta * 3600;
59
  e.ang.c = 0 * 3600;
60
  e.pwm = 7;
61
 
62
  event = insert_action_event(&(e));
63
  cprintf("Event num = %d\n",event);
64
 
65
  }
66
 
67
}
68
 
69
TASK walk_auto_6()
70
{
71
        short   i, n;
72
        int     vd, vs, auto_count = 0;
73
        int pos_a[6], pos_b[6], pos_c[6];
74
        struct action_event e;
75
 
76
        float   fake_sin[36] = { .00, .50, .80, 1.0, .80, .50, .00, .00, .00, .00, .00, .00,
77
                                 .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00,
78
                                 .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00};
79
 
80
        float   fake_cos[36] = {-1.0,-.75,-.50, .00, .50, .75, 1.0, .93, .86, .80, .73, .66,
81
                                 .60, .53, .46, .40, .33, .26, .20, .13, .06, .00,-.06, .13,
82
                                -.20,-.26,-.33,-.40,-.46,-.53,-.60,-.66,-.73,-.80,-.86,-.93};
83
 
84
        while (1) {
85
                vd = vel_dx;
86
                vs = vel_sx;
87
 
88
                kern_gettime(&(e.time));
89
                ADDUSEC2TIMESPEC(10000,&(e.time));
90
 
91
                for (i = 0; i<6; i++) {
92
                        switch(i) {
93
                                case 0:
94
                                        n = 0;
95
                                        break;
96
                                case 1:
97
                                        n = 3;
98
                                        break;
99
 
100
                                case 2:
101
                                        n = 4;
102
                                        break;
103
 
104
                                case 3:
105
                                        n = 1;
106
                                        break;
107
 
108
                                case 4:
109
                                        n = 2;
110
                                        break;
111
 
112
                                case 5:
113
                                        n = 5;
114
                                        break;
115
                        }                                                                                                                            
116
                        if (i%3)
117
                                pos_c[i] = vd * fake_cos[(auto_count+6*n)%36];
118
                        else
119
                                pos_c[i] = vs * fake_cos[(auto_count+6*n)%36];
120
                        if ((vs) || (vd))
121
                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+6*n)%36] * POS_B_UP;
122
                        else
123
                                pos_b[i] = POS_B_REF;
124
 
125
                        e.type = EVT_SET_MASK_LEG_ANGLE;
126
                        e.mask = 1 << i;
127
                        e.ang.a = (80) * 3600;
128
                        e.ang.b = (pos_b[i]) * 3600;
129
                        e.ang.c = (pos_c[i]) * 3600;
130
                        e.pwm = 7;
131
                        insert_action_event(&(e));
132
                }
133
                auto_count++;
134
                if (auto_count > 35) auto_count = 0;
135
 
136
                task_testcancel();
137
                task_endcycle();
138
        }
139
        return 0;
140
}
141
 
142
TASK walk_auto_3()
143
{
144
        short   i;
145
        int     vd, vs, auto_count = 0;
146
        float   cos_a;
147
        int pos_a[6], pos_b[6], pos_c[6];
148
 
149
        struct action_event e;
150
 
151
 
152
        float   fake_cos[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301,
153
                                 .70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802,
154
                                 .00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439,
155
                                -.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518};
156
        float   fake_sin[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05,
157
                                  0.00,  0.00,  0.40,  0.40,  0.70,  0.70,  0.90,  0.90,
158
                                  1.00,  1.00,  0.90,  0.90,  0.70,  0.70,  0.40,  0.40,
159
                                  0.00,  0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40,
160
                                 -0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
161
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
162
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
163
                                 -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 };
164
 
165
        while (1) {
166
                cos_a = (auto_count < 32) ? fake_cos[auto_count] : -fake_cos[auto_count-32];
167
 
168
                vd = vel_dx;
169
                vs = vel_sx;
170
 
171
                kern_gettime(&(e.time));
172
                ADDUSEC2TIMESPEC(10000,&(e.time));
173
 
174
                for (i = 0; i<6; i++) {
175
                        switch (i) {
176
                                case 0:
177
                                        /* Leg 0 */
178
                                        pos_c[i] =  vs * cos_a;
179
                                        if ((vs) || (vd))
180
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
181
                                        else
182
                                                pos_b[i] = POS_B_REF;
183
                                        break;
184
                                case 1:
185
                                        /* Leg 1 */
186
                                        pos_c[i] = -vd * cos_a;
187
                                        if ((vs) || (vd))
188
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
189
                                        else
190
                                                pos_b[i] = POS_B_REF;
191
                                        break;
192
                                case 2:
193
                                        /* Leg 2 */
194
                                        pos_c[i] = -vs * cos_a;
195
                                        if ((vs) || (vd))
196
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
197
                                        else
198
                                                pos_b[i] = POS_B_REF;
199
                                        break;
200
                                case 3:
201
                                        /* Leg 3 */
202
                                        pos_c[i] =  vd * cos_a;
203
                                        if ((vs) || (vd))
204
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
205
                                        else
206
                                                pos_b[i] = POS_B_REF;
207
                                        break;
208
                                case 4:
209
                                        /* Leg 4 */
210
                                        pos_c[i] =  vs * cos_a;
211
                                        if ((vs) || (vd))
212
                                                pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
213
                                        else
214
                                                pos_b[i] = POS_B_REF;
215
                                        break;
216
                                case 5:
217
                                        /* Leg 5 */
218
                                        pos_c[i] = -vd * cos_a;
219
                                        if ((vs) || (vd))
220
                                                pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
221
                                        else
222
                                                pos_b[i] = POS_B_REF;
223
                                        break;
224
                        }
225
 
226
                        e.type = EVT_SET_MASK_LEG_ANGLE;
227
                        e.mask = 1 << i;
228
                        e.ang.a = (80) * 3600;
229
                        e.ang.b = (pos_b[i]) * 3600;
230
                        e.ang.c = (pos_c[i]) * 3600;
231
                        e.pwm = 7;
232
                        insert_action_event(&(e));
233
                }
234
 
235
                auto_count += 4;
236
                if (auto_count > 63) auto_count = 0;
237
 
238
                task_testcancel();
239
                task_endcycle();
240
        }
241
        return 0;
242
}
243
 
244
void action_walk(void) {
245
        HARD_TASK_MODEL ms;
246
        PID pid_walk;
247
 
248
        hard_task_default_model(ms);
249
        hard_task_def_ctrl_jet(ms);
250
        hard_task_def_wcet(ms, 1000);
251
        hard_task_def_mit(ms, 30000);
252
        hard_task_def_usemath(ms);
253
        pid_walk = task_create("Walk_Task", walk_auto_3, &ms, NULL);
254
        if (pid_walk == NIL) {
255
                perror("Could not create task <Walk_Auto>");
256
                sys_end();
257
        } else
258
                task_activate(pid_walk);
259
}
260
 
261
int main(int argc, char **argv)
262
{
263
        TIME seme;
264
        KEY_EVT k;
265
 
266
        seme = sys_gettime(NULL);
267
        srand(seme);
268
 
269
        k.flag = ALTL_BIT;
270
        k.scan = KEY_C;
271
        k.ascii = 'c';
272
        keyb_hook(k,program_key_end);
273
 
274
        clear();
275
 
276
        trace_init(1000000);
277
 
278
        init_send();            /* Comunication */
279
        init_key();             /* Keyboard */
280
 
281
        init_action_event(100);
282
 
283
        if (argc < 2) sys_end();
284
 
285
        beta = atoi(argv[1]);
286
        cprintf("Beta = %d\n",beta);
287
 
288
        //action_stand_up();
289
        action_walk();
290
 
291
        return 0;
292
 
293
}