1,3 → 1,4 |
|
/* |
* Project: S.Ha.R.K. |
* |
10,6 → 11,25 |
* http://shark.sssup.it |
*/ |
|
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
|
#include "chimera.h" |
|
//#define DEBUG_SEND /* Print Sent Values */ |
17,18 → 37,18 |
|
/* Servo Tasks Constants */ |
#ifdef DUBUG_SEND |
#define SEND_TASK_WCET 20000 |
#define SEND_TASK_WCET 6000 |
#else |
#define SEND_TASK_WCET 20000 |
#define SEND_TASK_WCET 6000 |
#endif |
#define SEND_TASK_MIT 40000 |
#define SEND_TASK_MIT 20000 |
|
#ifdef DUBUG_SEND |
#define GET_TASK_WCET 5000 |
#define GET_TASK_WCET 6000 |
#else |
#define GET_TASK_WCET 5000 |
#define GET_TASK_WCET 6000 |
#endif |
#define GET_TASK_MIT 40000 |
#define GET_TASK_MIT 20000 |
|
#define LEG_A 100.0 |
#define LEG_B 66.0 |
43,6 → 63,7 |
const float todeg = 180.0 / PI; |
const float torad = PI / 180.0; |
|
sem_t mx_status, mx_adc_status, mx_servo; |
HEXAPOD_STATE status; |
|
extern struct leg_calibration calibration_table[]; |
142,8 → 163,10 |
if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
|
for (i=0;i<6;i++) |
if ((e->mask >> i) & 1) { |
if ((1 << i) & e->mask) { |
|
sem_wait(&mx_status); |
|
status.ang[i].a = e->ang.a; |
status.ang[i].b = e->ang.b; |
status.ang[i].c = e->ang.c; |
150,6 → 173,8 |
|
status.cfg[i].pwm = e->pwm; |
|
sem_post(&mx_status); |
|
#ifdef DEBUG_SEND |
printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i); |
#endif |
169,7 → 194,7 |
{ |
HEXAPOD_STATE old_status; |
register char changes, new_pos, new_pwm, new_power; |
int res,n; |
int n; |
|
for (n=0; n<6;n++) { |
old_status.ang[n].a = 0; |
189,6 → 214,8 |
new_pwm = 0; |
new_power = 0; |
|
sem_wait(&mx_status); |
|
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)) { |
207,32 → 234,37 |
old_status.power = status.power; |
new_power++; |
} |
sem_post(&mx_status); |
|
if (new_pos) { |
#ifdef SERIAL_ON |
res = servo_set_angle_sec(com(n*3 ), pin(n*3 ), adjust(status.ang[n].a,n,0)); |
if (res != 0) cprintf("Error send data\n"); |
res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1)); |
if (res != 0) cprintf("Error send data\n"); |
res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2)); |
if (res != 0) cprintf("Error send data\n"); |
sem_wait(&mx_servo); |
servo_set_angle_sec(COM_PORT, n*3+2, adjust(status.ang[n].a,n,0)); |
servo_set_angle_sec(COM_PORT, n*3+1, adjust(status.ang[n].b,n,1)); |
servo_set_angle_sec(COM_PORT, n*3 , adjust(status.ang[n].c,n,2)); |
sem_post(&mx_servo); |
#endif |
|
} |
|
if (new_pwm) { |
#ifdef SERIAL_ON |
(old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3 ), pin(n*3 )) : servo_turn_off(com(n*3 ), pin(n*3 )); |
(old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n*3+1), pin(n*3+1)) : servo_turn_off(com(n*3+1), pin(n*3+1)); |
(old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n*3+2), pin(n*3+2)) : servo_turn_off(com(n*3+2), pin(n*3+2)); |
sem_wait(&mx_servo); |
(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 |
} |
if (new_power) { |
#ifdef SERIAL_ON |
if (old_status.power) { |
servo_set_RC5_switch(COM2, 1); |
sem_wait(&mx_servo); |
servo_set_RC5_switch(COM_PORT, 1); |
sem_post(&mx_servo); |
} else { |
servo_set_RC5_switch(COM2, 0); |
sem_wait(&mx_servo); |
servo_set_RC5_switch(COM_PORT, 0); |
sem_post(&mx_servo); |
} |
#endif |
} |
256,12 → 288,16 |
|
while (1) { |
#ifdef SERIAL_ON |
status.cfg[i ].adc_in = servo_get_analog(COM1, i ); |
status.cfg[i+1].adc_in = servo_get_analog(COM1, i+1); |
sem_wait(&mx_adc_status); |
sem_wait(&mx_servo); |
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); |
#endif |
i = (i+2)%6; |
sem_post(&mx_adc_status); |
#endif |
task_endcycle(); |
} |
272,9 → 308,7 |
int init_serial() |
{ |
int err; |
err = servo_open(COM1, COM_SPEED); |
if (!err) |
err = servo_open(COM2, COM_SPEED); |
err = servo_open(COM_PORT, 19200); |
|
return err; |
} |
281,20 → 315,19 |
|
void end_serial() |
{ |
servo_close(COM1); |
servo_close(COM2); |
servo_close(COM_PORT); |
} |
|
void init_send_task() |
{ |
HARD_TASK_MODEL ms; |
SOFT_TASK_MODEL ms; |
PID pid; |
|
hard_task_default_model(ms); |
hard_task_def_ctrl_jet(ms); |
hard_task_def_wcet(ms, SEND_TASK_WCET); |
hard_task_def_mit(ms, SEND_TASK_MIT); |
hard_task_def_usemath(ms); |
soft_task_default_model(ms); |
soft_task_def_ctrl_jet(ms); |
soft_task_def_met(ms, SEND_TASK_WCET); |
soft_task_def_period(ms, SEND_TASK_MIT); |
soft_task_def_usemath(ms); |
pid = task_create("Send_Task", servo_send, &ms, NULL); |
if (pid == NIL) { |
perror("Could not create task <Send_Task>"); |
302,17 → 335,17 |
} else |
task_activate(pid); |
|
hard_task_default_model(ms); |
hard_task_def_ctrl_jet(ms); |
hard_task_def_wcet(ms, GET_TASK_WCET); |
hard_task_def_mit(ms, GET_TASK_MIT); |
hard_task_def_usemath(ms); |
soft_task_default_model(ms); |
soft_task_def_ctrl_jet(ms); |
soft_task_def_met(ms, GET_TASK_WCET); |
soft_task_def_period(ms, GET_TASK_MIT); |
soft_task_def_usemath(ms); |
pid = task_create("Get_Task", servo_get, &ms, NULL); |
if (pid == NIL) { |
perror("Could not create task <Get_Task>"); |
sys_end(); |
} //else |
//task_activate(pid); |
} else |
task_activate(pid); |
} |
|
void init_send() |
324,6 → 357,10 |
sys_end(); |
} |
|
sem_init(&mx_status,0,1); |
sem_init(&mx_adc_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; |
338,5 → 375,7 |
|
void end_send() |
{ |
|
end_serial(); |
|
} |