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