program.nxc

#define GYRO_PORT    IN_1
#define LEFT_MOTOR   OUT_C
#define RIGHT_MOTOR  OUT_B
#define BOTH_MOTORS  OUT_BC
#define WAIT_TIME    7.0
#define KGYROANGLE   0.681
#define KSPEED       0.016
#define KGYROSPEED   0.092
#define KPOS         0.009
#define KWANTED      0.009
#define WANTEDSPEED  -500
#define K_P_STEER    0.4

float d_time, last_time;
float segway_angle = 0;
float segway_speed;
float wheel_angle = 0, last_wheel_angle;
float wheel_speed;
float max_voltage;
int u;
float wanted_wheel_angle = 0;
int first = 0;
float powerSteer = 0;
int light = 0;
int leftU = 0;
int rightU = 0;
float gyro_offset;
float dist_crit;
mutex dist_mutex;
bool its_all = false;
int distance_to_obstacle = 255;

long int lt = 0;
int obst = 0;
long int tObst1 = 1000;
byte handle;
string str;
int str_size;

int distance;

int getGyroOffset()
{
    float offset = 0;
    int i;
    
    for(i=0; i<100; i++)
             offset = offset + 1.0*SensorHTGyro(GYRO_PORT);
    return (offset/100);
}

void findLine(int & first, int & gyro_offset) 
{
	int lineSpeed = -300;
	float k_p = 0;
	int i = 0;
    last_time = CurrentTick();
    wheel_angle = (MotorRotationCount(LEFT_MOTOR) + MotorRotationCount(RIGHT_MOTOR)) / 2;
    Wait(WAIT_TIME);
	while(i <= 2) {
		max_voltage = BatteryLevel() / 1000;
        light = Sensor(S2);
        
        d_time = (CurrentTick() - last_time) / 1000;
        last_time = CurrentTick();
        segway_speed = (SensorHTGyro(GYRO_PORT) - gyro_offset);
        segway_angle += segway_speed * d_time;
        

        last_wheel_angle = wheel_angle;
        wheel_angle = (MotorRotationCount(LEFT_MOTOR) + MotorRotationCount(RIGHT_MOTOR)) / 2;
        wheel_speed = (wheel_angle - last_wheel_angle) / d_time;
        wanted_wheel_angle = wanted_wheel_angle + lineSpeed * d_time;
        
	    u = KGYROANGLE * segway_angle +
            KSPEED * (wheel_speed - lineSpeed) +
            KGYROSPEED * segway_speed +
            KPOS * (wheel_angle - wanted_wheel_angle) +
            KWANTED * lineSpeed;

        powerSteer = k_p * (light - first);
        if (abs(light - first) == 2) i = i + 1;

        leftU = u - powerSteer;
        rightU = u + powerSteer;

        leftU = leftU * 100 / max_voltage;
        if (abs(leftU) > 100) leftU = sign(leftU) * 100;

        rightU = rightU * 100 / max_voltage;
        if (abs(rightU) > 100) rightU = sign(rightU) * 100;


        OnFwd(LEFT_MOTOR, leftU);
        OnFwd(RIGHT_MOTOR, rightU);
        Wait(WAIT_TIME);
	}
    PlayTone(400, 1000);
}

task brick_us() {
    SetSensorLowspeed(S3);
	int distUS;
    while(true) {
		distUS = SensorUS(S3);
		Acquire(dist_mutex);
        distance_to_obstacle = distUS;
		Release(dist_mutex);
    }
}

task brick_go(){
    its_all = true;
    int wanted_speed = WANTEDSPEED;
    while(true) {

        max_voltage = BatteryLevel() / 1000;
        light = Sensor(S2);

        d_time = (CurrentTick() - last_time) / 1000;
        last_time = CurrentTick();
        segway_speed = (SensorHTGyro(GYRO_PORT) - gyro_offset);
        segway_angle += segway_speed * d_time;

        last_wheel_angle = wheel_angle;
        wheel_angle = (MotorRotationCount(LEFT_MOTOR) + MotorRotationCount(RIGHT_MOTOR)) / 2;
        wheel_speed = (wheel_angle - last_wheel_angle) / d_time;
        wanted_wheel_angle = wanted_wheel_angle + wanted_speed * d_time;

        u =  KGYROANGLE * segway_angle +
            KSPEED * (wheel_speed - wanted_speed) +
            KGYROSPEED * segway_speed +
            KPOS * (wheel_angle - wanted_wheel_angle) +
            KWANTED * wanted_speed;

		Acquire(dist_mutex);
		if (distance_to_obstacle < 40 && lt == 0){
			obst = 1;
            wanted_speed = -500;
            lt = CurrentTick();
			PlayTone(500, 500);
		}
		else if (obst == 1 && CurrentTick() - lt > tObst1){
			obst = 2;
            wanted_speed = WANTEDSPEED;
            lt = CurrentTick();
			PlayTone(500, 500);
		}
		else if (obst == 2 && abs(light - first) == 2){
			obst = 0;
			PlayTone(500, 500);
		}
		Release(dist_mutex);

		switch(obst){
		case 0:
			powerSteer = K_P_STEER * (light - first);
			break;
		case 1:
			powerSteer = 1;
			break;
		case 2:
			powerSteer = -1;
			break;
		}


        leftU = u - powerSteer;
        rightU = u + powerSteer;

        leftU = leftU * 100 / max_voltage;
        if (abs(leftU) > 100) leftU = sign(leftU) * 100;

        rightU = rightU * 100 / max_voltage;
        if (abs(rightU) > 100) rightU = sign(rightU) * 100;


        OnFwd(LEFT_MOTOR, leftU);
        OnFwd(RIGHT_MOTOR, rightU);

        Wait(WAIT_TIME);
        ClearScreen();
    }
}


