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},{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();

        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;

        }

}