bool NMCFile::loadFile(const std::string& filename)
{
FILE* handle = NULL;
try{
handle = fopen(filename.c_str(), "rb");
if(handle == NULL) return false;
std::unique_ptr<char> name1(new char[33]); // US-ASCII chars
std::unique_ptr<char> name2(new char[33]);
unsigned int len = 0;
if(fread(name1.get(), sizeof(char), 32, handle) != 32){
fclose(handle);
return false;
}
if(fread(name2.get(), sizeof(char), 32, handle) != 32){
fclose(handle);
return false;
}
if(fread(&len, sizeof(unsigned int), 1, handle) != 1){ // assumes little endianess here...
fclose(handle);
return false;
}
if(len > 10000){ // sanity check [in practice values must be always "small"]
fclose(handle);
return false;
}
name1.get()[32] = '\0';
name2.get()[32] = '\0';
// next we read actual "program" data (floats)
std::unique_ptr<float> program1(new float[len]); // 32bit floats [little endian]
std::unique_ptr<float> program2(new float[len]);
if(fread(program1.get(), sizeof(float), len, handle) != len){
fclose(handle);
return false;
}
if(fread(program2.get(), sizeof(float), len, handle) != len){
fclose(handle);
return false;
}
// all data has been successfully read, stores values to internal class variables
signalName[0] = std::string(name1.get());
signalName[1] = std::string(name2.get());
// trimming
signalName[0].erase(signalName[0].find_last_not_of(" ")+1);
signalName[1].erase(signalName[1].find_last_not_of(" ")+1);
program[0].resize(len);
program[1].resize(len);
for(unsigned int i=0;i<len;i++){
program[0][i] = program1.get()[i];
program[1][i] = program2.get()[i];
}
fclose(handle);
return true;
}
catch(std::exception& e){
return false;
}
}
task main()
{
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |knock down poles
2 parking |knock down poles
3 |
4 |
5 |
6 |
7 |
8 |
10 |
11 |
12 |
13 |
14 |
15 anywhere|go straight 5 seconds
*/
wait1Msec(2000);
disableDiagnosticsDisplay();
short colorValues[4];//these are the scanned values
int j;
long r;
long g;
long b;
// wait1Msec(500);
int minDifference = 32767; // MAX_INT
int selectedColor = 0;//this is the color closest to the scanned color
while(true)
{
for(j =0; j<4; j++)//get color sensor values
{
getColorSensorData(colorPort, 0, &colorValues[j]);
r = colorValues[0];
g = colorValues[1];
b = colorValues[2];
}
for(int i = 0; i < COLOR_COUNT; i++)//here we find the closest color in our database
{
difference(//passing in the database values and the scanned values to find the difference
colorTags[i][0],
colorTags[i][1],
colorTags[i][2],
r, g, b);
long diff = thediff;
if (diff <= minDifference) {//if the difference between the color in the database that we are currently checking and the scanned color
minDifference = diff; //is less than the smallest difference between the other database colors, then the database color we are
selectedColor = i; //currently checking is the closest match so far
}
nxtDisplayCenteredTextLine(0, "%d, %d", diff, minDifference);//print all the values
nxtDisplayCenteredTextLine(1, "%d,%d,%d", colorValues[0], colorValues[1], colorValues[2]);
nxtDisplayCenteredTextLine(4, "%d,%d,%d,", colorTags[i][0], colorTags[i][1], colorTags[i][2]);
}
nxtDisplayCenteredTextLine(2, "%s", colornames[selectedColor]);
nxtDisplayCenteredTextLine(3, "Is this the color?");
if(nNxtButtonPressed==2)
{
break;
}
}
waitForStart();
switch(selectedColor)
{
case 1:
program1();
break;
case 2:
program2();
break;
/*case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
break;*/
case 15:
program15();
break;
default:
program15();
break;
}
}
//----------------------------------------------------------------------------START TASK MAIN---------------------------------------------
task main()
{
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |drop ball in small goal and move to parking
2 parking |drop ball in small goal and move to parking
3 |
4 |
5 |
6 |
7 |
8 |
10 |
11 |
12 |
13 |
14 |
15 anywhere|go straight 6 feet (can get of ramp)
*/
//wait1Msec(2000);
disableDiagnosticsDisplay();
short colorValues[4];//these are the scanned values
int j;
long r;
long g;
long b;
// wait1Msec(500);
int minDifference = 32767; // MAX_INT
int selectedColor = 0;//this is the color closest to the scanned color
servo[gate]=gateup;
// gostraight
sFRS = 161;
sBRS = 154;
sFLS = 132;
sBLS = 131;
servo[frontRS]= sFRS;
servo[backRS]= sBRS;
servo[frontLS]= sFLS;
servo[backLS]= sBLS;
wait1Msec(500);
servo[leftmandible] = openwideL;
servo[rightmandible] = openwideR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
servo[mouth]= mouthup;
turnServos(1,0);
wait1Msec(1000);
/*
while(true)
{
for(j =0; j<4; j++)//get color sensor values
{
getColorSensorData(colorPort, 0, &colorValues[j]);
r = colorValues[0];
g = colorValues[1];
b = colorValues[2];
}
for(int i = 0; i < COLOR_COUNT; i++)//here we find the closest color in our database
{
difference(//passing in the database values and the scanned values to find the difference
colorTags[i][0],
colorTags[i][1],
colorTags[i][2],
r, g, b);
long diff = thediff;
if (diff <= minDifference) {//if the difference between the color in the database that we are currently checking and the scanned color
minDifference = diff; //is less than the smallest difference between the other database colors, then the database color we are
selectedColor = i; //currently checking is the closest match so far
}
nxtDisplayCenteredTextLine(0, "%d, %d", diff, minDifference);//print all the values
nxtDisplayCenteredTextLine(1, "%d,%d,%d", colorValues[0], colorValues[1], colorValues[2]);
nxtDisplayCenteredTextLine(4, "%d,%d,%d,", colorTags[i][0], colorTags[i][1], colorTags[i][2]);
}
nxtDisplayCenteredTextLine(2, "%s", colornames[selectedColor]);
nxtDisplayCenteredTextLine(3, "Is this the color?");
if(nNxtButtonPressed==2)
{
break;
}
}
waitForStart();*/
servo[gate]=gatedrive;
/*
switch(selectedColor)
{
case 1:
program1();
break;
case 2:
program2();
break;
/*case 3:
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
break;*//*
case 15:
program15();
break;
default:
program15();
break;
}
*/
program1();
}
//----------------------------------------------------------------------------START TASK MAIN---------------------------------------------
task main()
{
// clearDebugStream();
//waitForStart();
/*
list of programs
starting |what to
position |do
______________________________
1 ramp |drop ball in small goal and move to parking
2 parking |drop ball in small goal and move to parking//values not tested
3 parking |score in center goal
4 parking |knocks down kickstand
5 parking |messes up oponents rolling goals
6 |
7 parking |get in way of opponent 120 goal
8 |
9 |
10 |
11 ramp |drop ball in small goal and move to parking(with compass)
12 parking |drop ball in small goal and move to parking//values not tested
13 |
14 |
15 anywhere|go straight 6 feet (can get of ramp)
21 ramp |drop ball in small goal and move to parking (BACKWARDS)
3|lightblue//program1
3|darkblue
5|darkyellow
7|pink
7|red//program3
7|orange
13|lightgreen
14|white//program11
14|tan
14|brown
14|lightyellow
17|purple
17|black//default(program15)
*/
//wait1Msec(2000);
// disableDiagnosticsDisplay();
short colorValues[4];//these are the scanned values
int j;
long r;
long g;
long b;
// wait1Msec(500);
int minDifference = 32767; // MAX_INT
int selectedColor = 0;//this is the color closest to the scanned color
servo[gate]=gateup;
// gostraight
sFRS = 161;
sBRS = 154;
sFLS = 132;
sBLS = 131;
servo[frontRS]= sFRS;
servo[backRS]= sBRS;
servo[frontLS]= sFLS;
servo[backLS]= sBLS;
wait1Msec(500);
//servo[leftmandible] = outL;
//servo[rightmandible] = outR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
servo[mouth]= mouthup;
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
selectedColor=HTCS2readColor(colorport);
turnServos(1,0);
clearDebugStream();
/* while(true)
{
if(!HTCS2readRGB(colorport, r, g,b))
{
writeDebugStream("BAD\n");
}
writeDebugStream("%d, %d, %d\n", r,g,b);
//writeDebugStream("----------------\n");
wait1Msec(1000);
}
*/
// wait1Msec(4000);
/*
3|lightblue//program1
3|darkblue
5|darkyellow
7|pink
7|red//program3
7|orange
13|lightgreen
14|white//program11
14|tan
14|brown
14|lightyellow
17|purple
17|black//default(program15)
*/
/* while(true)
{
for(j =0; j<4; j++)//get color sensor values
{
// x=HTCS2readRGB(colorport, r, g,b);
//getColorSensorData(colorport, 0, &colorValues[j]);
r = colorValues[0];
g = colorValues[1];
b = colorValues[2];
// writeDebugStream("R:%d,G:%d,B:%d\n",r,g,b);
//writeDebugStream("color:%d\n",HTCS2readColor(colorport));
}
}*//*
for(int x=0; x<10; x++)//test values a couple of times to make sure we have the right one
{
for(int i = 0; i < COLOR_COUNT; i++)//here we find the closest color in our database
{
difference(//passing in the database values and the scanned values to find the difference
colorTags[i][0],
colorTags[i][1],
colorTags[i][2],
r, g, b);
long diff = thediff;
if (diff <= minDifference) {//if the difference between the color in the database that we are currently checking and the scanned color
minDifference = diff; //is less than the smallest difference between the other database colors, then the database color we are
selectedColor = i; //currently checking is the closest match so far
}
nxtDisplayCenteredTextLine(0, "%d, %d", diff, minDifference);//print all the values
nxtDisplayCenteredTextLine(1, "%d,%d,%d", colorValues[0], colorValues[1], colorValues[2]);
nxtDisplayCenteredTextLine(4, "%d,%d,%d,", colorTags[i][0], colorTags[i][1], colorTags[i][2]);
}
}
nxtDisplayCenteredTextLine(2, "%s", colornames[selectedColor]);
nxtDisplayCenteredTextLine(3, "Is this the color?");
if(nNxtButtonPressed==2)
{
break;
}
}
*/
// disableDiagnosticsDisplay();
// eraseDisplay();
switch(selectedColor)
{
case 1:
case 3:
nxtDisplayCenteredTextLine(2,"BLUE");
break;
case 7:
nxtDisplayCenteredTextLine(2,"RED");
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
nxtDisplayCenteredTextLine(2,"WHITE");
break;
case 15:
break;
case 16:
break;
case 17:nxtDisplayCenteredTextLine(2,"BLACK");
break;
default:
nxtDisplayCenteredTextLine(2,"BLACK");
break;
}
turnServos(1,0);
waitForStart();
servo[gate]=gatedrive;
switch(selectedColor)
{
case 1:
break;
case 2:
break;
case 3:
program1();
break;
case 4:
break;
case 5:
break;
case 6:
break;
case 7:
writeDebugStream("7");
program3();
break;
case 8:
break;
case 9:
break;
case 10:
break;
case 11:
break;
case 12:
break;
case 13:
break;
case 14:
program16();
break;
case 15:
break;
case 16:
break;
program15();
break;
default:
program15();
break;
}
// program1();
//while(true){godirection(1,120);}
//turnleft90();
//compassturn(180);
//compassturn(180);
//while(true)
// godirection(4,120);
//program1();
//turnServos(4,0);
//while(true);
}
