Система ранней диагностики

#pragma config(Sensor, S1, camera1, sensorI2CCustomFast)

#pragma config(Sensor, S2,, sensorTouch)

#pragma config(Motor, motorA, A, tmotorNXT, PIDControl, encoder)

#pragma config(Motor, motorB, B, tmotorNXT, PIDControl, encoder)

#pragma config(Motor, motorC, C, tmotorNXT, PIDControl, encoder)

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

 

/*

Color Recognition - ROBOTC for the NXT

 

Description: The NXT displays the name of the color returned by the Mindsensors NXTCam v2

 

NXT Configuration:

- Mindsensors NXTCam v2 plugged into NXT Sensor Port 1

 

Camera Configuration:

- Color Index 0: Red

- Color Index 1: Orange

- Color Index 2: Yellow

- Color Index 3: Green

- Color Index 4: Blue

- Color Index 5: Purple

- Color Index 6: Black

- Color Index 7: Brown

 

Notes:

- As of October 2008, NXTCam color index must be configured through

NXTCamView, found at: http://nxtcamview.sourceforge.net/

- More information can be found at: http://mindsensors.com/index.php?module=pagemaster&PAGE_user_op=view_page&PAGE_id=78

*/

 

//Include NXTCam Library

#include "NXTCamRobotCSamplePrograms for RobotC/nxtcamlib.c"

 

//Define global variables

string colorName;

int numBlobs,a=0,b=0,c=0, iteration = 0;

bool foundRed = false;

int_array blobColor;

int_array blobX1;

int_array blobTop;

int_array blobX2;

int_array blobBottom;

 

void printColorName();

void carryUp();

void carryDown();

void dropToCure();

void dropToRelease();

 

task main()

{

//Initialize the NXTCam

init_camera(camera1);

//Recognize Colors Forever

while (true)

{

printColorName();

 

nxtDisplayTextLine(5, "iter =%d", iteration);

 

if (foundRed)

{

carryUp();

dropToCure();

carryDown();

dropToRelease();

}

 

iteration++;

 

wait1Msec(500);

}

 

}

 

void printColorName()

//void printColorName() - Writes the color returned from the camera to the NXT screen

{

//Retrieves Camera Data

init_camera(camera1);

get_blobs(camera1, numBlobs, blobColor, blobX1, blobTop, blobX2, blobBottom);

 

nxtDisplayTextLine(1, "color:");

nxtDisplayTextLine(7, "numBlobs=%d", numBlobs);

colorName = "NONE ";

foundRed = false;

 

if(numBlobs>0) //If at least 1 "blob" or color is seen...

{

if(blobColor == 0) //...check the blobColor index.

{

colorName = "RED ";

foundRed = true;

a = 1;

}

else

{

colorName = "OTHER ";

foundRed = false;

a = 0;

}

}

nxtDisplayTextLine(3, colorName);

}

 

void carryUp() // carry the seal up by the carriage

{

nMotorEncoder[A]=0;

while(nMotorEncoder[A]>=-790)

{

motor[A]=-10;

nxtDisplayTextLine(5,"A=%d",nMotorEncoder[A]);

wait1Msec(100);

}

motor[A]=0;

}

 

void carryDown() // bring carriage to initial position

{

while(nMotorEncoder[A]!=-10)

{

motor[A]=50;

nxtDisplayTextLine(5,"A=%d",nMotorEncoder[A]);

motor[A]=0;

}

}

 

void dropToCure() // drop the seas from the carriage

{

while(SensorValue[S2]==0)

{

motor[B]=20;

}

motor[B]=0;

sleep(100);

motor[B] = -11;

sleep(2000);

motor[B]=0;

}

 

void dropToRelease()

{

motor[C]=-15;

sleep(2000);

motor[C]=15;

sleep(2000);

motor[C]=0;

}