WebSVN - shark - Rev 1625 - /advdemos/trunk/chimera/send.c

/*
* 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
*/

#include "chimera.h"
#include "tracer.h"
#include "unistd.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 26000
#else
#define SEND_TASK_WCET 26000
#endif
#define SEND_TASK_MIT 30000

#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;

HEXAPOD_STATE status;

void *start_tracer = NULL;
void *end_tracer = NULL;
void *trace_pointer = NULL;

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 ((e->mask >> i) & 1) {

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;

#ifdef DEBUG_SEND
printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i);
#endif

}

e->status = EVT_STATUS_DONE;

}

}

}

int trace_init(int buffer_size)
{

start_tracer = trace_pointer = (void *)malloc(buffer_size);
end_tracer = start_tracer + buffer_size;
memset(trace_pointer,0,buffer_size);

if (trace_pointer == NULL) return -1;

return 0;
}

/* Init the network stack */
int init_network(char *local_ip)
{

struct net_model m = net_base;

net_setudpip(m, local_ip, "255.255.255.255");

if (net_init(&m) != 1) {
cprintf("Network: Init Error.\n");
return -1;
}

return 0;

}

int trace_consumption(int sensor, int value)
{

struct timespec t;
SYS_FLAGS f;

if (trace_pointer == NULL) return -1;

if (trace_pointer >= (end_tracer-16)) return -1;

f = kern_fsave();

sys_gettime(&t);

*(unsigned int *)trace_pointer = (sensor & 0xFF) | 0xAABBFF00;
*(unsigned int *)(trace_pointer + 4) = value;
*(unsigned int *)(trace_pointer + 8) = t.tv_sec;
*(unsigned int *)(trace_pointer + 12) = t.tv_nsec;

trace_pointer += 16;

kern_frestore(f);

return 0;

}

int trace_send() {

static char pkg_buffer[1100];
int actual = 0;

UDP_ADDR target, local;
char local_ip[20], target_ip[20];
int socket;
IP_ADDR bindlist[5];

strcpy(local_ip, "192.168.0.99");
strcpy(target_ip, "192.168.0.104");

if (init_network(local_ip)) sys_end();

/* local IP string to addr */
ip_str2addr(local_ip,&(local.s_addr));
/* set the source port */
local.s_port = 20000;

/* target IP string to addr */
ip_str2addr(target_ip,&(bindlist[0]));
memset(&(bindlist[1]), 0, sizeof(IP_ADDR));
/* bind */
socket = udp_bind(&local, NULL);

/* target IP string to addr */
ip_str2addr(target_ip,&(target.s_addr));
/* target port */
target.s_port = 20000;

trace_pointer = start_tracer;
actual = 0;

while(trace_pointer < end_tracer) {

if (((*(int *)(trace_pointer) >> 8) & 0xAABBFF) == 0xAABBFF) {

cprintf(".");

memcpy(&(pkg_buffer[actual]),trace_pointer,16);

actual += 16;

if (actual > 800) {
pkg_buffer[actual] = 0;
cprintf("X");
udp_sendto(socket, pkg_buffer, actual, &target);
usleep(100000);
actual = 0;
}

}

trace_pointer += 16;

}

pkg_buffer[actual] = 0;
cprintf("X");
udp_sendto(socket, pkg_buffer, actual+1, &target);
usleep(10000);
actual = 0;

return 0;

}

TASK servo_send()
{
HEXAPOD_STATE old_status;
register char new_pos, new_pwm, new_power;
int res,n;
struct timespec t;
int actual_leg = 0;

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) {
new_pos = 0;
new_pwm = 0;
new_power = 0;

update_event_action();

if (status.power != old_status.power) {
#ifdef SERIAL_ON
task_nopreempt();
if (old_status.power) {
servo_set_RC5_switch(COM2, 1);
} else {
servo_set_RC5_switch(COM2, 0);
}
task_preempt();
old_status.power = status.power;
#endif
}

for (n=0; n<6; n++){
#ifdef SERIAL_ON
task_nopreempt();
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);
task_preempt();
#endif
sys_gettime(&t);
printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d) ",
status.cfg[0].adc_in,
status.cfg[1].adc_in,
status.cfg[2].adc_in,
status.cfg[3].adc_in,
status.cfg[4].adc_in,
status.cfg[5].adc_in);
actual_leg = (actual_leg+1)%6;

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 += 1 << n;
}

if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
old_status.cfg[n].pwm = status.cfg[n].pwm;
new_pwm += 1 << n;
}

if (new_pos && (1<<n)) {
#ifdef SERIAL_ON
task_nopreempt();
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");
task_preempt();
#endif

}

if (new_pwm && (1<<n)) {
#ifdef SERIAL_ON
task_nopreempt();
(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));
task_preempt();
#endif
}
}

task_testcancel();
task_endcycle();
}

return 0;
}

int init_serial()
{
int err;
err = servo_open(COM1, COM_SPEED);
if (!err)
err = servo_open(COM2, COM_SPEED);

return err;
}

void end_serial()
{
servo_close(COM1);
servo_close(COM2);
}

PID servo_pid;

void init_send_task()
{
HARD_TASK_MODEL ms;

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);
servo_pid = task_create("Servo_Task", servo_send, &ms, NULL);
if (servo_pid == NIL) {
perror("Could not create task <Send_Task>");
sys_end();
} else
task_activate(servo_pid);

}

void init_send()
{
int i;

if (init_serial()) {
perror("Could not initialize serial port.");
sys_end();
}

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()
{
task_kill(servo_pid);

end_serial();
}

Subversion Repositories shark

(root)/advdemos/trunk/chimera/send.c - Rev 1625