#pragma config(Sensor, S3, colorLeft, sensorCOLORFULL)

#pragma config(Sensor, S2, colorRight, sensorCOLORFULL)

#pragma config(Motor, motorC, left, tmotorNormal, openLoop, encoder)

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

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

/*------------------------------------------------------------------------

2013_master.c - 2013-04-13

Written for ROBOTC NXT 3.60

Property of BCCS Robotics team nKISA

Updated at https://github.com/mr64bit/2012-BCCS-ION-MUC

------------------------------------------------------------------------*/

//global variables

int dirIndex = 0; //which number in the array are we reading?

int whiteSpeed = 520; //in DPS

int yellowSpeed = 312;

int powerOutput = 0;

bool lineFollow = false;

int mean = 260;

float turn = 0;

int powerLeft = 0;

int powerRight = 0;

float motorDPS = 0; //white = 750 DPS, yellow = 450 DPS

int targetDPS = 0;

int lastPos = 0;

int lineReading = 0;

int sonar = 0;

int nAtoDValues[3]={0,0,0};

int red = 0, green = 0, blue = 0;

int stopSensor, lineSensor, lineColor;

bool turning = false;

bool parking = false;

bool resetMotors = false;

int count = 0;

const int parkDistance = 725;

int m_targetDistance = 0;

int m_targetSpeed = 0;

float m_turnRatio = 0;

bool m_controlType = false;

bool m_distanceReached = false;

int directions[50][4]; //just a large enough number, enough for 50 turns. the program

//will end when it comes to a "0"

void intDirections() //feed the directions into the array

{

directions[0][0] = 210;

directions[1][0] = 210;

directions[3-1][0] = 210;

directions[4-1][0] = 614;

directions[5-1][0] = 120;

directions[6-1][0] = 120;

directions[7-1][0] = 120;

directions[8-1][0] = 220;

directions[9-1][0] = 310;

directions[10-1][0] = 110;

directions[11-1][0] = 613;

directions[12-1][0] = 110;

directions[13-1][0] = 110;

directions[14-1][0] = 110;

directions[15-1][0] = 210;

directions[24-8-1][0] = 722;

directions[25-8-1][0] = 210;

directions[26-8-1][0] = 210;

directions[27-8-1][0] = 220;

directions[28-8-1][0] = 721;

directions[29-8-1][0] = 210;

directions[30-8-1][0] = 210;

directions[31-8-1][0] = 210;

directions[32-8-1][0] = 120;

directions[33-8-1][0] = 722;

directions[34-8-1][0] = 210;

directions[35-8-1][0] = 210;

directions[36-8-1][0] = 110;

directions[37-8-1][0] = 210;

directions[38-8-1][0] = 120;

directions[39-8-1][0] = 120;

directions[40-8-1][0] = 120;

directions[41-8-1][0] = 210;

directions[42-8-1][0] = 110;

directions[43-8-1][0] = 611;

directions[44-8-1][0] = 110;

//.....

//0**=start/end program;

//1**=straight;

//2**=left;

//3**=right;

//4**=left(loop);

//5**=right(loop);

//6**=parkingLeft;

//7**=parkingRight;

//9**=end in start/stop square

//*1*=follow left line;

//*2*=follow right line;

//**1,2,3,4 = park in X number of parking places from the beginning of the

wait1Msec(100); //parking lot. zero if not parking.

}

void jumpTo();

task line();

task sensors();

task motors();

void dirDecode();

void goStraight();

void turnLeft();

void turnRight();

void turnLeftL();

void turnRightL();

void parkLeft();

void parkRight();

void exit();

void squareLine(int direction);

void motorSet(int distance, int speed, float ratio, bool control);

task main()

