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;
}
void purgeBT(TMailboxIDs box) {
while(cCmdMessageGetSize(box)>0) {
cCmdMessageRead(dataReceived, cCmdMessageGetSize(box), box);
wait1Msec(100);
}
// while(nxtReadRawBluetooth(dataReceived, dataSize)!=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 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;
}
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
}
void checkForRecieve(){
if (cCmdMessageGetSize(mailbox19) > 0)
{
ubyte data[1];
cCmdMessageRead(data, 1, mailbox19);
checkForMove(data[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);
}
}
/* 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;
}
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;
}
}
}
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")
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;
}
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();
}
}
}
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
}
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);
}
}
}
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 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()
{
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 = "";
}
}
}
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;
}
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 = "";
}
}
}
