#pragma config(Sensor, S1, sonar, sensorSONAR)

#pragma config(Sensor, S2, light, sensorLightActive)

#pragma config(Motor, motorA, left, tmotorNormal, PIDControl, encoder)

#pragma config(Motor, motorB, right, tmotorNormal, PIDControl, encoder)

#pragma config(Motor, motorC, sonarMotor, tmotorNormal, PIDControl, encoder)

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

int sonarOffset = 4;

const float ENC_P_CM = 21.157;

const int drive_power = 30;

bool extend = false;

const int distance = 300;

void turnOut()

{

int boundary = 1;

int originWallDistance = 55;

nSyncedMotors = synchAB;

nSyncedTurnRatio = -100;

motor[motorA] = drive_power;

while(abs(SensorValue[sonar] - originWallDistance) > boundary)

;

motor[motorA] = 0;

}

void sonarTo(int degrees)

{

degrees = - degrees;

const int speed = 20;

int difference = nMotorEncoder[motorC] - degrees;

if (difference < 0)

{

motor[motorC] = speed;

while(nMotorEncoder[motorC] < degrees)

;

} else {

motor[motorC] = -speed;

while(nMotorEncoder[motorC] > degrees)

;

}

motor[motorC] = 0;

}

void follow_wall()

{

sonarTo(-90);

int balance = 0;

int reading = 0;

const int wall_follow_distance = 20;

const float damper = 1.25;

const int drive_power = 30;

const int base = drive_power/10;

nMotorPIDSpeedCtrl[motorA] = mtrSpeedReg;

nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;

motor[motorA] = drive_power;

motor[motorB] = drive_power;

while(SensorValue[S2] < 30)

{

reading = SensorValue[S1];

if(reading < 100)

{

balance = damper*(SensorValue[S1] - wall_follow_distance);

} else {

if (balance < 0)

{

balance = -base;

} else {

balance = base;

}

}

nxtDisplayCenteredTextLine(1, "bal: %d", balance);

motor[motorA] = drive_power - balance;

motor[motorB] = drive_power + balance;

}

sonarTo(0);

}

void driveToBackWall()

{

//Drive forwards till the sonar gives a reading of 21.

nSyncedMotors = synchAB;

nSyncedTurnRatio = 100;

motor[motorA] = 30;

while(SensorValue(sonar) > 21 - sonarOffset)

;

motor[motorA] = 0;

}

/*

void chamberAdjust()

{

//Sonar look left + take reading.

nMotorEncoder[motorC] = 0;

motor[motorC] = 5;

while(nMotorEncoder[motorC] < 90)

;

motor[motorC] = 0;

int leftReading = SensorValue(sonar);

nxtDisplayCenteredTextLine(1, "LR: %d", leftReading);

//Sonar look right + take reading.

motor[motorC] = -5;

while(nMotorEncoder[motorC] > -90)

;

motor[motorC] = 0;

int rightReading = SensorValue(sonar);

nxtDisplayCenteredTextLine(2, "RR: %d", rightReading);

wait1Msec(5000);

//Recenter sonar

motor[motorC] = 5;

while(nMotorEncoder[motorC] < 0)

;

motor[motorC] = 0;

//Adjust the position of the robot so that the left reading = right reading.

if(leftReading > rightReading)

{

turnRadiansClockwise(PI/2);

nSyncedMotors = synchAB;

nSyncedTurnRatio = 100;

motor[motorA] = 5;

while( SensorValue(sonar) > 21 - sonarOffset)

;

motor[motorA] = 0;

turnRadiansClockwise(-PI/2);

}

else if(leftReading < rightReading)

{

turnRadiansClockwise(-PI/2);

nSyncedMotors = synchAB;

nSyncedTurnRatio = 100;

motor[motorA] = 5;

while( SensorValue(sonar) > 21 - sonarOffset)

;

motor[motorA] = 0;

turnRadiansClockwise(PI/2);

}

}

*/

void beep()

{

//beep when done.

PlayImmediateTone(780, 100);

}

task main()

{

/*initialise encoders*/

nMotorEncoder[motorA] = 0;

nMotorEncoder[motorB] = 0;

nMotorEncoder[motorC] = 0;

turnOut();

follow_wall();

//chamberAdjust();

/*

turnOut();

follow_wall();

//chamberAdjust();

turnOut();

follow_wall();

//chamberAdjust();

*/

}