Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1624 giacomo 1
/*
2
 * Project: S.Ha.R.K.
3
 *
4
 * Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
5
 *
6
 *
7
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
8
 *
9
 * http://www.sssup.it
10
 * http://retis.sssup.it
11
 * http://shark.sssup.it
12
 */
13
 
14
#include "chimera.h"
15
#include <ll/i386/64bit.h>
16
 
17
volatile int calibrate_status = 0;
18
volatile int calibrate_exec_step = 0;
19
 
20
extern HEXAPOD_STATE status;
21
extern unsigned char active_leg;
22
 
23
struct leg_calibration {
24
        int side;
25
        int pos[6];
26
        int delta_90[3];
27
        int zero[3];
28
};
29
 
30
struct leg_calibration calibration_table[] = {
31
        {1,{0,90,-45,45,-90,0},{432001,403201,432001},{-201600,28800,241201}},
32
        {-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}},
33
        {1,{0,90,-45,45,-45,45},{417601,388801,424741},{-216000,-43200,82770}},
34
        {-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
35
        {1,{0,90,-45,45,0,90},{414001,424801,410401},{-162000,39600,-122400}},
36
        {-1,{0,90,-45,45,0,90},{410399,446399,399599},{154800,-3599,108000}},
37
};
38
 
39
int adjust(int angle_sec, int leg, int num) {
40
 
41
        int temp;
42
 
43
        smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp);
44
 
45
        return calibration_table[leg].side * temp + calibration_table[leg].zero[num];
46
 
47
}
48
 
49
TASK calibrate(void *arg) {
50
 
51
        int i, num = 0, angsec = 0, angsec_temp[6];
52
        static int test_angle[20],set,turn_on,servo_count;
53
        char servo_name[10];
54
 
55
        for (i=0;i<20;i++) test_angle[i] = 0;
56
 
57
        clear();
58
 
59
        servo_turn_off(com(active_leg*3), pin(active_leg*3));
60
        servo_turn_off(com(active_leg*3+1), pin(active_leg*3+1));
61
        servo_turn_off(com(active_leg*3+2), pin(active_leg*3+2));
62
 
63
        servo_count = 0;
64
        servo_turn_on(com(active_leg*3), pin(active_leg*3));
65
        turn_on = 0;
66
 
67
        while (calibrate_status == 1) {
68
 
69
                if (calibrate_exec_step == 100000) {
70
 
71
                        angsec_temp[servo_count] = test_angle[3*active_leg+num];                                                                                                    
72
                        printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
73
                        servo_turn_off(com(3*active_leg+num),pin(3*active_leg+num));
74
                        servo_count++;
75
 
76
                        if (servo_count == 6) {
77
 
78
                                for (i=0;i<3;i++) {
79
                                        calibration_table[active_leg].delta_90[i] = abs(angsec_temp[2*i+1] - angsec_temp[2*i] + 1);
80
                                        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];
81
 
82
                                        printf_xy(22*i,22,WHITE,"D%d %7d Z%d %7d",
83
                                                        i,calibration_table[active_leg].delta_90[i],
84
                                                        i,calibration_table[active_leg].zero[i]);
85
 
86
                                }
87
 
88
                                calibrate_status = 0;
89
                                calibrate_exec_step = 0;
90
 
91
                                task_kill(exec_shadow);
92
                                task_testcancel();
93
 
94
                        }
95
 
96
                        turn_on = 1;
97
 
98
                        calibrate_exec_step = 0;
99
 
100
                }
101
 
102
                switch (servo_count) {
103
                        case 0:
104
                        case 1:
105
                                sprintf(servo_name,"ALFA ");
106
                                num = 0;
107
                                break;
108
                        case 2:
109
                        case 3:
110
                                sprintf(servo_name,"BETA ");
111
                                num = 1;
112
                                break;
113
                        case 4:
114
                        case 5:
115
                                sprintf(servo_name,"GAMMA");
116
                                num = 2;
117
                                break;
118
                }
119
 
120
                if (turn_on == 1) {
121
                        servo_turn_on(com(3*active_leg+num),pin(3*active_leg+num));
122
                        turn_on = 0;
123
                }      
124
 
125
                set = calibration_table[active_leg].pos[servo_count];
126
                printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
127
 
128
                servo_turn_on(com(3*active_leg+num),pin(3*active_leg+num));
129
 
130
                if (calibrate_exec_step != 0) {
131
 
132
                        test_angle[3*active_leg+num] += calibrate_exec_step;
133
                        printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3*active_leg+num],3*active_leg+num);
134
                        servo_set_angle_sec(com(3*active_leg+num), pin(3*active_leg+num), test_angle[3*active_leg+num]);
135
 
136
                        calibrate_exec_step = 0;
137
 
138
                }
139
 
140
                angsec = servo_get_angle_sec(com(3*active_leg+num), pin(3*active_leg+num));
141
 
142
                printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);
143
 
144
                task_endcycle();
145
 
146
        }
147
 
148
        return 0;
149
 
150
}
151
 
152
void calibrate_init() {
153
 
154
        SOFT_TASK_MODEL st;
155
        PID st_pid;
156
 
157
        if (calibrate_status == 0) {
158
                calibrate_status = 1;
159
 
160
                soft_task_default_model(st);
161
                soft_task_def_period(st,300000);
162
                soft_task_def_met(st,30000);
163
                soft_task_def_usemath(st);
164
                soft_task_def_ctrl_jet(st);
165
 
166
                st_pid = task_create("Calibration task",calibrate,&st,NULL);
167
                if (st_pid == NIL) {
168
                        cprintf("Error creating calibration task\n");
169
                        sys_end();
170
                }
171
 
172
                task_activate(st_pid);
173
 
174
        } else {
175
                return;
176
        }
177
 
178
}
179
 
180
void calibrate_step(int step) {
181
 
182
        if (calibrate_status != 0 && calibrate_exec_step == 0) {
183
 
184
                calibrate_exec_step = step;
185
 
186
        }
187
 
188
}