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"
#include <ll/i386/64bit.h>

volatile int calibrate_status = 0;
volatile int calibrate_exec_step = 0;

extern HEXAPOD_STATE status;
extern unsigned char active_leg;

extern sem_t mx_servo;

struct leg_calibration {
        int side;
        int pos[6];
        int delta_90[3];
        int zero[3];
};

struct leg_calibration calibration_table[] = {
        {1,{0,90,-45,45,-90,0},{432001,403201,432001},{-201600,28800,241201}},
        {1,{0,90,-45,45,-45,45},{417601,388801,424741},{-216000,-43200,82770}},
        {1,{0,90,-45,45,0,90},{414001,424801,410401},{-162000,39600,-122400}},
        {-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}},
        {-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
        {-1,{0,90,-45,45,0,90},{0,0,0},{0,0,0}},
};

int adjust(int angle_sec, int leg, int num) {

        int temp;

        smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp);

        return calibration_table[leg].side * temp + calibration_table[leg].zero[num];
       
}

TASK calibrate(void *arg) {

        int i, num = 0, angsec = 0, angsec_temp[6];
        static int test_angle[20],set,turn_on,servo_count;
        char servo_name[10];

        for (i=0;i<20;i++) test_angle[i] = 0;

        clear();
       
        sem_wait(&mx_servo);
        servo_turn_off(COM_PORT, active_leg*3+2);
        servo_turn_off(COM_PORT, active_leg*3+1);
        servo_turn_off(COM_PORT, active_leg*3);

        servo_count = 0;
        servo_turn_on(COM_PORT, active_leg*3+2);
        turn_on = 0;
        sem_post(&mx_servo);

        while (calibrate_status == 1) {

                sem_wait(&mx_servo);

                if (calibrate_exec_step == 100000) {
                   
                        angsec_temp[servo_count] = test_angle[3*active_leg+num];                                                                                                    
                        printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
                        servo_turn_off(COM_PORT,3*active_leg+num);
                        servo_count++;

                        if (servo_count == 6) {

                                for (i=0;i<3;i++) {
                                        calibration_table[active_leg].delta_90[i] = abs(angsec_temp[2*i+1] - angsec_temp[2*i] + 1);
                                        calibration_table[active_leg].zero[i] = calibration_table[active_leg].side * abs(calibration_table[active_leg].pos[2*i] * calibration_table[active_leg].delta_90[i] / 90) + angsec_temp[2*i];

                                        printf_xy(22*i,22,WHITE,"D%d %7d Z%d %7d",
                                                        i,calibration_table[active_leg].delta_90[i],
                                                        i,calibration_table[active_leg].zero[i]);

                                }

                                calibrate_status = 0;
                                calibrate_exec_step = 0;

                                sem_post(&mx_servo);

                                task_kill(exec_shadow);
                                task_testcancel();

                        }

                        turn_on = 1;

                        calibrate_exec_step = 0;

                }

                switch (servo_count) {
                        case 0:
                        case 1:
                                sprintf(servo_name,"ALFA ");
                                num = 2;
                                break;
                        case 2:
                        case 3:
                                sprintf(servo_name,"BETA ");
                                num = 1;
                                break;
                        case 4:
                        case 5:
                                sprintf(servo_name,"GAMMA");
                                num = 0;
                                break;
                }

                if (turn_on == 1) {
                        servo_turn_on(COM_PORT,3*active_leg+num);
                        turn_on = 0;
                }      

                set = calibration_table[active_leg].pos[servo_count];
                printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
                       
                servo_turn_on(COM_PORT, active_leg*3+num);

                if (calibrate_exec_step != 0) {
       
                        test_angle[3*active_leg+num] += calibrate_exec_step;
                        printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3*active_leg+num],3*active_leg+num);
                        servo_set_angle_sec(COM_PORT, 3*active_leg+num, test_angle[3*active_leg+num]);

                        calibrate_exec_step = 0;

                }

                angsec = servo_get_angle_sec(COM_PORT, 3*active_leg+num);
               
                sem_post(&mx_servo);

                printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);

                task_endcycle();

        }

        return 0;

}

void calibrate_init() {

        SOFT_TASK_MODEL st;
        PID st_pid;

        if (calibrate_status == 0) {
                calibrate_status = 1;

                soft_task_default_model(st);
                soft_task_def_period(st,300000);
                soft_task_def_met(st,30000);
                soft_task_def_usemath(st);
                soft_task_def_ctrl_jet(st);

                st_pid = task_create("Calibration task",calibrate,&st,NULL);
                if (st_pid == NIL) {
                        cprintf("Error creating calibration task\n");
                        sys_end();
                }

                task_activate(st_pid);

        } else {
                return;
        }
       
}

void calibrate_step(int step) {

        if (calibrate_status != 0 && calibrate_exec_step == 0) {

                calibrate_exec_step = step;

        }

}