Subversion Repositories shark

/*

 * 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<16;i++) {

                servo_set_max_angle_sec(COM_PORT,i,460000);

                servo_set_min_angle_sec(COM_PORT,i,-460000);

        }

        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();

}