/*void main() {
enable_servos();
calibrate_servo();
lsb_ms_per_deg = 256.0;

start_press();
calibrate_gyro();
start_process(update_angle());

straight_syncro_vertical(0.0, 100, 8000L, 1);
servo_horizontal();
straight_syncro_horizontal(0.0, 100, 3000L, 1);
alloff();
disable_servos();
}*/

void servo_horizontal(){
calibrate_gyro();
servo(1, 3000);
servo(2, 2700);
servo(3, 1200);
servo(5, 2300);
motor(1, 50);
motor(2, -50);
sleep(1.0);
alloff();
}

void servo_vertical(){
calibrate_gyro();
servo(1, 500);
servo(2, 900);
servo(3, 3500);
servo(5, 500);
/*motor(2, 50);
motor(1, -50);*/

}

void servo_turn(float angle){
/*degs is an integer between 0 and 4000 which indicates the servo turning */
float a = (angle * 4000.0)/ 180.0;
float b = (angle * 4000.0)/ (180.0 * ( 40.0/56.0));

int servo_deg1 = servo1;
int servo_deg2 = servo2;
int servo_deg3 = servo3;
int servo_deg5 = servo5;
/*if(servo0 != 4000 || servo0 != 0 || servo1 != 0 || servo1 != 4000
|| servo2 != 4000 || servo2 != 0 || servo3 != 0 || servo3 != 4000){*/
servo(1, servo_deg1 - (int)b);
servo(2, servo_deg2 + (int)a);
servo(3, servo_deg3 - (int) b);
servo(5, servo_deg5 + (int)a);
/*}*/
}

void calibrate_servo(){
printf("press start servo\n");
servo(1, 500);
servo(2, 900);
servo(3, 3500);
servo(5, 500);
printf("done servo calib\n");
}