task mountain_us(){
    SetSensorLowspeed(S4);
    int dists[20];
    int i;
    for(i = 0; i < 20; i++)
        dists[i] = 24;
    i = 0;
    dist_crit = 24;
    while(!its_all){
        distance = SensorUS(S4);
        Acquire(dist_mutex);
        dist_crit = dist_crit*20 - dists[i];
        if(abs(distance - 24) < 10)
            dists[i] = distance;
        else
            dists[i] = 24;
        dist_crit = (dist_crit + dists[i]) / 20;
        Release(dist_mutex);
        i = (i+1)%20;
    }
}

task mountain_go(){
    long int mountain_time = 0;
    bool mountain_in_past = false;
    int wanted_speed = WANTEDSPEED;
    int l_motor_angle, r_motor_angle;
    int straight_control;
    while(!mountain_in_past){

        max_voltage = BatteryLevel() / 1000;
        light = Sensor(S2);
        
        d_time = (CurrentTick() - last_time) / 1000;
        last_time = CurrentTick();
        segway_speed = (SensorHTGyro(GYRO_PORT) - gyro_offset);
        segway_angle += segway_speed * d_time;

        last_wheel_angle = wheel_angle;
        l_motor_angle = MotorRotationCount(LEFT_MOTOR);
        r_motor_angle = MotorRotationCount(RIGHT_MOTOR);
        wheel_angle = (l_motor_angle + r_motor_angle) / 2;
        wheel_speed = (wheel_angle - last_wheel_angle) / d_time;
        wanted_wheel_angle = wanted_wheel_angle + wanted_speed * d_time;
        
        u =  KGYROANGLE * segway_angle +
            KSPEED * (wheel_speed - wanted_speed) +
            KGYROSPEED * segway_speed +
            KPOS * (wheel_angle - wanted_wheel_angle) + 
            KWANTED * wanted_speed;
        

        powerSteer = K_P_STEER * (light - first);
        if(!mountain_in_past){
            straight_control = 0.1 * (l_motor_angle-r_motor_angle);
        }
        else
            straight_control = 0;
        leftU = u - powerSteer - straight_control;
        rightU = u + powerSteer + straight_control;

        leftU = leftU * 100 / max_voltage;
        if (abs(leftU) > 100) leftU = sign(leftU) * 100;

        rightU = rightU * 100 / max_voltage;
        if (abs(rightU) > 100) rightU = sign(rightU) * 100;


        OnFwd(LEFT_MOTOR, leftU);
        OnFwd(RIGHT_MOTOR, rightU);

        Acquire(dist_mutex);
        if(dist_crit > 24.6){
            wanted_speed = -200;
            PlayTone(1000, 300);
            mountain_time = CurrentTick();
        }
        if((dist_crit < 24.0) && !mountain_in_past && (mountain_time != 0) && (CurrentTick() - mountain_time > 2000)){
            wanted_speed = WANTEDSPEED;
            mountain_in_past = true;
            PlayTone(400, 300);
        }
        Release(dist_mutex);
        str = NumToStr(dist_crit);
        WriteLnString(handle, str, str_size);
        Wait(WAIT_TIME);
    }
    Precedes(brick_go, brick_us);
}

task main()
{
    SetSensorLight(S2);
    SetSensorHTGyro(GYRO_PORT);

    DeleteFile("mount_us.txt");
    CreateFile("mount_us.txt", 20000, handle);
    Wait(2000);

    gyro_offset = getGyroOffset();
    PlayTone(400, 1000);
    Wait(5000);
    first = Sensor(S2);
    PlayTone(400, 1000);
    Wait(3000);
    PlayTone(400, 1000);
    Wait(1000);

    findLine(first, gyro_offset);
    Precedes(mountain_go, mountain_us);
}