{

intDirections();

dirDecode();

StartTask(sensors); //start all our other tasks

StartTask(motors);

StartTask(line);

jumpTo();

bFloatDuringInactiveMotorPWM = false; //make sure the motors brake, not float

// resetMotors = true;

while(true)

{

if(directions[dirIndex][0] == 000)

{

StopAllTasks();

}

eraseDisplay();

nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo

nxtDisplayBigStringAt(65, 55, "%d", dirIndex);

nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);

nMotorPIDSpeedCtrl[left] = mtrSpeedReg; //turn off the PID for the motors,

nMotorPIDSpeedCtrl[right] = mtrSpeedReg; //better reaction time

if(dirIndex>1)

{

if(directions[dirIndex-1][1] == 2 && directions[dirIndex][2] == 2) //if our last turn was a left turn AND we

{ //are going to follow the opposite line, move over to that side

turning = true;

motor[left] = 50;

while(nMotorEncoder[left] <= 200) {}

motor[left] = 0;

motor[right] = 50;

while(nMotorEncoder[right] <= 200) {}

motor[right] = 0;

turning = false;

}

if(directions[dirIndex-1][1] == 3 && directions[dirIndex][2] == 1) //same as last, except this is done if

{ //last turn was a right, AND we are

turning = true; //following the left line

motor[right] = 50;

while(nMotorEncoder[right] <= 200) {}

motor[right] = 0;

motor[left] = 50;

while(nMotorEncoder[left] <= 200) {}

motor[left] = 0;

turning = false;

}

}

resetMotors = true;

targetDPS = 450;

lineFollow = true;

wait1Msec(100);

if(directions[dirIndex][1] == 6 || directions[dirIndex][1] == 7) // if parking...

{

for(int i = 0; i < 30; i = i) //if we have seen blue continually for a

{ //certain amount of time,

if(lineColor == 2)

{

i++;

}

else

{

i = 0;

}

writeDebugStreamLine("lineColor is: %d", lineColor);//white to the debug

wait1Msec(10); //stream, better than

} //viewing variables over BT

resetMotors = true;

wait1Msec(20);

while(lastPos < ((parkDistance * (directions[dirIndex][3] - 1)) + 100))

{

wait1Msec(10);

}

lineFollow = false;

targetDPS = 0;

wait1Msec(500);

resetMotors = true;

wait1Msec(500);

turning = true;

switch(directions[dirIndex][1]) // which side of the road are we parking on?

{

case 6:

parkLeft();

break;

case 7:

parkRight();

break;

}

}

else // if not parking, follow the procedure for stopping

{

while(stopSensor != 5) // while the stopSensor is not seeing a stop sign

{

writeDebugStreamLine("stopsensor: %d", stopSensor);

wait1Msec(10); // keep following the line

}

writeDebugStreamLine("!!!STOPPED AT: %d", stopSensor);

PlaySound(soundBeepBeep);

lineFollow = false;

targetDPS = 0;

wait1Msec(500);

resetMotors = true;

wait1Msec(500);

turning = true;

// squareLine(1);

switch(directions[dirIndex][1]) // go to the respective turning functioin

{

case 1: goStraight(); break;

case 2: turnLeft(); break;

case 3: turnRight(); break;

case 4: turnLeftL(); break;

case 5: turnRightL(); break;

}

}

if(directions[dirIndex][1] == 9) //for exiting the city

{

while(lineColor != 5) {}

resetMotors = true;

wait1Msec(20);

while(lastPos < 200)

{

wait1Msec(10);

}

lineFollow = false;

targetDPS = 0;

wait1Msec(500);

resetMotors = true;

wait1Msec(500);

turning = true;

exit();

}

turning = false;

resetMotors = true;

//directions[dirIndex-1][1] = directions[dirIndex][1]; // needed for if we need to move over to the

dirIndex ++; //other side of the road after turn

// if(dirIndex == 14) // for an endless loop on our practice mat

// {

// dirIndex = 0;

// }

//dirDecode();

}

}

void jumpTo() //a function to jump to a specific point in our directions

{

while(nNxtButtonPressed != 3) //while the orange button is NOT pressed...

{

bNxtLCDStatusDisplay = true;

if(nNxtButtonPressed == 1) //if the right button is pressed...

{

dirIndex ++; //increse the index variable by 1

}

if(nNxtButtonPressed == 2) // if the left button is pressed...

{

dirIndex --; //decrese the index variable by 1.

}

if(dirIndex < 0) //to keep the index variable from going negative

{ //(no negative array slots)

dirIndex = 0;

}

eraseDisplay();

nxtDisplayRICFile(0, 0, "nKISA.RIC"); // displaying our logo

nxtDisplayBigStringAt(65, 55, "%d", dirIndex);

nxtDisplayStringAt(65, 25, "%d", directions[dirIndex][0]);

wait1Msec(200);

}

}

task line()

