76,11 → 76,11 |
|
if (n>=6) { |
for (i=0; i<6;i++) |
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.leg[i].pwm ); |
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.cfg[i].pwm ); |
cprintf("Power: %d", status.power); |
cprintf("\n"); |
} else { |
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.leg[n].pwm, status.power); |
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.cfg[n].pwm, status.power); |
} |
} |
|
153,6 → 153,47 |
return 0; |
} |
|
void update_event_action(void) { |
|
struct timespec t; |
struct action_event *e; |
int i; |
|
kern_gettime(&t); |
|
while ((e = get_first_old_event(&t)) != NULL) { |
|
if (e->type == EVT_SET_MASK_LEG_POS) { |
|
e->status = EVT_STATUS_DONE; |
|
} |
|
if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
|
for (i=0;i<6;i++) |
if ((1 << i) & e->mask) { |
|
status.ang[i].a = angles[i].a = *(int *)(e->data); |
status.ang[i].b = angles[i].b = *(int *)(e->data+4); |
status.ang[i].c = angles[i].c = *(int *)(e->data+8); |
status.cfg[i].pwm = *(unsigned char *)(e->data+16); |
|
//#ifdef DEBUG_SEND |
cprintf("Update leg %d angle\n",i); |
//#endif |
|
|
} |
|
e->status = EVT_STATUS_DONE; |
|
} |
|
} |
|
} |
|
TASK servo_send() |
{ |
HEXAPOD_STATE old_status; |
161,14 → 202,19 |
|
for (n=0; n<6;n++) { |
old_status.leg[n].x = 150; |
old_status.leg[n].y = 0; |
old_status.leg[n].z = 0; |
old_status.leg[n].pwm = 0; |
old_status.leg[n].y = 0; |
old_status.leg[n].z = 0; |
old_status.ang[n].a = 0; |
old_status.ang[n].b = 0; |
old_status.ang[n].c = 0; |
old_status.cfg[n].pwm = 0; |
} |
old_status.power = 0; |
|
while (1) { |
changes = 0; |
|
update_event_action(); |
|
for (n=0; n<6; n++){ |
new_pos = 0; |
175,7 → 221,7 |
new_pwm = 0; |
new_power = 0; |
|
//sem_wait(&mx_status); |
sem_wait(&mx_status); |
if ((status.leg[n].x != old_status.leg[n].x) || (status.leg[n].y != old_status.leg[n].y) || (status.leg[n].z != old_status.leg[n].z)) { |
if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) { |
old_status.leg[n].x = status.leg[n].x; |
191,8 → 237,19 |
status.leg[n].z = old_status.leg[n].z; |
} |
} |
if (status.leg[n].pwm != old_status.leg[n].pwm) { |
old_status.leg[n].pwm = status.leg[n].pwm; |
|
if ((status.ang[n].a != old_status.ang[n].a) || |
(status.ang[n].b != old_status.ang[n].b) || |
(status.ang[n].c != old_status.ang[n].c)) { |
|
old_status.ang[n].a = status.ang[n].a; |
old_status.ang[n].b = status.ang[n].b; |
old_status.ang[n].c = status.ang[n].c; |
new_pos++; |
} |
|
if (status.cfg[n].pwm != old_status.cfg[n].pwm) { |
old_status.cfg[n].pwm = status.cfg[n].pwm; |
new_pwm++; |
} |
if (status.power != old_status.power) { |
199,7 → 256,7 |
old_status.power = status.power; |
new_power++; |
} |
//sem_post(&mx_status); |
sem_post(&mx_status); |
|
if (new_pos) { |
#ifdef SERIAL_ON |
213,11 → 270,11 |
,(int)(status.leg[n].y) |
,(int)(status.leg[n].z)); |
|
printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a) |
,(int)(angles[n].b) |
,(int)(angles[n].c)); |
printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a/3600) |
,(int)(angles[n].b/3600) |
,(int)(angles[n].c/3600)); |
|
printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0),adjust(angles[n].b,n,1),adjust(angles[n].c,n,2)); |
printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600); |
|
#endif |
} |
225,9 → 282,9 |
if (new_pwm) { |
#ifdef SERIAL_ON |
sem_wait(&mx_servo); |
(old_status.leg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2); |
(old_status.leg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1); |
(old_status.leg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 ); |
(old_status.cfg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2); |
(old_status.cfg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1); |
(old_status.cfg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 ); |
sem_post(&mx_servo); |
#endif |
} |
266,8 → 323,8 |
#ifdef SERIAL_ON |
//sem_wait(&mx_status); |
sem_wait(&mx_servo); |
status.leg[i ].adc_in = servo_get_analog(COM_PORT, i ); |
status.leg[i+1].adc_in = servo_get_analog(COM_PORT, i+1); |
status.cfg[i ].adc_in = servo_get_analog(COM_PORT, i ); |
status.cfg[i+1].adc_in = servo_get_analog(COM_PORT, i+1); |
sem_post(&mx_servo); |
#ifdef DEBUG_SEND |
cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in); |
333,14 → 390,17 |
sys_end(); |
} |
|
//sem_init(&mx_status,0,1); |
sem_init(&mx_status,0,1); |
sem_init(&mx_servo,0,1); |
|
for (i=0; i<6;i++) { |
status.ang[i].a = 0; |
status.ang[i].b = 0; |
status.ang[i].c = 0; |
status.leg[i].x = 150; |
status.leg[i].y = 0; |
status.leg[i].z = 0; |
status.leg[i].pwm = 0; |
status.cfg[i].pwm = 0; |
} |
status.power = 0; |
|