bool readAndSend(TMailboxIDs incomingMailbox, TMailboxIDs outGoingMailbox)
{
  TFileIOResult nBTCmdErrorStatus;
  int nSizeOfMessage;

    // Check to see if a message is available

//  waitBtIdle();
    nSizeOfMessage = cCmdMessageGetSize(incomingMailbox);
    while (nSizeOfMessage == 0)
    {
      nSizeOfMessage = cCmdMessageGetSize(incomingMailbox);
      wait1Msec(50);
    }

//      PlaySound(soundLowBuzz);
      if (nSizeOfMessage > dataSize)
        nSizeOfMessage = dataSize;
      waitBtIdle();
      nBTCmdErrorStatus = cCmdMessageRead(dataReceived, nSizeOfMessage, incomingMailbox);
	    waitBtIdle();

      if(dataReceived[0]==1) {
//	  TFileIOResult res = cCmdBTCheckStatus(0);
	        prepareDataToSend();
	        waitBtIdle();
          nBTCmdErrorStatus = cCmdMessageWriteToBluetooth(0, dataToSend, dataSize, outGoingMailbox);
    	    waitBtIdle();
          count++;
        }
  return false;
}
示例#2
0
void check_bleutooth(string *s){
	/*
		This function makes a bleutooth connection and then waits for input from the connected phone
	*/
	TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

	while(1){
		//wait on bleutooth
		 nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage){//make the message shorter if its to long
      nSizeOfMessage = kMaxSizeOfMessage;
    }

    if (nSizeOfMessage > 0){//if the message is not empty, put the received information in string s
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	stringFromChars(*s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, *s);
    	break;
    }

    wait1Msec(100);//no need to do a continues check every 100 miliseconds is enough
	}
}
void purgeBT(TMailboxIDs box) {
  while(cCmdMessageGetSize(box)>0) {
    cCmdMessageRead(dataReceived, cCmdMessageGetSize(box), box);
    wait1Msec(100);
  }
//  while(nxtReadRawBluetooth(dataReceived, dataSize)!=0);
}
void readDataMsg()
{
	int mSizeOfMessage = cCmdMessageGetSize(queue);
	ubyte mBuffer[MaxSizeOfMessage];// no malloc with RobotC :(

	if (mSizeOfMessage <= 0)//there's no message
	{
		wait1Msec(1);//wait before we check again
		return;
	}
	if (mSizeOfMessage > MaxSizeOfMessage)// shouldn't happen but well, then we'll only read the first part of the message
	{
		mSizeOfMessage = MaxSizeOfMessage;
	}

	if (cCmdMessageRead(mBuffer, mSizeOfMessage, queue) == ioRsltSuccess)//we've successfully copied the message
	{
		mSizeOfMessage -= 1;// skip the terminator

		// do whatever you want here
		for (int i = 0; i < mSizeOfMessage; i++)
		{
			nxtDisplayBigTextLine(2," %d ", mBuffer[i]);
			wait(2);
		}
	}

	return;
}
示例#5
0
int bleutooth_control(string *s){
	/*
		This code is for taking over the robot completely using bleutooth to make sure you can stop de robot in case its needed.
		pressing the middle button ("FIRE") on the phone stops the robot due to it not being included in the code here.
		To get in to this function you have to press "B"
	*/
	TFileIOResult nBTCmdRdErrorStatus;
 	int nSizeOfMessage;
 	ubyte nRcvBuffer[kMaxSizeOfMessage];
 	int stopcode = 0;

	while (*s != "A"){//if A is pressed the robot will resume its duty the normal way
		nSizeOfMessage = cCmdMessageGetSize(INBOX);
    if (nSizeOfMessage > kMaxSizeOfMessage){
    	nSizeOfMessage = kMaxSizeOfMessage;
 		}
 		if (nSizeOfMessage > 0){
			nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
		  nRcvBuffer[nSizeOfMessage] = '\0';

		 	*s = "";
		  stringFromChars(*s, (char *) nRcvBuffer);//put bluetooth input in string s
		  displayCenteredBigTextLine(4, *s);
		}
		//The next 4 if statements are for controlling the robot manually
		if(*s == "LEFT"){//if bleutooth input is left turn left
		  motor(motorA) = 0;
		  motor(motorB) = 30;
		  startTask(music);//make sure the music is running
		}
    else if(*s == "RIGHT"){//if bleutooth input is right turn right
    	motor(motorA) = 30;
    	motor(motorB) = 0;
      startTask(music);
    }
    else if (*s == "UP"){//if bleutooth input is up drive forward
    	setMultipleMotors(50, motorB, motorA);
    	startTask(music);
    }
    else if (*s == "DOWN"){//if bleutooth input is down drive backwards
    	setMultipleMotors(-50, motorB, motorA);
    	startTask(music);
    }
    else {//if there is no correct input turn off the motors
    	setMultipleMotors(0, motorA, motorB);
    	stopTask(music);//make sure the music is stopped
    	clearSounds();//empty the buffer
    }
    if (*s == "C"){//if the C button is pressed the loop stops and the stopcode == 1 to stop de code from running
    	stopcode = 1;
    	stopTask(music);
    	clearSounds();
    	break;
    }
  }
  *s = "";//make sure s is empty
  return stopcode;//output of the function used to stop the code if chosen
}
示例#6
0
void checkForRecieve(){
	if (cCmdMessageGetSize(mailbox19) > 0)
	{
		ubyte data[1];
		cCmdMessageRead(data, 1, mailbox19);

		checkForMove(data[0]);
	}
}
示例#7
0
task main(){
  nMotorEncoder[UP] = 0;
  nMotorEncoder[RND] = 0;
  MUp = 0;
  MRnd = 0;
  distUp = 0;
  distRnd = 0;

  checkBTLinkConnected();
  while(true){
    wait1Msec(1);
    //while (bQueuedMsgAvailable()){
      //ClearMessage();
      cCmdMessageRead(Rec, 2, mailbox1);
      //Rec = messageParm[0];
    //}
    /*if(message == 0){
      wait1Msec(5);
      nxtDisplayTextLine(2,"no message");
      continue;
    }*/

    nxtDisplayTextLine(1,"UP=%d,RND=%d",Rec[0], Rec[1]);

    GUp = Rec[0];
    GRnd = Rec[1];

    MUp = GUp * 43;
    MRnd = GRnd * 35;

    nxtDisplayTextLine(2,"MUP=%d,MRND=%d",MUp, MRnd);

    nxtDisplayTextLine(3,"UP = %d",nMotorEncoder[UP]);
    nxtDisplayTextLine(4,"RND = %d",nMotorEncoder[RND]);

    distUp = MUp-nMotorEncoder[UP];
    if(MUp != nMotorEncoder[UP]){
      motor[UP] = distUp;
    }
    else{
      motor[UP] = 0;
    }

    distRnd = MRnd-nMotorEncoder[RND];
    if(MRnd != nMotorEncoder[UP]){
      motor[RND] = distRnd;
    }
    else{
      motor[RND] = 0;
    }

  }
}
void readMessages()
{


	while (true)
	{
	  cCmdMessageRead(nMessage, 4, mailbox1);

	  int Axis1 = nMessage[0]-100;
	  int Axis2 = nMessage[1]-100;
	  int Axis3 = nMessage[3]-100;
	  nxtDisplayTextLine(2,"1 = %d", Axis2);

	   if(Axis1 < 0){
	     Axis1 = Axis1+55;
	     Axis1 = Axis1*1.275;
	   }

	   else{
	     Axis1 =  Axis1 = Axis1*1.275;
	   }

	   if(Axis2 < 0){
	     Axis2 = Axis2+100;
	     Axis2 = Axis2*1.275;
	   }

	   else{
	     Axis2 =  Axis2 = Axis2*1.275;
	   }

	   if(Axis3 < 0){
	     Axis3 = Axis3+100;
	     Axis3 = Axis3*1.275;
	   }

	   else{
	     Axis3 =  Axis3 = Axis3*1.275;
	   }


   // nxtDisplayTextLine(2,"1 = %d", Axis1);

    servo[servo1] = Axis1;
    servo[servo2] = Axis2;
    //servo[srvo_S1_C2_3] = LftMUp;
    //servo[srvo_S1_C2_4] = LftMRnd;

    }


		}
