Rev 1624 |
Rev 1645 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*
* Project: S.Ha.R.K.
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/
#include "chimera.h"
#define POS_B_REF 0
#define POS_B_UP 50
int vel_dx
= 15;
int vel_sx
= 15;
void program_key_end
(KEY_EVT
* e
)
{
int i
;
TIME tmp
;
end_send
();
for (i
=3; i
<MAX_PROC
; i
++){
if (!jet_getstat
(i
, NULL
, &tmp
, NULL
, NULL
))
kern_printf
("Task Name : %s - Max Time : %d\n", proc_table
[i
].
name, (int)tmp
);
}
trace_send
();
sys_end
();
}
int beta
= 30;
void action_stand_up
(void) {
struct action_event e
;
int event
,alfa
;
status.
power = 1;
kern_gettime
(&(e.
time));
for (alfa
=0;alfa
<=90;alfa
+=15) {
ADDUSEC2TIMESPEC
(5000000,&(e.
time));
e.
type = EVT_SET_MASK_LEG_ANGLE
;
e.
mask = 0x3F;
e.
ang.
a = alfa
* 3600;
e.
ang.
b = beta
* 3600;
e.
ang.
c = 0 * 3600;
e.
pwm = 7;
event
= insert_action_event
(&(e
));
cprintf
("Event num = %d\n",event
);
}
}
TASK walk_auto_6
()
{
short i
, n
;
int vd
, vs
, auto_count
= 0;
int pos_a
[6], pos_b
[6], pos_c
[6];
struct action_event e
;
float fake_sin
[36] = { .00, .50, .80, 1.0, .80, .50, .00, .00, .00, .00, .00, .00,
.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00,
.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00};
float fake_cos
[36] = {-1.0,-.75,-.50, .00, .50, .75, 1.0, .93, .86, .80, .73, .66,
.60, .53, .46, .40, .33, .26, .20, .13, .06, .00,-.06, .13,
-.20,-.26,-.33,-.40,-.46,-.53,-.60,-.66,-.73,-.80,-.86,-.93};
while (1) {
vd
= vel_dx
;
vs
= vel_sx
;
kern_gettime
(&(e.
time));
ADDUSEC2TIMESPEC
(10000,&(e.
time));
for (i
= 0; i
<6; i
++) {
switch(i
) {
case 0:
n
= 0;
break;
case 1:
n
= 3;
break;
case 2:
n
= 4;
break;
case 3:
n
= 1;
break;
case 4:
n
= 2;
break;
case 5:
n
= 5;
break;
}
if (i
%3)
pos_c
[i
] = vd
* fake_cos
[(auto_count
+6*n
)%36];
else
pos_c
[i
] = vs
* fake_cos
[(auto_count
+6*n
)%36];
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[(auto_count
+6*n
)%36] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
e.
type = EVT_SET_MASK_LEG_ANGLE
;
e.
mask = 1 << i
;
e.
ang.
a = (80) * 3600;
e.
ang.
b = (pos_b
[i
]) * 3600;
e.
ang.
c = (pos_c
[i
]) * 3600;
e.
pwm = 7;
insert_action_event
(&(e
));
}
auto_count
++;
if (auto_count
> 35) auto_count
= 0;
task_testcancel
();
task_endcycle
();
}
return 0;
}
TASK walk_auto_3
()
{
short i
;
int vd
, vs
, auto_count
= 0;
float cos_a
;
int pos_a
[6], pos_b
[6], pos_c
[6];
struct action_event e
;
float fake_cos
[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301,
.70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802,
.00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439,
-.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518};
float fake_sin
[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05,
0.00, 0.00, 0.40, 0.40, 0.70, 0.70, 0.90, 0.90,
1.00, 1.00, 0.90, 0.90, 0.70, 0.70, 0.40, 0.40,
0.00, 0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40,
-0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 };
while (1) {
cos_a
= (auto_count
< 32) ? fake_cos
[auto_count
] : -fake_cos
[auto_count
-32];
vd
= vel_dx
;
vs
= vel_sx
;
kern_gettime
(&(e.
time));
ADDUSEC2TIMESPEC
(10000,&(e.
time));
for (i
= 0; i
<6; i
++) {
switch (i
) {
case 0:
/* Leg 0 */
pos_c
[i
] = vs
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[(auto_count
+32)%64] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
case 1:
/* Leg 1 */
pos_c
[i
] = -vd
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[auto_count
] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
case 2:
/* Leg 2 */
pos_c
[i
] = -vs
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[auto_count
] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
case 3:
/* Leg 3 */
pos_c
[i
] = vd
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[(auto_count
+32)%64] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
case 4:
/* Leg 4 */
pos_c
[i
] = vs
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[(auto_count
+32)%64] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
case 5:
/* Leg 5 */
pos_c
[i
] = -vd
* cos_a
;
if ((vs
) || (vd
))
pos_b
[i
] = POS_B_REF
+ fake_sin
[auto_count
] * POS_B_UP
;
else
pos_b
[i
] = POS_B_REF
;
break;
}
e.
type = EVT_SET_MASK_LEG_ANGLE
;
e.
mask = 1 << i
;
e.
ang.
a = (80) * 3600;
e.
ang.
b = (pos_b
[i
]) * 3600;
e.
ang.
c = (pos_c
[i
]) * 3600;
e.
pwm = 7;
insert_action_event
(&(e
));
}
auto_count
+= 4;
if (auto_count
> 63) auto_count
= 0;
task_testcancel
();
task_endcycle
();
}
return 0;
}
void action_walk
(void) {
HARD_TASK_MODEL ms
;
PID pid_walk
;
hard_task_default_model
(ms
);
hard_task_def_ctrl_jet
(ms
);
hard_task_def_wcet
(ms
, 1000);
hard_task_def_mit
(ms
, 30000);
hard_task_def_usemath
(ms
);
pid_walk
= task_create
("Walk_Task", walk_auto_3
, &ms
, NULL
);
if (pid_walk
== NIL
) {
perror("Could not create task <Walk_Auto>");
sys_end
();
} else
task_activate
(pid_walk
);
}
int main
(int argc
, char **argv
)
{
TIME seme
;
KEY_EVT k
;
seme
= sys_gettime
(NULL
);
srand(seme
);
k.
flag = ALTL_BIT
;
k.
scan = KEY_C
;
k.
ascii = 'c';
keyb_hook
(k
,program_key_end
);
clear
();
trace_init
(1000000);
init_send
(); /* Comunication */
init_key
(); /* Keyboard */
init_action_event
(100);
if (argc
< 2) sys_end
();
beta
= atoi(argv
[1]);
cprintf
("Beta = %d\n",beta
);
//action_stand_up();
action_walk
();
return 0;
}