/*Drives straight with shaft encoders and stops when a bump sensor is hit if selected*/
/*Left encoder=port 1, Right encoder=port 3*/

int straight (int desired_speed, int max_speed, float desired_distance, int sensor_check_switch)
{
int current_tics=0, L_motor_tics, R_motor_tics, L_motor_speed, R_motor_speed, tic_difference, Motor_speed_inc, Motor_speed, avg_tics, max_tics, tic_buffer=0, distance_to_go, check_sensors, reverse=0;

if (desired_speed<0)
{
desired_speed=-desired_speed;
reverse=1;
}

max_tics=(int)(desired_distance/.53);
L_motor_speed=desired_speed;
R_motor_speed=desired_speed;

if (reverse==0)
{
left_motor(desired_speed+0);
right_motor(desired_speed);
}
else
{
left_motor(-desired_speed-0);
right_motor(-desired_speed);
}

L_motor_tics=0;
R_motor_tics=0;
encoder1_counts=0;
encoder3_counts=0;

while (current_tics<max_tics)
{
printf("L %d %d R %d %d T %d\n",L_motor_speed, encoder1_counts, R_motor_speed, encoder3_counts, current_tics);
encoder1_counts=0;
encoder3_counts=0;
sleep(.3);
L_motor_tics=encoder1_counts;
R_motor_tics=encoder3_counts;

if (L_motor_tics<R_motor_tics && L_motor_speed < max_speed)
{
tic_difference = R_motor_tics - L_motor_tics;
Motor_speed_inc = 1;
Motor_speed=L_motor_speed;
Motor_speed = Motor_speed + Motor_speed_inc;
if (Motor_speed>max_speed) Motor_speed = max_speed;
if (reverse==0)
{
left_motor(Motor_speed);
}
else
{
left_motor(-Motor_speed);
}
L_motor_speed=Motor_speed;
}

if (L_motor_tics < R_motor_tics && L_motor_speed >= max_speed)
{
tic_difference=R_motor_tics-L_motor_tics;
Motor_speed_inc=1;
Motor_speed=R_motor_speed;
Motor_speed=Motor_speed-Motor_speed_inc;
if (reverse==0)
{
right_motor(Motor_speed);
}
else
{
right_motor(-Motor_speed);
}
R_motor_speed=Motor_speed;
}

if (R_motor_tics<L_motor_tics && R_motor_speed < max_speed)
{
tic_difference=L_motor_tics-R_motor_tics;
Motor_speed_inc=1;
Motor_speed=R_motor_speed;
Motor_speed=Motor_speed+Motor_speed_inc;
if (Motor_speed>max_speed) Motor_speed=max_speed;
if (reverse==0)
{
right_motor(Motor_speed);
}
else
{
right_motor(-Motor_speed);
}
R_motor_speed=Motor_speed;
}

if (R_motor_tics < L_motor_tics && R_motor_speed >= max_speed)
{
tic_difference=L_motor_tics-R_motor_tics;
Motor_speed_inc=1;
Motor_speed=L_motor_speed;
Motor_speed=Motor_speed-Motor_speed_inc;
if (reverse==0)
{
left_motor(Motor_speed);
}
else
{
left_motor(-Motor_speed);
}
L_motor_speed=Motor_speed;
}

avg_tics=(L_motor_tics + R_motor_tics)/2;
current_tics=current_tics+avg_tics;

if (sensor_check_switch==1)
{
check_sensors=sensor_check();
if (check_sensors!=0)
{
distance_to_go=max_tics-current_tics;
current_tics=max_tics;
}
}
tic_buffer=current_tics;
}
left_motor(0);
right_motor(0);
if (check_sensors==0) return 0;
if (check_sensors>0) return distance_to_go;
}