{

float kp = 10; // initialize the variables

float ki = 0.1;

float kd = 1;

float error = 0;

float integral = 0;

float lastError = 0;

float derivitive = 0;

while(true)

{

if(lineFollow == true)

{

error = lineReading - mean; //

integral = (integral + error) * .5; //

derivitive = (error - lastError); // the guts of the PID controler

if(error > 0) {error = error * 2.5;} //

turn = ((kp * error) + (ki * integral) + (kd * derivitive)) / 100;//

lastError = error; //

if(turning == true || (directions[dirIndex][1] == 6 || directions[dirIndex][1] == 7))

{

targetDPS = 300;

kp = 12;

ki = 0.2;

}

else

{

switch(lineColor)

{

case 2: targetDPS = whiteSpeed; kp = 15; ki = 0.1; mean = 182; break; //go slower

case 4: targetDPS = yellowSpeed; kp = 25; ki = 0.3; mean = 185; break; //on yellow

case 6: targetDPS = whiteSpeed; kp = 10; ki = 0.1; mean = 240; break; // roads

default: targetDPS = yellowSpeed; kp = 15; ki = 0.1; mean = 240; break;

}

}

}

else

{

error = 0;

lastError = 0;

integral = 0; // if not following a line, set the variables to 0

derivitive = 0;

turn = 0;

}

count++;

wait1Msec(10);

}

}

void dirDecode() // to separate a 3 digit number into 3 separate numbers

{

int index = 0;

while(directions[index][0] != 0)

{

directions[index][1] = directions[index][0] / 100; //take advantage of the fact that integers

directions[index][2] = (directions[index][0] / 10) - (directions[index][1] * 10); //round to the nearest

directions[index][3] = directions[index][0] - ((directions[index][1] * 100) + (directions[index][2] * 10)); //whole number

index++; // in the program

}

}

task sensors()

{

while(true)

{

switch(directions[dirIndex][2])

{

case 1: // if following left line

switch (SensorValue[colorLeft]) //assign numbers to the left color sensor

{ //and put it in the lineSensor variable

case BLACKCOLOR: lineSensor = 1; break;

case BLUECOLOR: lineSensor = 2; break;

case GREENCOLOR: lineSensor = 3; break;

case YELLOWCOLOR: lineSensor = 4; break;

case REDCOLOR: lineSensor = 5; break;

case WHITECOLOR: lineSensor = 6; break;

default: lineSensor = 0; break;

}

switch (SensorValue[colorRight]) //do the same thing to the right color

{ //sensor, but put the number in the stopSensor variable

case BLACKCOLOR: stopSensor = 1; break;

case BLUECOLOR: stopSensor= 2; break;

case GREENCOLOR: stopSensor = 3; break;

case YELLOWCOLOR: stopSensor = 4; break;

case REDCOLOR: stopSensor = 5; break;

case WHITECOLOR: stopSensor = 6; break;

default: stopSensor = 0; break;

}

getColorSensorData(colorLeft, colorAtoD, nAtoDValues); //get the individual

break; //color values from the line sensor

case 2: //same thing as above, but reversed because we are following the

switch (SensorValue[colorRight]) //other side of the line

{

case BLACKCOLOR: lineSensor = 1; break;

case BLUECOLOR: lineSensor = 2; break;

case GREENCOLOR: lineSensor = 3; break;

case YELLOWCOLOR: lineSensor = 4; break;

case REDCOLOR: lineSensor = 5; break;

case WHITECOLOR: lineSensor = 6; break;

default: lineSensor = 0; break;

}

switch (SensorValue[colorLeft])

{

case BLACKCOLOR: stopSensor = 1; break;

case BLUECOLOR: stopSensor = 2; break;

case GREENCOLOR: stopSensor = 3; break;

case YELLOWCOLOR: stopSensor = 4; break;

case REDCOLOR: stopSensor = 5; break;

case WHITECOLOR: stopSensor = 6; break;

default: stopSensor = 0; break;

}

getColorSensorData(colorRight, colorAtoD, nAtoDValues );

break;

}

red = nAtoDValues[0]; //take the color values from an array,

green = nAtoDValues[1]; //and put them in variables

blue = nAtoDValues[2];

if(lineSensor != 1) // if not seeing black, record that color

{

lineColor = lineSensor;

}

switch(lineColor) // depending on what color line we are seeing, use only some

{ //of the values, so that a low color count

case 3: lineReading = green; break; // doesn't lower the overall count

case 4: lineReading = (red + green) / 2; break;

case 2: lineReading = blue; break;

case 5: lineReading = red; break;

default: lineReading = (red + green + blue) / 3; break;

}

wait1Msec(10);

}

}

