Rev 1338 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*
* 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
*/
/*
* 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 */
#define SERIAL_ON /* Send Data using Serial Port */
/* Servo Tasks Constants */
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 6000
#else
#define SEND_TASK_WCET 6000
#endif
#define SEND_TASK_MIT 20000
#ifdef DUBUG_SEND
#define GET_TASK_WCET 6000
#else
#define GET_TASK_WCET 6000
#endif
#define GET_TASK_MIT 20000
#define LEG_A 100.0
#define LEG_B 66.0
#define LEG_C 26.0
#define LEG_D 38.0
#define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
#define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */
const float c0
= LEG_C
* LEG_C
;
const float c1
= LEG_B
* LEG_B
;
const float c2
= LEG_B
* LEG_B
- LEG_A
* LEG_A
;
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
[];
void print_status
(int n
){
}
int leg_to_ang
(float px
, float py
, float pz
, int *alfa
, int *beta
, int *gamma
)
{
float px2
= px
* px
;
float py2
= py
* py
;
float pz2
= pz
* pz
;
float pxz2
= px2
+ pz2
;
float alfa1
,beta1
,alfa2
,beta2
,gamma1
,gamma2
;
float m
,dsqrt
;
float delta_xz
= pxz2
- c0
;
float s
,k
,k2
,y1
,delta_xy
;
if (delta_xz
< 0.0) return -1;
if (pz
>= LEG_C
) {
gamma2
= acos((pz
* LEG_C
+ px
* sqrt(delta_xz
)) / pxz2
);
gamma1
= gamma2
* todeg
;
} else {
gamma2
= -acos((pz
* LEG_C
+ px
* sqrt(delta_xz
)) / pxz2
);
gamma1
= gamma2
* todeg
;
}
m
= pxz2
- LEG_CD_2IPO
* (pz
* sin(gamma2
+LEG_CD_ANG
) + px
* cos(gamma2
+LEG_CD_ANG
) - LEG_CD_2IPO
/ 4.0);
s
= m
+ py2
;
k
= c2
+ s
;
k2
= k
* k
;
delta_xy
= py2
* k2
- s
* (k2
- 4.0 * m
* c1
);
if (delta_xy
>= 0.0) {
dsqrt
= sqrt(delta_xy
);
y1
= (py
* k
+ dsqrt
) / (2.0 * s
);
beta1
= asin(y1
/LEG_B
) * todeg
;
alfa1
= asin((y1
- py
)/LEG_A
) * todeg
+ beta1
;
y1
= (py
* k
- dsqrt
) / (2.0 * s
);
beta2
= asin(y1
/LEG_B
) * todeg
;
alfa2
= asin((y1
- py
)/LEG_A
) * todeg
+ beta2
;
if ((alfa1
>= 0.0 && alfa1
<= 180.0) && (beta1
>= -90.0 && beta1
<= 90.0)) {
*alfa
= (int)(alfa1
* 3600.0);
*beta
= (int)(beta1
* 3600.0);
*gamma
= (int)(gamma1
* 3600.0);
return 0;
} else if ((alfa2
>= 0.0 && alfa2
<= 180.0) && (beta2
>= -90.0 && beta2
<= 90.0)) {
*alfa
= (int)(alfa2
* 3600.0);
*beta
= (int)(beta2
* 3600.0);
*gamma
= (int)(gamma1
* 3600.0);
return 0;
} else {
return -1;
}
} else
return -1;
return -1;
}
int ang_to_leg
(int alfa
, int beta
, int gamma
, float *px
, float *py
, float *pz
) {
float alfa1
= (float)(alfa
)/3600.0 * torad
;
float beta1
= (float)(beta
)/3600.0 * torad
;
float sin_gamma
= sin((float)(gamma
)/3600.0 * torad
);
float cos_gamma
= cos((float)(gamma
)/3600.0 * torad
);
float m
;
m
= LEG_B
* cos(beta1
) + LEG_A
* cos(alfa1
- beta1
);
*py
= LEG_B
* sin(beta1
) - LEG_A
* sin(alfa1
- beta1
);
*pz
= (LEG_D
+ m
) * sin_gamma
+ LEG_C
* cos_gamma
;
*px
= (LEG_D
+ m
) * cos_gamma
- LEG_C
* sin_gamma
;
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_ANGLE
) {
for (i
=0;i
<6;i
++)
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;
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
}
e
->status
= EVT_STATUS_DONE
;
}
}
}
TASK servo_send
()
{
HEXAPOD_STATE old_status
;
register char changes
, new_pos
, new_pwm
, new_power
;
int n
;
for (n
=0; n
<6;n
++) {
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;
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)) {
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) {
old_status.
power = status.
power;
new_power
++;
}
sem_post
(&mx_status
);
if (new_pos
) {
#ifdef SERIAL_ON
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
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) {
sem_wait
(&mx_servo
);
servo_set_RC5_switch
(COM_PORT
, 1);
sem_post
(&mx_servo
);
} else {
sem_wait
(&mx_servo
);
servo_set_RC5_switch
(COM_PORT
, 0);
sem_post
(&mx_servo
);
}
#endif
}
if (new_pos
|| new_pwm
|| new_power
) {
changes
++;
#ifdef DEBUG_SEND
print_status
(n
);
#endif
}
}
task_endcycle
();
}
return 0;
}
TASK servo_get
()
{
int i
= 0;
while (1) {
#ifdef SERIAL_ON
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
();
}
return 0;
}
int init_serial
()
{
int err
;
err
= servo_open
(COM_PORT
, 19200);
return err
;
}
void end_serial
()
{
servo_close
(COM_PORT
);
}
void init_send_task
()
{
SOFT_TASK_MODEL ms
;
PID pid
;
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>");
sys_end
();
} else
task_activate
(pid
);
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
);
}
void init_send
()
{
int i
;
if (init_serial
()) {
perror("Could not initialize serial port.");
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;
status.
ang[i
].
c = 0;
status.
cfg[i
].
pwm = 0;
}
status.
power = 0;
init_send_task
();
}
void end_send
()
{
end_serial
();
}