Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1436 → Rev 1437

/demos/trunk/chimera/chimera.c
37,41 → 37,109
 
}
 
int beta = 30;
 
void action_stand_up(void) {
struct action_event e;
int event;
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 = 0 * 3600;
e.ang.b = 70 * 3600;
e.ang.c = 0;
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);
 
ADDUSEC2TIMESPEC(1000000,&(e.time));
}
 
e.type = EVT_SET_MASK_LEG_ANGLE;
e.mask = 0x3F;
e.ang.a = 0 * 3600;
e.ang.b = 0 * 3600;
e.ang.c = 0;
e.pwm = 7;
}
 
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;
event = insert_action_event(&(e));
cprintf("Event num = %d\n",event);
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()
TASK walk_auto_3()
{
short i;
int vd, vs, auto_count = 0;
164,7 → 232,7
insert_action_event(&(e));
}
 
auto_count++;
auto_count += 4;
if (auto_count > 63) auto_count = 0;
 
task_testcancel();
174,20 → 242,20
}
 
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, 25000);
hard_task_def_usemath(ms);
pid_walk = task_create("Walk_Task", walk_auto, &ms, NULL);
if (pid_walk == NIL) {
perror("Could not create task <Walk_Auto>");
sys_end();
} else
task_activate(pid_walk);
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)
212,6 → 280,11
 
init_action_event(100);
 
if (argc < 2) sys_end();
 
beta = atoi(argv[1]);
cprintf("Beta = %d\n",beta);
 
//action_stand_up();
action_walk();