task motors()

{

const float m_kp = 0.005;

int m_error = 0;

while(true)

{

if(resetMotors == true)

{

nSyncedMotors = synchNone; // make motors independant

nMotorPIDSpeedCtrl[left] = mtrNoReg; // turn off the PID control

nMotorPIDSpeedCtrl[right] = mtrNoReg; //for both motors

nMotorEncoder[left] = 0;

nMotorEncoder[right] = 0;

lastPos = 0;

resetMotors = false;

}

lastPos = (nMotorEncoder[left] + nMotorEncoder[right]) / 2; //calculate how

wait1Msec(10); //fast the motors are moving

motorDPS = (((nMotorEncoder[left] + nMotorEncoder[right]) / 2) - lastPos)*100;

if(targetDPS != 0) //if we are supposed to be moving, adjust the motor

{ //power based on the target and actual speed

m_error = targetDPS - motorDPS;

powerOutput = powerOutput + (m_error * m_kp);

}

else // if we are supposed to be stopped, stop the motors

{

m_error = 0;

powerOutput = 0;

}

if(lineFollow == true) //if following the line, figure out

{ //how to control the motors

switch(directions[dirIndex][2])

{

case 1:

powerLeft = powerOutput + turn;

powerRight = powerOutput - turn;

if(powerLeft > 100) {powerRight = powerRight - turn;}

break;

case 2:

powerLeft = powerOutput - turn;

powerRight = powerOutput + turn;

if(powerRight > 100) {powerLeft = powerLeft - turn;}

break;

}

}

else // just go straight

{

powerLeft = powerOutput;

powerRight = powerOutput;

}

if(turning == false) // change motor power to follow line

{

motor[left] = powerLeft;

motor[right] = powerRight;

}

}

}

void motorSet(int distance, int speed, float ratio, bool control)

{

m_targetDistance = distance;

m_targetSpeed = speed;

m_turnRatio = ratio;

m_controlType = control;

while(control == false || m_distanceReached == true)

{

wait1Msec(100);

}

}

void goStraight() // I'm not going comment all the turn functions,

{ // because the're all pretty straight forward,

nSyncedMotors = synchCB;

nSyncedTurnRatio = 100;

motor[left] = 45;

while(nMotorEncoder[left] <= 750) {}

motor[left] = 0;

}

void turnLeft()

{

nSyncedMotors = synchBC;

nSyncedTurnRatio = 100;

motor[right] = 45;

while(nMotorEncoder[right] <= 450) {}

motor[right] = 0;

wait1Msec(100);

resetMotors = true;

wait1Msec(100);

nSyncedMotors = synchBC;

nSyncedTurnRatio = -120;

motor[right] = 45;

while(SensorValue(colorLeft) != 5) {}

motor[right] = 0;

}

void turnRight()

{

nSyncedMotors = synchCB;

nSyncedTurnRatio = 100;

motor[left] = 45;

while(nMotorEncoder[left] <= 450) {}

motor[left] = 0;

wait1Msec(100);

resetMotors = true;

wait1Msec(100);

nSyncedMotors = synchCB;

nSyncedTurnRatio = -120;

motor[left] = 45;

while(SensorValue(colorRight) != 5) {}

motor[left] = 0;

}

void turnLeftL()

{

nSyncedMotors = synchCB;

nSyncedTurnRatio = 65;

motor[left] = 45;

while(nMotorEncoder[left] <= 680) {}

motor[left] = 0;

wait1Msec(100);

resetMotors = true;

wait1Msec(100);

nSyncedMotors = synchBC;

nSyncedTurnRatio = 0;

motor[right] = 45;

while(nMotorEncoder[right] <= 740) {}

motor[right] = 0;

}

void turnRightL()

{

nSyncedMotors = synchBC;

nSyncedTurnRatio = 65;

motor[right] = 45;

while(nMotorEncoder[right] <= 680) {}

motor[right] = 0;

wait1Msec(100);

resetMotors = true;

wait1Msec(100);

nSyncedMotors = synchCB;

nSyncedTurnRatio = 0;

motor[left] = 45;

while(nMotorEncoder[left] <= 710) {}

motor[left] = 0;

}

