| 12,6 → 12,12 |
|---|
| |
| #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) |
| { |
| |
| 65,6 → 71,125 |
|---|
| |
| } |
| |
| TASK walk_auto() |
| { |
| 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++; |
| 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, 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); |
| } |
| |
| int main(int argc, char **argv) |
| { |
| TIME seme; |
| 87,7 → 212,8 |
|---|
| |
| init_action_event(100); |
| |
| action_stand_up(); |
| //action_stand_up(); |
| action_walk(); |
| |
| return 0; |
| |