task main()
{
eraseDisplay();
bNxtLCDStatusDisplay = true; // Enable top status line display
setupHighSpeedLink(true);
//
// Configure the link for raw read and write. User program will have complete control over the link.
// User program will be responsible for managing the half-duplex operation and must prevent collisions!
//
nxtHS_Mode = hsRawMode;
while(true)
{
char ILOVEHKK[10] = {'I',' ','L','O','V','E',' ','H','K','K'};
transmitList = 'I';
count = 1;
TransmitASCII();
Receive();
string receivedcharacters;
StringFromChars(receivedcharacters, receiveList);
nxtDisplayString(30, receivedcharacters);
wait10Msec(100);
eraseDisplay();
}
}
void parseInput()
{
writeDebugStreamLine("Beging parsing...");
ubyte BytesRead[20];
ubyte currByte[] = {0};
ubyte prevByte[] = {0};
ubyte conn[] = {0};
int cid;
string tmpString;
int index = 0;
while (true)
{
alive();
if (nxtGetAvailHSBytes() > 0)
{
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'S')) {
index = 0;
memset(rxbuffer, 0, sizeof(rxbuffer));
wait1Msec(1);
nxtReadRawHS(conn[0], 1);
cid = conn[0] - 48;
writeDebugStreamLine("Conn: %d", cid);
while (true) {
while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
nxtReadRawHS(currByte[0], 1);
if ((prevByte[0] == 27) && (currByte[0] == 'E')) {
rxbuffer[index--] = 0;
rxbuffer[index--] = 0;
PlaySound(soundShortBlip);
while(bSoundActive) EndTimeSlice();
break;
}
prevByte[0] = currByte[0];
rxbuffer[index++] = currByte[0];
}
for (int i = 0; i < ((index / 19) + 1); i++) {
memset(BytesRead[0], 0, 20);
memcpy(BytesRead[0], rxbuffer[i*19], 19);
StringFromChars(tmpString, BytesRead);
writeDebugStream(tmpString);
}
genResponse(cid);
}
prevByte[0] = currByte[0];
}
}
}
void genResponse(int cid) {
int power = motor[motorA];
string tmpString;
int index = 0;
ubyte linebuff[20];
StringFromChars(tmpString,rxbuffer);
index = StringFind(tmpString, "/");
StringDelete(tmpString, 0, index);
index = StringFind(tmpString, "HTTP");
StringDelete(tmpString, index, strlen(tmpString));
writeDebugStreamLine("Request:%s", tmpString);
nxtDisplayTextLine(2, "Request: ");
nxtDisplayTextLine(3, tmpString);
if (StringFind(tmpString, "MOTA") > 0) {
StringDelete(tmpString, 0, 6);
index = StringFind(tmpString, " ");
if (index > -1)
StringDelete(tmpString, index, strlen(tmpString));
//power = RC_atoix(tmpString);
power = clip(RC_atoix(tmpString), -100, 100);
writeDebugStreamLine("Power:%d", power);
} else {
writeDebugStreamLine("NO POWER: %s", tmpString);
}
sendHeader(cid);
while(nxtHS_Status == HS_SENDING) wait1Msec(5);
wait1Msec(100);
index = 0;
linebuff[0] = 27; // escape;
linebuff[1] = 'S'; // the CID;
linebuff[2] = (ubyte)cid + 48; // the CID;
index = appendToBuff(buffer, index, linebuff, 3);
StringFormat(tmpString, "MotorA=%d", power);
memcpy(linebuff, tmpString, strlen(tmpString));
index = appendToBuff(buffer, index, linebuff, strlen(tmpString));
linebuff[0] = 27; // escape;
linebuff[1] = 'E'; // the CID;
index = appendToBuff(buffer, index, endmarker, 2);
writeRawHS(buffer, index);
motor[motorA] = power;
closeConn(1);
memset(rxbuffer, 0, sizeof(rxbuffer));
wait1Msec(100);
clear_read_buffer();
}
task main () {
string w_string = "MindstormsNXT";
string r_string;
tByteArray data;
memcpy(data, w_string, sizeof(w_string));
EEPROMwriteBytes(EEPROM, 0, data, 13);
// After a write, you *must* wait 5 msec
sleep(5);
eraseDisplay();
memset(data, 0, sizeof(tByteArray));
EEPROMreadBytes(EEPROM, 0, data, 13);
// Make sure the string is 0 terminated.
data[13] = 0;
StringFromChars(r_string, &data[0]);
displayTextLine(1, "%s", r_string);
sleep(2000);
}
int JedkNejblizsimu(){
dokoncil = false;
while(!dokoncil){
HledejNejkratsiCestu();
string cesta;
char ccesta[20];
for(int i=0; i<20 && znovu == false; i++) ccesta[i] = nejkratsiCesta[i];
StringFromChars(cesta, ccesta);
nxtDisplayCenteredTextLine(0, cesta);
for(int i=0; i<cestaSize; i++){
if(nejkratsiCesta[i] == 0){
dokoncil = true;
break;
}
else if(nejkratsiCesta[i] == 'R'){
cleanExit = true;
Rovne(false);
}
else if(nejkratsiCesta[i] == '0'){
if(PoziceRobota[2] == 3) VpravoC();
else if(PoziceRobota[2] == 1) VlevoC();
else{ Vlevo(); Vlevo();}
}else if(nejkratsiCesta[i] == '1'){
if(PoziceRobota[2] == 0) VpravoC();
else if(PoziceRobota[2] == 2) VlevoC();
else{ Vlevo(); Vlevo();}
}else if(nejkratsiCesta[i] == '2'){
if(PoziceRobota[2] == 1) VpravoC();
else if(PoziceRobota[2] == 3) VlevoC();
else{ Vlevo(); Vlevo();}
}else if(nejkratsiCesta[i] == '3'){
if(PoziceRobota[2] == 2) VpravoC();
else if(PoziceRobota[2] == 0) VlevoC();
else{ Vlevo(); Vlevo();}
}
}
}
return nejkratsiCesta_size();
}
wStep* wSequence_getNext() {
if ( _nowReadIndex >= _nowWriteIndex)
return NULL;
if ( _nowWriteIndex < 0)
return NULL;
_nowReadIndex++;
char cStep[_maxActionNameLength];
for (int i=0; i<_maxActionNameLength ; i++) {
cStep[i] = _wStepStrArr[_nowReadIndex][i];
}
string strStep="";
StringFromChars(strStep, cStep);
wSequence_parseWStep(strStep);
return &_oneStctStep;
}
void genResponse(int cid) {
int power = motor[motorA];
float temp = 0.0;
string tmpString;
int index = 0;
ubyte linebuff[20];
StringFromChars(tmpString,rxbuffer);
index = StringFind(tmpString, "/");
StringDelete(tmpString, 0, index);
index = StringFind(tmpString, "HTTP");
StringDelete(tmpString, index, strlen(tmpString));
writeDebugStreamLine("Request:%s", tmpString);
nxtDisplayTextLine(2, "Request: ");
nxtDisplayTextLine(3, tmpString);
if (StringFind(tmpString, "MOTA") > 0) {
StringDelete(tmpString, 0, 6);
index = StringFind(tmpString, " ");
if (index > -1)
StringDelete(tmpString, index, strlen(tmpString));
//power = RC_atoix(tmpString);
power = clip(atoi(tmpString), -100, 100);
writeDebugStreamLine("Power:%d", power);
} else {
writeDebugStreamLine("NO POWER: %s", tmpString);
}
sendHeader(cid);
while(nxtHS_Status == HS_SENDING) wait1Msec(5);
wait1Msec(100);
index = 0;
linebuff[0] = 27; // escape;
linebuff[1] = 'S'; // the CID;
linebuff[2] = (ubyte)cid + 48; // the CID;
index = appendToBuff(buffer, index, linebuff, 3);
StringFormat(tmpString, "MotorA=%d\n", power);
memcpy(linebuff, tmpString, strlen(tmpString));
index = appendToBuff(buffer, index, linebuff, strlen(tmpString));
DTMPreadTemp(DTMP, temp);
StringFormat(tmpString, "Temp: %2.2f C", temp);
memcpy(linebuff, tmpString, strlen(tmpString));
index = appendToBuff(buffer, index, linebuff, strlen(tmpString));
linebuff[0] = 27; // escape;
linebuff[1] = 'E'; // the CID;
index = appendToBuff(buffer, index, endmarker, 2);
writeRawHS(buffer, index);
if (power != 0) nMotorEncoderTarget[motorA] = 2000;
motor[motorA] = power;
if (power > 0)
SensorType[COLOUR] = sensorCOLORGREEN;
else if (power < 0)
SensorType[COLOUR] = sensorCOLORBLUE;
else if (nMotorRunState[motorA] == runStateIdle)
SensorType[COLOUR] = sensorCOLORRED;
else
SensorType[COLOUR] = sensorCOLORRED;
wait1Msec(300);
clear_read_buffer();
closeConn(1);
memset(rxbuffer, 0, sizeof(rxbuffer));
//wait1Msec(100);
Receive();
//clear_read_buffer();
}