WebSVN - shark - Rev 1625 - /advdemos/trunk/chimera/chimera.c

/*
* 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;

}

Subversion Repositories shark

(root)/advdemos/trunk/chimera/chimera.c - Rev 1625