void squareLine(int direction) // square up on the line using both sensors

{ // originally written in NXT-G for BOTS4HIM, 2010 FLL

int index = 0;

bool done = false;

while(done != true)

{

if(SensorValue(colorLeft) == 2)

{

index++;

}

if(SensorValue(colorRight) == 2)

{

index = index + 2;

}

switch(index)

{

case 1:

motor[left] = 0;

motor[right] = 35 * direction;

break;

case 2:

motor[left] = 35 * direction;

motor[right] = 0;

break;

case 3:

motor[left] = 0;

motor[right] = 0;

done = true;

break;

default:

motor[left] = 35 * direction;

motor[right] = 35 * direction;

break;

}

index = 0;

wait1Msec(10);

}

}

void parkLeft()

{

PlaySound(soundBeepBeep);

nSyncedMotors = synchCB;

nSyncedTurnRatio = 25;

motor[right] = 45;

while((lineSensor != 4) && (lineSensor != 6)) {}

motor[right] = 0;

resetMotors = true;

targetDPS = 350;

turning = false;

lineFollow = true;

while(stopSensor != 6) {}

lineFollow = false;

targetDPS = 0;

wait1Msec(10);

resetMotors = true;

turning = true;

wait1Msec(250);

nMotorPIDSpeedCtrl[left] = mtrSpeedReg;

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

motor[left] = -45;

while(nMotorEncoder[left] > -100) {}

motor[left] = 0;

wait1Msec(250);

motor[right] = -45;

while(nMotorEncoder[right] > -110) {}

motor[right] = 0;

wait1Msec(10);

resetMotors = true;

wait1Msec(5000);

squareLine(-1);

wait1Msec(10);

resetMotors = true;

wait1Msec(10);

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

nSyncedMotors = synchCB;

nSyncedTurnRatio = 10;

motor[right] = -50;

while(nMotorEncoder[right] > -750) {}

motor[right] = 0;

}

void parkRight()

{

PlaySound(soundBeepBeep);

nSyncedMotors = synchBC;

nSyncedTurnRatio = 25;

motor[left] = 45;

while((lineSensor != 4) && (lineSensor != 6)) {}

motor[left] = 0;

resetMotors = true;

targetDPS = 350;

turning = false;

lineFollow = true;

while(stopSensor != 6) {}

lineFollow = false;

targetDPS = 0;

wait1Msec(10);

resetMotors = true;

turning = true;

wait1Msec(250);

nMotorPIDSpeedCtrl[left] = mtrSpeedReg;

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

motor[right] = -35;

while(nMotorEncoder[right] > -100) {}

motor[right] = 0;

wait1Msec(250);

motor[left] = -35;

while(nMotorEncoder[left] > -110) {}

motor[left] = 0;

wait1Msec(10);

resetMotors = true;

wait1Msec(5000);

squareLine(-1);

wait1Msec(10);

resetMotors = true;

wait1Msec(10);

nMotorPIDSpeedCtrl[left] = mtrSpeedReg;

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

nSyncedMotors = synchBC;

nSyncedTurnRatio = 10;

motor[left] = -50;

while(nMotorEncoder[left] > -750) {}

motor[left] = 0;

}

void exit()

{

PlaySound(soundBeepBeep);

nSyncedMotors = synchCB;

nSyncedTurnRatio = 25;

motor[right] = 45;

while((lineSensor != 4) && (lineSensor != 6)) {}

motor[right] = 0;

resetMotors = true;

targetDPS = 350;

turning = false;

lineFollow = true;

while(stopSensor != 6) {}

lineFollow = false;

targetDPS = 0;

wait1Msec(10);

resetMotors = true;

turning = true;

wait1Msec(10);

nMotorPIDSpeedCtrl[left] = mtrSpeedReg;

nMotorPIDSpeedCtrl[right] = mtrSpeedReg;

motor[left] = -45;

while(nMotorEncoder[left] > -130) {}

motor[left] = 0;

wait1Msec(500);

motor[right] = -45;

while(nMotorEncoder[right] > -130) {}

motor[right] = 0;

wait1Msec(10);

}