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

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

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,-90,0},{413949,453599,431999},{216000,50401,-215599}},
        {1,{0,90,-45,45,-45,45},{417601,388801,424741},{-216000,-43200,82770}},
        {-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
        {1,{0,90,-45,45,0,90},{414001,424801,410401},{-162000,39600,-122400}},
        {-1,{0,90,-45,45,0,90},{410399,446399,399599},{154800,-3599,108000}},
};

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();
       
        servo_turn_off(com(active_leg*3), pin(active_leg*3));
        servo_turn_off(com(active_leg*3+1), pin(active_leg*3+1));
        servo_turn_off(com(active_leg*3+2), pin(active_leg*3+2));

        servo_count = 0;
        servo_turn_on(com(active_leg*3), pin(active_leg*3));
        turn_on = 0;

        while (calibrate_status == 1) {

                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(3*active_leg+num),pin(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;

                                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 = 0;
                                break;
                        case 2:
                        case 3:
                                sprintf(servo_name,"BETA ");
                                num = 1;
                                break;
                        case 4:
                        case 5:
                                sprintf(servo_name,"GAMMA");
                                num = 2;
                                break;
                }

                if (turn_on == 1) {
                        servo_turn_on(com(3*active_leg+num),pin(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(3*active_leg+num),pin(3*active_leg+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(3*active_leg+num), pin(3*active_leg+num), test_angle[3*active_leg+num]);

                        calibrate_exec_step = 0;

                }

                angsec = servo_get_angle_sec(com(3*active_leg+num), pin(3*active_leg+num));
               
                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;

        }

}