benedettelli-nxt2wifi-test4.c

#pragma config(Sensor, S1,     TOUCH,          sensorTouch)
#pragma config(Sensor, S2,     COLOUR,         sensorCOLORFULL)
#pragma config(Sensor, S3,     SONAR,          sensorSONAR)
#pragma config(Sensor, S4,     NXT2WIFI,       sensorHighSpeed)
#pragma config(Motor,  motorA,          MOT_ACTION,          tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          MOT_LEFT,          tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          MOT_RIGHT,          tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/common.h"
#include "drivers/benedettelli-nxt2wifi.h"

#define DRIVESPEED  50
#define TURNSPEED   20
#define SHOOTSPEED  30

string IPaddress = "0.0.0.0";
string connStatus = "disconnected";
string dataStrings[5];
tHugeByteArray data;

int prevDetectedColour = 0;
int currDetectedColour = 0;

int prevBatteryLevel = 0;
int currBatteryLevel = 0;

int prevTouchState = 0;
int currTouchState = 0;

int prevSonarDistance = 0;
int currSonarDistance = 0;

long prevEncMotorA = 0;
long currEncMotorA = 0;
long prevEncMotorB = 0;
long currEncMotorB = 0;
long prevEncMotorC = 0;
long currEncMotorC = 0;


task updateScreen()
{
	while(true)
	{
		nxtDisplayTextLine(0, "Stat: %s", connStatus);
		nxtDisplayTextLine(1, "%s",IPaddress);
		nxtDisplayTextLine(2, "-------------------");
		nxtDisplayTextLine(3, "%s", dataStrings[0]);
		nxtDisplayTextLine(4, "%s", dataStrings[1]);
		nxtDisplayTextLine(5, "%s", dataStrings[2]);
		nxtDisplayTextLine(6, "%s", dataStrings[3]);
		nxtDisplayTextLine(7, "%s", dataStrings[4]);
		wait1Msec(100);
	}
}

void handleColour(ubyte state)
{
	switch (state)
	{
	case 3: SensorType[COLOUR] = sensorCOLORFULL;
		break;
	case 4: SensorType[COLOUR] = sensorCOLORRED;
		break;
	case 5: SensorType[COLOUR] = sensorCOLORGREEN;
		break;
	case 6: SensorType[COLOUR] = sensorCOLORBLUE;
		break;
	case 7: SensorType[COLOUR] = sensorCOLORNONE;
		break;
	}
}

void doStraight(ubyte state)
{
	motor[MOT_LEFT]  = state * DRIVESPEED;
	motor[MOT_RIGHT] = state * DRIVESPEED;
}

void doRight(ubyte state)
{
	motor[MOT_LEFT]  = state *   TURNSPEED;
	motor[MOT_RIGHT] = state * -TURNSPEED;
}

void doStop(ubyte state)
{
	motor[MOT_LEFT]  = 0;
	motor[MOT_RIGHT] = 0;
}

void doLeft(ubyte state)
{
	motor[MOT_LEFT]  = state * -TURNSPEED;
	motor[MOT_RIGHT] = state *  TURNSPEED;
}

void doReverse(ubyte state)
{
	motor[MOT_LEFT]  = state * -DRIVESPEED;
	motor[MOT_RIGHT] = state * -DRIVESPEED;
}

void doAction(ubyte state)
{
	motor[MOT_ACTION] = state * SHOOTSPEED;
}


