Subversion Repositories shark

Rev

Rev 1333 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed


/*
 * Project: S.Ha.R.K.
 *
 * Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
 *
 * Authors     : Paolo Gai <pj@hartik.sssup.it>
 * (see authors.txt for full list of hartik's authors)
 *
 * 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  34.380      /* arctg(LEG_C/LEG_D) in gradi */

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;

sem_t           mx_status, mx_servo;
HEXAPOD_STATE   status;
ANGLES_STATE    angles[6];

extern struct leg_calibration calibration_table[];

void print_status(int n){
        int i;

        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.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.cfg[n].pwm, status.power);
        }
}

int set_leg_position(int n, float px, float py, float pz)
{
        float px2 = px * px;
        float py2 = py * py;
        float pz2 = pz * pz;

        float pxz2 = px2 + pz2;

        float alfa1,beta1,alfa2,beta2,gamma1;
        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) {
                gamma1 =  acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
        } else {
                gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
        }

        if (gamma1 < -90.0 || gamma1 > 90.0) return -1;

        m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG));

        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)) {
                        angles[n].a = (int)(alfa1 * 3600.0);
                        angles[n].b = (int)(beta1 * 3600.0);
                        angles[n].c = (int)(gamma1 * 3600.0);
#ifdef  DEBUG_SEND
                        cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
#endif
                        return 0;
                } else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
                        angles[n].a = (int)(alfa2 * 3600.0);
                        angles[n].b = (int)(beta2 * 3600.0);
                        angles[n].c = (int)(gamma1 * 3600.0);
#ifdef  DEBUG_SEND
                        cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
#endif
                        return 0;
                } else {
#ifdef  DEBUG_SEND
                        cprintf("No possible !\n");
#endif
                        return 1;
                }
        } else
                return 1;

        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+12);

//#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.leg[n].x = 150;
                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;
                        new_pwm = 0;
                        new_power = 0;
                       
                        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;
                                        old_status.leg[n].y = status.leg[n].y;
                                        old_status.leg[n].z = status.leg[n].z;
                                        new_pos++;
                                } else {
#ifdef  DEBUG_SEND
                                        cprintf("Position not reachable.\n");
#endif
                                        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 ((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(angles[n].a,n,0));
                                servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1));
                                servo_set_angle_sec(COM_PORT, n*3  , adjust(angles[n].c,n,2));
                                sem_post(&mx_servo);
       
                                printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x)
                                                                                ,(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/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)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600);

#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_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_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_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.cfg[i].pwm = 0;
        }
        status.power = 0;

        init_send_task();
}

void end_send()
{
        end_serial();
}