示例#9
0
// Updates the string s to give it the value of the last input of the app.
void CheckString(){
	TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];
  nSizeOfMessage = cCmdMessageGetSize(INBOX);

	if (nSizeOfMessage > kMaxSizeOfMessage){
   	nSizeOfMessage = kMaxSizeOfMessage;
	}
	if (nSizeOfMessage > 0){
   	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    nRcvBuffer[nSizeOfMessage] = '\0';
    stringFromChars(s, (char *) nRcvBuffer);
    displayCenteredBigTextLine(4, s);
	}
}
示例#10
0
/* BlueTooth functie
	 Pollt de brick elke 100ms voor nieuwe
	 instructies en retourneert deze vervolgens.

	 How-to:
	 bluetooth();

	 mogelijke return waarden (int)
	 UP = 1
	 DOWN = 2
	 LEFT = 3
	 RIGHT = 4
	 A = 5
	 B = 6
	 C = 7
*/
int bluetooth()
{
  TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

	while (true)
  {
    // Check to see if a message is available
    nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    if (nSizeOfMessage > 0){
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	if(s == "UP"){
    			return 1;
    	}
     if(s == "DOWN"){
    			return 2;
    	}
    	if(s == "LEFT"){
    			return 3;
    	}
    	if(s == "RIGHT"){
    			return 4;
    	}
    	if(s == "A"){
    			return 5;
    	}
    	if(s == "B"){
    			return 6;
    	}
    	if(s == "C"){
    			return 7;
    	}
    }
    wait1Msec(100);
  }
  //Als er niets gebeurt 0 retourneren.
  return 0;
}
示例#11
0
task main()
{
	/*
		The main function contains just an infinite while loop that continues to call all the functions before mentioned. In the called
		functions we check for specific things like the line curving or a bleutooth input.
	*/
	int stopcode2 = 0;//for stopping the while loop in case of a remote code shutdown

	startTask(music);//start the music in a seperate thread

	string s = "";//create a string
	TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

	while(1){
		 nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage){//make the message shorter if its to long
      nSizeOfMessage = kMaxSizeOfMessage;
    }

    if (nSizeOfMessage > 0){//if the message is not empty, put the received information in string s
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	s = "";
    	stringFromChars(s, (char *) nRcvBuffer);//put the received data in string s
    	displayCenteredBigTextLine(4, s);
    }
    if (s == "B"){//bluetooth override
    	setMultipleMotors(0, motorA, motorB);
			stopcode2 = bleutooth_control(&s);//check for bleutooth input. This is the version of bleutooth input that takes over the robot
		}
		junction(&s);//check for an intersection
		linefollow();//check if the line is curving and if not then just drive
		if(stopcode2 == 1){//if "C" is pressed in the bleutooth_control function then stop the code completely
			return;
		}
	}
}
示例#12
0
task main()
{
	int mailbox = 5, a = 0;
	string btmessage="";
	nMotorEncoder[MotorLinks] = 0;
	nMotorEncoder[MotorRechts] = 0;

	while(true)
	{
		// lees mailbox
		a = cCmdMessageGetSize(mailbox);  //haal de hoeveel bytes in eerst volgende bericht
		if(a > 0)
		{
			cCmdMessageRead(btmessage, a, mailbox); // lees aantal bytes a in en plaats ze in btmessage


			if(btmessage == "AUTO")
			{
				auto = true;
				startTask(rij_auto);
			}
			else if(btmessage == "MANUAL")
示例#13
0
bool readSamanthaStatus()
{
  int nSizeOfMessage;
  int i;

  TFileIOResult nBTCmdRdErrorStatus;
  const int kMaxSizeOfMessage = sizeof(TSamanathaStatusMessage);
  ubyte tempBuffer[kMaxSizeOfMessage];

   // Check to see if a message is available.
   nSizeOfMessage = cCmdMessageGetSize(kQueueID);

   if (nSizeOfMessage <= 0)
   {
     return false;
   }

    if (nSizeOfMessage > sizeof(tempBuffer))
      nSizeOfMessage = sizeof(tempBuffer);

    nBTCmdRdErrorStatus = cCmdMessageRead(tempBuffer, nSizeOfMessage, kQueueID);

    if (nBTCmdRdErrorStatus != ioRsltSuccess)
    {
       return true;
    }

    status.nLine = (int)tempBuffer[0];
    status.nSizeOfMessage = (int)nSizeOfMessage;

    //    StringFromChars(samanthaStatusCopy.message,tempBuffer);
    status.message ="";
    for(i=0; i<20; i++)
      status.message += tempBuffer[i+1];

    return true;
}
示例#14
0
task main()
{

	int timer_release = 1;
	int app_timeout = 3000;
	int stop_functie = 0;
	string app_message = "";
	wait1Msec(50);                        // The program waits 50 milliseconds to initialize the light sensor.
	playSound(soundException);
	while(true)                           // Infinite loop
	{
		/** check if there is a message available from app */
    nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage){
      nSizeOfMessage = kMaxSizeOfMessage;
    }
  	/** store the message of the app into a variable*/
    if (nSizeOfMessage > 0) {
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	stringFromChars(app_message, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, app_message);
    	if (app_message == "FIRE") {
    		if (stop_functie == 0) {
    			stop_functie = 1;
    			app_message = "STOP";
    			app_move(app_message, false);
    			continue;
    		}
    		else if (stop_functie == 1){
    			//motor[motorA] = 0;
    			//motor[motorB] = 0;
    			stop_functie = 0;
    			timer_release = 1;
    		}
    	}
	  }

		/** check the sonar sensor and stop the robot if it sees sth */
		if (stop_functie == 0 && SensorValue[sonar1] < 20) {
			for(int i = 0; i < 20; i++){
							motor[motorA] = -20 + (1 * i);
							motor[motorB] = -20 + (1 * i);
							wait1Msec(1000);
				}
			}
		/** check the color sensor if it's black stop the robot */
		else if (stop_functie == 0 && SensorValue[colorSensor] ==  BLACKCOLOR) {
			motor[motorA] = 0;
			motor[motorB] = 0;



			/* Reset Timer1 value (only the first time (when the timer is free)
				and check if its reached app_timeout */
			if (timer_release == 1){
					ClearTimer(T1);
					timer_release = 0;
				}

			if (timer_release == 0 && time1[T1] < app_timeout) {
	    		if (nSizeOfMessage > 0){
	    			app_message = app_message;
	    			app_move(app_message, true)
	    			stop_functie = 1;
	    			/* Release the timer */
	    			timer_release = 1;
			    }
			}
			else if (timer_release == 0 && time1[T1] > app_timeout){
				nMotorEncoder[motorA] = 0;
				while (nMotorEncoder[motorA] >  -180) {
					motor[motorA] = -50;
					motor[motorB] = -50;
				}
				/* Release the timer */
				timer_release = 1;
			}
		}
	else if (stop_functie == 0) {
		displayCenteredBigTextLine(4, "%d", SensorValue[lightSensor]);
			motor[motorA] = -20 + ((60 - SensorValue[lightSensor]));
			motor[motorB] = -20 - ((60 - SensorValue[lightSensor]));
		}
	else if (stop_functie == 1) {
		app_move(app_message, false);
		app_message = "";
		}
}
}
示例#15
0
task main()
{

	int timer_release = 1;
	int app_timeout = 3000;
	int stop_functie = 1;
	bool intersection = false;
	nVolume = 3;


	string app_message = "";
	wait1Msec(50);                        // The program waits 50 milliseconds to initialize the light sensor.
	while(true)                           // Infinite loop
	{
		/** check if there is a message available from app */
    nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage){
      nSizeOfMessage = kMaxSizeOfMessage;
    }
  	/** store the message of the app into a variable*/
    if (nSizeOfMessage > 0) {
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	stringFromChars(app_message, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, app_message);
    	if (app_message == "FIRE" && timer_release == 1) {
    		if (stop_functie == 0) {
    			stop_functie = 1;
    			app_message = "STOP";
    			app_move(app_message, false);
    			continue;
    		}
    		else if (stop_functie == 1){
    			//motor[motorA] = 0;
    			//motor[motorB] = 0;
    			stop_functie = 0;
    			timer_release = 1;
    		}
    	}
	  }

		/** check the sonar sensor and stop the robot if it sees sth */
		if (stop_functie == 0 && intersection == false && SensorValue[sonar1] < 30) {
				int i = -15;
				clearSounds();
				while (i < 0) {
					playSound(soundBeepBeep);
					motor[motorA] = i;
					motor[motorB] = i;
					i +=  4;
					wait1Msec(300);
					}
			motor[motorA] = 0;
			motor[motorB] = 0;
			stop_functie = true;
		}
		/** check the color sensor if it's black stop the robot */
		else if (stop_functie == 0 && SensorValue[colorSensor] ==  BLACKCOLOR && SensorValue[lightSensor] < 50) {
				int i = -15;
				clearSounds();
				while (i < 0) {
					playSound(soundException);
					motor[motorA] = i;
					motor[motorB] = i;
					i +=  4;
					wait1Msec(300);
			}
			motor[motorA] = 0;
			motor[motorB] = 0;
			intersection = true;

		}

		/** if the robot is at intersection, wait for a reactie from app
				The time-out is max "app_timeout" */
		else if (stop_functie == 0 && intersection == true) {

			/* Reset Timer1 value (only the first time (when the timer is free)
				and check if its reached app_timeout) */
			if (timer_release == 1){
					ClearTimer(T1);
					timer_release = 0;
				}

			if (timer_release == 0 && time1[T1] < app_timeout) {
	    		if (nSizeOfMessage > 0){
	    			/* Release the timer */
	    			timer_release = 1;
	    			intersection = false;
	    			stop_functie = 1;
	    			if (app_message == "FIRE") {
	    				stop_functie = 0;
	    			}
	    			else if (app_message != "UP") {
	    				app_move(app_message, true);
	    			}
			    }
			}
			else if (timer_release == 0 && time1[T1] > app_timeout){
				intersection = false;
				/* Release the timer */
				timer_release = 1;
			}
		}
	else if (stop_functie == 0 && intersection == false) {
		displayCenteredBigTextLine(4, "%d", SensorValue[lightSensor]);
			motor[motorA] = -22 + (power((float)(60 - SensorValue[lightSensor]) / 12, 3) * 12);
			motor[motorB] = -22 - (power((float)(60 - SensorValue[lightSensor]) / 12, 3) * 12);
			playSound(soundBlip);
		}
	else if (stop_functie == 1) {
		app_move(app_message, false);
		app_message = "";
		clearSounds();
		}
}
}
示例#16
0
task readMsgFromPC()
{
    bool bMsgFound;

    TFileIOResult nBTCmdRdErrorStatus;
    const int kMaxSizeOfMessage = 18;
    sbyte tempBuffer[kMaxSizeOfMessage];

    // Initialize setting to default values in case communications with PC is broken.

    //joystickCopy.TeamColor = false;
    joystickCopy.UserMode  = false;
    joystickCopy.StopPgm   = true;

    joystickCopy.joy1_x1 = 0;
    joystickCopy.joy1_y1 = 0;
    joystickCopy.joy1_x2 = 0;
    joystickCopy.joy1_y2 = 0;
    joystickCopy.joy1_Buttons = 0;
    joystickCopy.joy1_TopHat = -1;

    joystickCopy.joy2_x1 = 0;
    joystickCopy.joy2_y1 = 0;
    joystickCopy.joy2_x2 = 0;
    joystickCopy.joy2_y2 = 0;
    joystickCopy.joy2_Buttons = 0;
    joystickCopy.joy2_TopHat = -1;

    while (true)
    {
        // Check to see if a message is available.
        bMsgFound = false;
        while (true)
        {
            //
            // There may be more than one message in the queue. We want to get to the last received
            // message and discard the earlier "stale" messages. This loop simply discards all but
            // the last message.
            //
            int nSizeOfMessage;

            nSizeOfMessage = cCmdMessageGetSize(kJoystickQueueID);

            if (nSizeOfMessage <= 0)
            {
                if (!bMsgFound)
                {
                    wait1Msec(4);    // Give other tasks a chance to run
                    continue;        // No message this time. Loop again
                }
                //
                // No more messages available and at least one message found. This is not essential but
                // useful to ensure that we're working with the latest message. We simply discard earlier
                // messages. This is useful because there could be many messages queued and we don't
                // want to work with stale data.
                //
                break;
            }
            if (nSizeOfMessage > sizeof(tempBuffer))
                nSizeOfMessage = sizeof(tempBuffer);
            nBTCmdRdErrorStatus = cCmdMessageRead((ubyte)tempBuffer, nSizeOfMessage, kJoystickQueueID);
            nBTCmdRdErrorStatus = nBTCmdRdErrorStatus; //Get rid of info message
            //
            // Repeat loop until there are no more messages in the queue. We only want to process the
            // last message in the queue.
            //
            bMsgFound = true;
        }

        // Once we've reached here, a valid message is available

        hogCPU();   // grab CPU for duration of critical section

        ++ntotalMessageCount;

        joystickCopy.UserMode           = tempBuffer[1];
        joystickCopy.StopPgm            = tempBuffer[2];

        joystickCopy.joy1_x1            = tempBuffer[3];
        joystickCopy.joy1_y1            = tempBuffer[4];
        joystickCopy.joy1_x2            = tempBuffer[5];
        joystickCopy.joy1_y2            = tempBuffer[6];
        joystickCopy.joy1_Buttons       = (tempBuffer[7] & 0x00FF) | (tempBuffer[8] << 8);
        joystickCopy.joy1_TopHat        = tempBuffer[9];

        joystickCopy.joy2_x1            = tempBuffer[10];
        joystickCopy.joy2_y1            = tempBuffer[11];
        joystickCopy.joy2_x2            = tempBuffer[12];
        joystickCopy.joy2_y2            = tempBuffer[13];
        joystickCopy.joy2_Buttons       = (tempBuffer[14] & 0x00FF) | (tempBuffer[15] << 8);
        joystickCopy.joy2_TopHat        = tempBuffer[16];

        joystickCopy.joy1_y1            = -joystickCopy.joy1_y1; // Negate to "natural" position
        joystickCopy.joy1_y2            = -joystickCopy.joy1_y2; // Negate to "natural" position

        joystickCopy.joy2_y1            = -joystickCopy.joy2_y1; // Negate to "natural" position
        joystickCopy.joy2_y2            = -joystickCopy.joy2_y2; // Negate to "natural" position


        releaseCPU(); // end of critical section
    }
}
task main()
{
	string sig = "";
  TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

  while (true)
  {
    // Check to see if a message is available

    nSizeOfMessage = cCmdMessageGetSize(INBOX);
    writeDebugStream("%c\n", nRcvBuffer);

    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    if (nSizeOfMessage > 0){
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, s);
    	sig = nRcvBuffer;
    	writeDebugStream(sig);
			  if(sig == "U"){
   				setMotor(motorA, 50);
   				setMotor(motorB, 50);
   				wait1Msec(10);

  }
 				if(sig=="D"){
  				setMotor(motorA, -50);
  				setMotor(motorB, -50);
					wait1Msec(1);
  }
  			if(sig=="L"){
  				setMotorTarget(motorA,0, 0);
  				setMotorTarget(motorB,360, 50);
  				waitUntilMotorStop(motorB);
  				startTask(Rijden);
					k=1;

    	}
    		if(sig=="R"){
    		  setMotorTarget(motorA,360, 50);
  			  setMotor(motorB, 0);
  			  waitUntilMotorStop(motorA);
  			  startTask(Rijden);

  			  }
  			if(sig=="F"){
  				setMotorTarget(motorA, 180,30);
  				setMotorTarget(motorB,180, 30);
  				waitUntilMotorStop(motorA);
				waitUntilMotorStop(motorB);
				k=1;
				startTask(Rijden);

  			}
  			if(sig=="A"){
  				setMultipleMotors(motorA, motorB, 0);
  				stopTask(Rijden);
  			}
  			if(sig=="C"){
  				startTask(Rijden);
  			}
    wait1Msec(100);
  }
  }


}
void bluetooth()
{

	string kutzooi = "";
  TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

  while (true)
  {
    // Check to see if a message is available

    nSizeOfMessage = cCmdMessageGetSize(INBOX);
    writeDebugStream("%c\n", nRcvBuffer);

    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    if (nSizeOfMessage > 0){
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, s);
    	kutzooi = nRcvBuffer;
    	writeDebugStream(kutzooi);
			  if(kutzooi == "U"){
   				setMotor(motorA, 30);
   				setMotor(motorB, 30);

   			wait1Msec(10);
   			break;
  }
 				if(kutzooi=="D"){
 					setMotorTarget(motorA,1080, 100);
  				setMotorTarget(motorB,1080, 100);
  				waitUntilMotorStop(motorA);
  				waitUntilMotorStop(motorB);
					wait1Msec(1);
				break;


  }
  			if(kutzooi=="L"){
  				setMotorTarget(motorA,0, 0);
  				setMotorTarget(motorB,360, 25);
  				waitUntilMotorStop(motorB);

  				break;

    	}
    		if(kutzooi=="R"){
    		  setMotorTarget(motorA,360, 25);
  			  setMotor(motorB, 0);
  			  waitUntilMotorStop(motorA);
  			  break;

  			}
  			if(kutzooi=="F"){
  				setMotorTarget(motorA, 1080,100);
  				setMotor(motorB, 0);
  				waitUntilMotorStop(motorA);
					setMotor(motorA, 0);
  				setMotorTarget(motorB, 1080,100);
  			break;
  			}
  			if(kutzooi=="A"){
  				setMultipleMotors(motorA, motorB, 0);

  			}
    wait1Msec(100);
  }
  }



}
task main()
{
	// Gekregen code voor BlueTooth
	string sig = "";
	TFileIOResult nBTCmdRdErrorStatus;
	int nSizeOfMessage;
	ubyte nRcvBuffer[kMaxSizeOfMessage];
	// Einde gekregen code voor BlueTooth

  while(true) // Main loop
  {
  	/*
		 * Gekregen code voor BlueTooth
		 * leest berichten die BlueTooth stuurt uit en stopt die in een string.
		 */
    nSizeOfMessage = cCmdMessageGetSize(INBOX);
    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    if (nSizeOfMessage > 0){
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, s);
    /*
		 * Einde gekregen code voor BlueTooth
		 */

    	sig = nRcvBuffer;// zet bluetoothsignaal in variabele
    	//writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven

    	// naar voren rijden zonder lijndetectie
    	if(sig == "U"){
   			setMotor(motorA, 30);
   			setMotor(motorB, 30);
   			wait1Msec(10);
   		}
   		// naar achter rijden zonder lijndetectie
			if(sig=="D"){
				setMotor(motorA, -30);
				setMotor(motorB, -30);
				wait1Msec(10);
			}
			// maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten)
			if(sig=="L"){
				setMotorTarget(motorA, 0, 0);
				setMotorTarget(motorB, 360, 30);
				waitUntilMotorStop(motorB);
				startTask(Rijden);
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
			}
    	// maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten)
  		if(sig=="R"){
  		  setMotorTarget(motorA, 360, 30);
			  setMotor(motorB, 0);
			  waitUntilMotorStop(motorA);
			  startTask(Rijden);
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
			}
			// rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten)
			if(sig=="F"){
				setMotorTarget(motorA, 180, 30);
				setMotorTarget(motorB, 180, 30);
				waitUntilMotorStop(motorA);
				waitUntilMotorStop(motorB);
				k=1; // zet anti-lijnhump op 1
				startTask(Rijden);
			}
			// Stoppen
			if(sig=="A"){
				setMultipleMotors(motorA, motorB, 0);
				stopTask(Rijden);
				clearSounds();

			}
			// Zet lijndetectie aan
			if(sig=="C"){
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
				startTask(Rijden);
			}

			/*
			 * nog 1 knop ongebruikt (B).
			 */

			// Na elk commando, wacht 0,1s.
    	wait1Msec(100);
    }
	}
}
示例#20
0
void readMultipleDataMsgs()
{
  TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

  while (true)
  {
    // Check to see if a message is available

    nSizeOfMessage = cCmdMessageGetSize(kQueueID);
    if (nSizeOfMessage <= 0)
    {
      wait1Msec(1);    // Give other tasks a chance to run
      break;           // No more message this time
    }

    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, kQueueID);





     cmps = 180+(nRcvBuffer[0]-nRcvBuffer[1]);
    int oxmps;
   if(nRcvBuffer[1] == 0){oxmps = nRcvBuffer[0];}
   if(nRcvBuffer[0] == 0){oxmps = -nRcvBuffer[1];}

     nxtDisplayTextLine(3, "button:   %4d", nRcvBuffer[2]);
     nxtDisplayTextLine(4, "Abs:   %4d", oxmps);
     nxtDisplayTextLine(5, "curr:   %4d", HTMCreadRelativeHeading(HTMC));
     if(nRcvBuffer[2]==1){
       int mspeed = 0;
       int mangle = 0;
       //stopMotors();
         blob_array _blobs;
         // combine all colliding blobs into one
         bool _condensed = true;

          //blob_array _blobs;
         int _l, _t, _r, _b;
         int _nblobs;
         eraseDisplay();

            // Initialise the camera
           NXTCAMinit(cam);
           servo[servo1] = 35;
             while(true) {
              eraseDisplay();

              _nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
              // Fetch all the blobs, have the driver combine all
              // the colliding blobs.

              if(_nblobs>0){

                int cx = 176-(SIDE_CENTER(_blobs[0].x1, _blobs[0].x2));
                int cy = SIDE_CENTER(_blobs[0].y1, _blobs[0].y2);
                float dx = cx-88;
            /*    if(cy < C_STOP){
                mspeed = (cy/C_STOP)*25;
              }
                  else{
                    mspeed = 0;
                  }*/

                  mspeed=50;
                //  if((cx-44)>0){
                  float rathura = (dx*90)/88;
                mangle = rathura;
            //  }
            //  else{
                 //mangle = (dx/44)*-90;
              //}


                      // Draw the scaled blobs
                          _l = xscale(_blobs[0].x1);
                          _t = yscale(_blobs[0].y1);
                         _r = xscale(_blobs[0].x2);
                          _b = yscale(_blobs[0].y2);
                        nxtFillRect(_l, _t, _r, _b);


                  nxtDisplayTextLine(1, "%d", _nblobs);
                  nxtDisplayTextLine(2, "%d", mangle);
                  nxtDisplayTextLine(3, "%d", (int)dx);
                  runMotorSpeeds(mSD, mSE, mSF, mSG, mangle, mspeed);
                  _nblobs = 0;
                   wait1Msec(100);

          }
        }


       flagged = true;
     }
    // else{
    // stopMotors();
     else{
      if (abs(oxmps-HTMCreadRelativeHeading(HTMC)) > 5)
     {
     flagged = false;
    }
  }





     if(flagged == true){

      stopMotors();
    }

    else{
      turntocmps(cmps, oxmps);
    }

  // }






     //}
  }
  return;
}
示例#21
0
task main()
{

	nVolume = 3;
	//motor[motorC] = -20;

	/** PDI Algorithm variables */
	int Kp = 138;
	int Ki = 0;
	int Kd = 2800;
	int offset = 56;
	int Tp = 45;
	int integral = 0;
	int lastError = 0;
	int derivative = 0;
	int LightValue = 0;
	int error = 0;
	int turn = 0;
	int powerA = 0;
	int powerB = 0;

	int app_timeout = 3000;
	bool stop_functie = true;					// if it's false the robot drives by itself and if it's true it drives by the app
	bool intersection = false;				// if true we are at intersection

	string app_message = "";					// the message from app
	wait1Msec(50);                        // The program waits 50 milliseconds to initialize the light sensor.
	while(true)                           // Infinite loop
	{
		/*** check if there is a message available from app ***/
    nSizeOfMessage = cCmdMessageGetSize(INBOX);

    if (nSizeOfMessage > kMaxSizeOfMessage){
      nSizeOfMessage = kMaxSizeOfMessage;
    }
  	/** store the message of the app into a variable*/
    if (nSizeOfMessage > 0) {
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	stringFromChars(app_message, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, app_message);
    	/* Stop or Start the robot by tapping on "Fire" */
    	if (app_message == "FIRE" && intersection == false) {
    		if (stop_functie == false) {
    			stop_functie = true;
    			app_message = "STOP";
    			app_move(app_message, false);
    			continue;
    		}
    		else if (stop_functie == true){
    			stop_functie = false;
    		}
    	}
	  }

		/*** if the robot drives by itself ***/
	  if (stop_functie == false) {

				/** check the sonar sensor and stop the robot if it see's sth **/
				if (intersection == false && SensorValue[sonar1] < 30) {
						int i = -15;
						clearSounds();
							while (i < 0) {
								playSound(soundBeepBeep);
								motor[motorA] = i;
								motor[motorB] = i;
								i +=  4;
								wait1Msec(300);
						}
					motor[motorA] = 0;
					motor[motorB] = 0;
					stop_functie = true;
				}

				/** Detect intersections for the first time **/
				else if (SensorValue[colorSensor] ==  BLACKCOLOR && SensorValue[lightSensor] < 50) {
						playSound(soundException);
						int i = -20;
						clearSounds();
						while (i < 0) {
							playSound(soundException);
							motor[motorA] = i;
							motor[motorB] = i;
							i +=  4;
							wait1Msec(300);
					}
					motor[motorA] = 0;
					motor[motorB] = 0;
					intersection = true;
					/* Reset Timer1 value (only the first time the robot sees a intersection **/
					ClearTimer(T1);
				}

				/** if the robot is at intersection, wait for a message from app
						The time-out is max "app_timeout" **/
				else if (intersection == true) {

					if (time1[T1] < app_timeout) {
			    		if (nSizeOfMessage > 0){
			    			intersection = false;
			    			stop_functie = true;
			    			if (app_message == "FIRE") {
			    				stop_functie = false;
			    			}
			    			else if (app_message != "UP") {
			    				app_move(app_message, true);
			    			}
					    }
					}
					else if (time1[T1] > app_timeout){
						intersection = false;
					}
				}

				/** if there is no other condition: (NO intersection, No sonar) **/
				else if (intersection == false) {

					//motor[motorA] = -23 + power((float)(58 - SensorValue[lightSensor]) / 5.15, 3);
					//motor[motorB] = -23 - power((float)(58 - SensorValue[lightSensor]) / 5.15, 3);
					//playSound(soundBlip);

					LightValue = SensorValue(lightSensor);
					error = LightValue - offset;
					//integral = integral + error;
					derivative = error - lastError;
					Turn = (Kp*error) + (Ki*integral) + (Kd*derivative);
					Turn = Turn/100;
					powerA = Tp + Turn;
					powerB = Tp - Turn;
					motor[motorA] = -1*powerA;
					motor[motorB] = -1*powerB;
					lastError = error;
					displayCenteredBigTextLine(4, "%d", error);
				}
	}

	/*** if the robot drives by the app ***/
	else if (stop_functie == true) {
		clearSounds();
		app_move(app_message, false);
		app_message = "";
		}
}
}
示例#22
0
task main()
{
//InitRS485();

nxtEnableHSPort();                                //Enable High Speed Port #4
nxtSetHSBaudRate(9600);                           //Xbee Default Speed
nxtHS_Mode = hsRawMode;                           //Set to Raw Mode (vs. Master/Slave Mode)

//char messagereceive,messagesend='f';
string my_message;
string xRec,yRec,xSend,ySend;
int turnA,count,newcount,stopsignal1=0;
float travelled,theta=0,x=60,y=60,newx,newy,angle;
int num,rotation,distance;
int times=0;
//ubyte send,incomingData;

// turn - 1 -> left, -1->right
while(SensorValue[lightSensor] < 35 && stopsignal1==0)
{
	times=times+1;
	wait1Msec(1000);
cCmdMessageRead(my_message,1,1);
if(my_message == "f")
	{
		stopsignal1=1;
		break;
	}


	num=abs(rand()%100+1);
	rotation=abs(rand()%180+1);
	distance=abs(rand()%360);
	if (num<=50)	turnA=1;
	else		turnA=-1;

  count=turn(rotation,turnA);
  travelled=move(distance);//* 2*PI*2.8;
	travelled=travelled*2*PI*2.8/360;
  if (times==1)
		theta=90+count/2;
	else
		theta=theta+count/2;

	if(theta>180)
		theta=theta-360;
	else if (theta<-180)
		theta=360+theta;
	else
		theta=theta;

	x+=travelled*cosDegrees(theta);
	y+=travelled*sinDegrees(theta);
	nxtDisplayTextLine(1, "Angle %f",theta);  // Display the text on line number 4 of 8 on the LCD
	nxtDisplayTextLine(2, "X-loca %f",x);  // Display the text on line number 4 of 8 on the LCD
  nxtDisplayTextLine(3, "Y- loca %f",y);  // Display the text on line number 4 of 8 on the LCD
}

if(stopsignal1==1)
{
	eraseDisplay();
	nxtDisplayTextLine(6,"%s",my_message);
 //wait1Msec(1000);
	while(1)
	{
	cCmdMessageRead(xRec,10,2);

//	wait1Msec(1000);
  cCmdMessageRead(yRec,10,1);

newx=atof(xRec);
newy=atof(yRec);

if (newx!=0 && newy!=0)
	break;
}
 //   newx = messageParm[0];
  //  newy = messageParm[1];

    nxtDisplayTextLine(1,"Received");
		nxtDisplayTextLine(2, "New x: %f", newx);
		nxtDisplayTextLine(3, "New y: %f", newy);
  	distance=sqrt((x-newx)*(x-newx) + (y-newy)*(y-newy));
	  distance=abs(distance*360/(2*PI*2.8));
  	angle=atan((y-newy)/(x-newx))/(2*PI) * 360;

  	nxtDisplayTextLine(4, "Angle: %f", angle);
if(x<newx)
{
		newcount=abs((0-theta)*2);
if (theta<0)
	turnA=1;
else
	turnA=-1;
count=turn(newcount,turnA);
if(y>newy)
{
	turnA=-1;
	count=turn(abs(angle)*2,turnA);
}
else
{
	turnA=+1;
	count=turn(abs(angle)*2,turnA);
}

}
 else
{
  if(theta<0)
{		newcount=abs((-180-theta)*2);
  		turnA=-1;
}
 else
{
	newcount=abs((180-theta)*2);
	turnA=1;
}
count=turn(newcount,turnA);
if(y>newy)
{
	turnA=+1;
	count=turn(abs(angle)*2,turnA);
}
else
{
	turnA=-1;
	count=turn(abs(angle)*2,turnA);
}

}

nxtDisplayTextLine(5, "distance: %d", distance);
	travelled=move(distance);

	wait1Msec(5000);


}
else
{
	 eraseDisplay();
//wait1Msec(3000);

	my_message='f';
	cCmdMessageWriteToBluetooth(my_message,strlen(my_message),1);
  // sendMessageWithParm(x,y);
  //sendMessage(y);
 	 sprintf(xSend,"%f",x);
	 sprintf(ySend,"%f",y);
wait1Msec(3000);

	cCmdMessageWriteToBluetooth(xSend,strlen(xSend),2);
	nxtDisplayTextLine(2, "New x: %s", xSend);

  wait1Msec(500);
	cCmdMessageWriteToBluetooth(ySend,strlen(ySend),1);
	nxtDisplayTextLine(3, "New y: %s", ySend);

}
wait1Msec(3000);
nxtDisableHSPort();                               //Disable HS Port #4
}
示例#23
0
int bleutooth_control(void){
	/*
		This code is for taking over the robot completely using bleutooth to make sure you can stop de robot in case its needed.
		pressing the middle button ("FIRE") on the phone stops the robot due to it not being included in the code here for another function.
	*/
	TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];
  int stopcode = 0;

  nSizeOfMessage = cCmdMessageGetSize(INBOX);

	if (nSizeOfMessage > kMaxSizeOfMessage){
      nSizeOfMessage = kMaxSizeOfMessage;
 	}

	if (nSizeOfMessage > 0){//Only if an actual message is being received the bleutooth take over system should activate
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';

    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, s);

    	while (s != "A"){//if A is pressed the robot will resume its duty the normal way
    		nSizeOfMessage = cCmdMessageGetSize(INBOX);//misschien onderaan
    		if (nSizeOfMessage > kMaxSizeOfMessage){
      		nSizeOfMessage = kMaxSizeOfMessage;
 				}
 				if (nSizeOfMessage > 0){
		    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
		    	nRcvBuffer[nSizeOfMessage] = '\0';

		    	s = "";
		    	stringFromChars(s, (char *) nRcvBuffer);
		    	displayCenteredBigTextLine(4, s);
		  	}
				//The next 4 if statements are for controlling the robot manually
    		if(s == "LEFT"){//if bleutooth input is left turn left
    			motor(motorA) = 0;
    			motor(motorB) = 30;
    		}
    		else if(s == "RIGHT"){//if bleutooth input is right turn right
    			motor(motorA) = 30;
    			motor(motorB) = 0;
    		}
    		else if (s == "UP"){//if bleutooth input is up drive forward
    			setMultipleMotors(50, motorB, motorA);
    		}
    		else if (s == "DOWN"){//if bleutooth input is down drive backwards
    			setMultipleMotors(-50, motorB, motorA);
    		}
    		/*else {//hier
    			setMultipleMotors(0, motorA, motorB);
    		}*/
    		if (s == "C"){//if the C button is pressed the loop stops and the stopcode == 1 to stop de code if neccesairy
    			stopcode = 1;
    			break;
    		}
    	}
   }
   return stopcode;//either 1 or 0 depending on the "C" button being pressed or not
}