DIWIFI-test1.c

#pragma config(Motor,  motorA,          M1,            tmotorNormal, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h"
#include "drivers/DIWIFI-driver.h"
#include "drivers/DTMP-driver.h"

ubyte rxbuffer[1500];
//ubyte txbuffer[300];

typedef enum {
  WIFI_STATE_NONE = 0,
  WIFI_STATE_START,
  WIFI_STATE_SENDING,
  WIFI_STATE_READING,
  WIFI_STATE_LISTENING,
  WIFI_STATE_SUCCESS
} tWiFiState;




float RC_pow(long x, byte p)
{
  float result = x;
  if(p == 0) return 1;
  if(x > 0 && p < 0) return pow(x, p);
  if(x < 0 && p < 0) return (pow(abs(x), p)) * -1;
  for(ubyte count = 0; count < p-1; count++)
  {
    result *= x;
  }
  return result;
}

int RC_atoi(string IncomingData)
{
	const ubyte charToInt = 48;
  int num = 0;
	int digits = strlen(IncomingData);
	long magnitude = RC_pow(10, digits-1);

	for (int i = (digits - 1); i >= 0; i--)
	{
	  num += ((char)IncomingData - charToInt) * magnitude;
		magnitude /= 10;
		StringDelete(IncomingData, 0, 1);
	}
	return num;
}





void sendHeader(int cid) {
  ubyte linebuff[20];
  int index = 0;
  string tmpString;

  linebuff[0] = 27; // escape;
  linebuff[1] = 'S'; // the CID;
  linebuff[2] = (ubyte)cid + 48; // the CID;
  index = appendToBuff(buffer, index, linebuff, 3);

  tmpString = "HTTP/1.1 200 OK";
  memcpy(linebuff, tmpString, strlen(tmpString));
  index = appendToBuff(buffer, index, linebuff, strlen(tmpString));
  index = appendToBuff(buffer, index, linefeed, sizeof(linefeed));

  tmpString = "Content-Type:";
  memcpy(linebuff, tmpString, strlen(tmpString));
  index = appendToBuff(buffer, index, linebuff, strlen(tmpString));

  tmpString = " text/plain";
  memcpy(linebuff, tmpString, strlen(tmpString));
  index = appendToBuff(buffer, index, linebuff, strlen(tmpString));
  index = appendToBuff(buffer, index, linefeed, sizeof(newline));
  index = appendToBuff(buffer, index, linefeed, sizeof(newline));
  linebuff[0] = 27; // escape;
  linebuff[1] = 'E'; // the CID;
  index = appendToBuff(buffer, index, endmarker, 2);
  writeRawHS(buffer, index);
}


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_atoi(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));
  StringFormat(tmpString, "Temp: Soham");
  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;
  clear_read_buffer();
  closeConn(1);
  memset(rxbuffer, 0, sizeof(rxbuffer));
  //wait1Msec(100);
  Receive();
  //clear_read_buffer();
}


void parseInput()
{
  writeDebugStreamLine("Beging parsing...");
  ubyte BytesRead[20];
  ubyte currByte[] = {0};
  ubyte prevByte[] = {0};
  ubyte conn[] = {0};
  int cid;
  string tmpString;
  int index = 0;
  time1[T1] = 0;
  while (true)
  {
    if (time1[T1] > 300000)
      return;

    alive();
	  if (nxtGetAvailHSBytes() > 0)
	  {
      nxtReadRawHS(currByte[0], 1);
      if ((prevByte[0] == 27) && (currByte[0] == 'F')) {
        return;
      } else 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);
        if (cid > 1)
          return;
        while (true) {
          if (time1[T1] > 300000)
            return;

          while (nxtGetAvailHSBytes() == 0) EndTimeSlice();
          nxtReadRawHS(currByte[0], 1);

          if ((prevByte[0] == 27) && (currByte[0] == 'F')) {
            return;
          } else 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];
          if (index == sizeof(rxbuffer))
            return;

				}
				StringFromChars(tmpString, rxbuffer[0]);
				writeDebugStreamLine(tmpString);
				/*
				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 startDemon() {
  time1[T2] = 0;
  while (true) {
    if (time1[T2] > 1200000)
      return;
    PlaySound(soundBlip);
    closeAllConns();
    wait1Msec(500);
	  startListen(80);

	  clear_read_buffer();
	  parseInput();
	}
}

void startWifi() {
  while (true) {
	  configureWiFi();
	  set_verbose(false);
	  Receive();
	  wait1Msec(100);
	  startDemon();
	}
}

task main()
{
  long rate = 0;
  eraseDisplay();
  bNxtLCDStatusDisplay = true; // Enable top status line display
  writeDebugStream("Scanning for wifi sensor: ");
  rate = scanBaudRate();
  writeDebugStreamLine("%d baud", rate);
  startWifi();
//  configureWiFi();
//  set_verbose(false);
//  Receive();
//  wait1Msec(100);
  /*
  closeAllConns();
  wait1Msec(1000);
  clear_read_buffer();
  startListen(80);
  SensorType[COLOUR] = sensorCOLORRED;
  clear_read_buffer();
  parseInput();
  */
//  startDemon();
}