newbie arduino 6dof

Hi,
hope that this is the right forum
I'v builded my first 6dof Stewart platform as a starting point to real life sized platform
and I stumbled already at some troubles.
I'v assembled the platform using six standard servos and Arduino Mega. Servos work if I use
standard sweep example code
but I get no movement when I start the following code:

#include <Servo.h>
const float pi = 3.14159,
theta_r = radians(48.0),
theta_p = radians(23.2),
theta_s[] = { -pi / 3, 2 * pi / 3, pi, 0, pi / 3, -2 * pi / 3},
RD = 2.395,
PD = 3.3,
L1 = 1.0,
L2 = 4.72,
z_home = 4.25,
servo_min = 3.5,
servo_max = 5.5,
servo_mult = 6,
p[2][6] = {{PD * cos(pi / 6 + theta_p), PD * cos(pi / 6 - theta_p), PD * cos(-(pi / 2 - theta_p)), -PD * cos(-(pi / 2 - theta_p)), -PD * cos(pi / 6 - theta_p), -PD * cos(pi / 6 + theta_p)},
{PD * sin(pi / 6 + theta_p), PD * sin(pi / 6 - theta_p), PD * sin(-(pi / 2 - theta_p)), PD * sin(-(pi / 2 - theta_p)), PD * sin(pi / 6 - theta_p), PD * sin(pi / 6 + theta_p)}
},
re[2][6] = {{RD * cos(pi / 6 + theta_r), RD * cos(pi / 6 - theta_r), RD * cos(-(pi / 2 - theta_r)), -RD * cos(-(pi / 2 - theta_r)), -RD * cos(pi / 6 - theta_r), -RD * cos(pi / 6 + theta_r)},
{RD * sin(pi / 6 + theta_r), RD * sin(pi / 6 - theta_r), RD * sin(-(pi / 2 - theta_r)), RD * sin(-(pi / 2 - theta_r)), RD * sin(pi / 6 - theta_r), RD * sin(pi / 6 + theta_r)}
};

/*
theta_r = angle between attachment points
theta_p = angle between rotation points
theta_s = orientation of the servos
RD = distance to end effector attachment points
PD = distance to servo rotation points
L1 = servo arm length
L2 = connecting arm length
z_home = default z height with servo arms horizontal
servo_min = lower limit for servo arm angle
servo_max = upper limit for servo arm angle
servo_mult = multiplier to convert to milliseconds
p = location of servo rotation points in base frame [x/y][1-6]
re = location of attachment points in end effector frame [x/y][1-6]
*/
const int servo_pin[] = {9, 3, 5, 11, 6, 10};
const int servo_zero[6] = {1710, 1280, 1700, 1300, 1680, 1300};
/*
servo_pin = servo pin assignments,
servo_zero = zero angles for each servo (horizontal)
*/
Servo servo[6];
/*
Servos 0, 2, 4: reversed (+ = down, - = up)
Servos 1, 3, 5: normal (+ = up, - = down)
*/
void setup()
{
//Serial.begin(9600);
for (int i = 0; i < 6; i++)
{

servo.attach(servo_pin);
servo.writeMicroseconds(servo_zero);
}
delay(1000);
}
void loop()
{
static float pe[6] = {0, 0, 0, radians(0), radians(0), radians(0)}, theta_a[6], servo_pos[6], q[3][6], r[3][6], dl[3][6], dl2[6];

}
/*
pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)
theta_a = angle of the servo arm
servo_pos = value written to each servo
q = position of lower mounting point of connecting link [x,y,x][1-6]
r = position of upper mounting point of connecting link
dl = difference between x,y,z coordinates of q and r
dl2 = distance between q and r
*/