| 43,6 → 43,7 |
|---|
| extern sem_t mx_servo; |
| |
| struct leg_calibration { |
| int side; |
| int pos[6]; |
| int delta_90[3]; |
| int zero[3]; |
| 49,12 → 50,12 |
|---|
| }; |
| |
| struct leg_calibration calibration_table[] = { |
| {{0,90,-45,45,-90,0},{0,0,0},{0,0,0}}, |
| {{0,90,-45,45,-45,45},{0,0,0},{0,0,0}}, |
| {{0,90,-45,45,0,90},{417601,403201,424801},{-169200,36000,-144000}}, |
| {{0,90,-45,45,-90,0},{0,0,0},{0,0,0}}, |
| {{0,90,-45,45,-45,45},{0,0,0},{0,0,0}}, |
| {{0,90,-45,45,0,90},{0,0,0},{0,0,0}}, |
| {1,{0,90,-45,45,-90,0},{450001,392401,435601},{-208800,41400,234001}}, |
| {1,{0,90,-45,45,-45,45},{403201,374401,349201},{-201600,-32400,63000}}, |
| {1,{0,90,-45,45,0,90},{417601,403201,424801},{-169200,36000,-144000}}, |
| {-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) { |
| 63,7 → 64,7 |
|---|
| |
| smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp); |
| |
| return temp + calibration_table[leg].zero[num]; |
| return calibration_table[leg].side * temp + calibration_table[leg].zero[num]; |
| |
| } |
| |
| 100,10 → 101,11 |
|---|
| |
| 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] = abs(calibration_table[active_leg].pos[2*i] * calibration_table[active_leg].delta_90[i] / 90) + angsec_temp[2*i]; |
| 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(20*i,22,RED,"D%d %7d Z%d %7d",i,calibration_table[active_leg].delta_90[i], |
| i,calibration_table[active_leg].zero[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]); |
| |
| } |
| |