task main ()
{
	ubyte type;
	ubyte ID;
	ubyte state;
	ubyte value;
	string dataString;
	string tmpString;

	// initialise the port, etc
	RS485initLib();

	StartTask(updateScreen);

	// Disconnect if already connected
	N2WDisconnect();
	N2WchillOut();
	wait1Msec(1000);
  if (!N2WCustomExist())
  {
    StopTask(updateScreen);
    wait1Msec(50);
    eraseDisplay();
    PlaySound(soundException);
    nxtDisplayCenteredBigTextLine(1, "ERROR");
    nxtDisplayTextLine(3, "No custom profile");
    nxtDisplayTextLine(4, "configured!!");
    while(true) EndTimeSlice();
  }

  N2WLoad();

	wait1Msec(100);

	N2WConnect(true);
	connStatus = "connecting";

	while (!N2WConnected()) wait1Msec(100);
	wait1Msec(1000);

	connStatus = "connected";
	PlaySound(soundBeepBeep);

	wait1Msec(3000);
	N2WgetIP(IPaddress);

	wait1Msec(1000);
	//                123456789012345
	dataStrings[0] = "Tch | Snr | Clr";
	//                on  | 011 |   1"
	while (true)
	{
		if (N2WreadWS(type, ID, state, value))
		{
			writeDebugStreamLine("btn: %d, state: %d", ID, state);
			switch (ID)
			{
			case 1: doStraight(state); break;
			case 3: doRight(state); break;
			case 4: doStop(state); break;
			case 5: doLeft(state); break;
			case 7: doReverse(state); break;
			case 9: doAction(state); break;
			case 11: handleColour(state); break;
			default: break;
			}
		}

		// All values are only updated when they've changed.
		// This cuts backs drastically on the number of messages
		// that have to be sent to the NXT2WIFI

		// Fetch the state of the Touch Sensor
		// This value is displayed by field 0 (in0) on the page
		currTouchState = SensorBoolean[TOUCH];
		if (currTouchState != prevTouchState)
		{
		  memset(data, 0, sizeof(data));
			data[0] = (currTouchState) ? '1' : '0';
			N2WwriteWS(1, 0, data, 2);
			prevTouchState = currTouchState;
			N2WchillOut();
		}

		// Fetch the currently detected colour.
		// This value is displayed by field 1 (in1) on the page
		currDetectedColour = SensorValue[COLOUR];
		if (currDetectedColour != prevDetectedColour)
		{
			sprintf(dataString, "%d", currDetectedColour);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 1, data, strlen(dataString));
			prevDetectedColour = currDetectedColour;
			N2WchillOut();
		}

		// Fetch the distance detected by the sonar sensor
		// This value is displayed by field 2 (in2) on the page
		currSonarDistance = SensorValue[SONAR];
		if (currSonarDistance != prevSonarDistance)
		{
			sprintf(dataString, "%d", currSonarDistance);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 2, data, strlen(dataString));
			prevSonarDistance = currSonarDistance;
			N2WchillOut();
		}

		// Fetch the tacho count for motor A
		// This value is displayed by field 3 (in3) on the page
		currEncMotorA = nMotorEncoder[MOT_ACTION];
		if (currEncMotorA != prevEncMotorA)
		{
			sprintf(dataString, "%d", currEncMotorA);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 3, data, strlen(dataString));
			prevEncMotorA = currEncMotorA;
			N2WchillOut();
		}

		// Fetch the tacho count for motor B
		// This value is displayed by field 4 (in4) on the page
		//currEncMotorB = nMotorEncoder[MOT_LEFT];
		if (currEncMotorB != prevEncMotorB)
		{
			sprintf(dataString, "%d", currEncMotorB);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 4, data, strlen(dataString));
			prevEncMotorB = currEncMotorB;
			N2WchillOut();
		}

		// Fetch the tacho count for motor C
		// This value is displayed by field 5 (in5) on the page
		currEncMotorC = nMotorEncoder[MOT_RIGHT];
		if (currEncMotorC != prevEncMotorC)
		{
			sprintf(dataString, "%d", currEncMotorC);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 5, data, strlen(dataString));
			prevEncMotorC = currEncMotorC;
			N2WchillOut();
		}

		// Fetch the current voltage level.  The average one
		// works best, the other one jumps around too much.
		// This value is displayed by field 6 (in6) on the page

		currBatteryLevel = nAvgBatteryLevel;
		if (currBatteryLevel != prevBatteryLevel)
		{
			sprintf(dataString, "%d", currBatteryLevel);
			memcpy(data, dataString, strlen(dataString));
			N2WwriteWS(1, 6, data, strlen(dataString));
			prevBatteryLevel = currBatteryLevel;
			N2WchillOut();
		}

		sprintf(dataStrings[2], "A: %d", currEncMotorA);
		sprintf(dataStrings[3], "B: %d", currEncMotorB);
		sprintf(dataStrings[4], "C: %d", currEncMotorC);
	  sprintf(tmpString, "%s | %3d", (currTouchState == 0) ? "off" : "on ", currSonarDistance);
		sprintf(dataStrings[1], "%s | %3d", tmpString, currDetectedColour);
	}
}