int main(int argc, char **argv)
{
const char serialBuffer[6] = "hello";
int serialOpen, nBytesRead;
serialOpen=serial.Open(6,9600);
cout<<serialOpen;
if(serialOpen){
nBytesRead=serial.SendData(serialBuffer,strlen(serialBuffer));
//ASSERT(nBytesRead==strlen(serialBuffer));
//delete [] serialBuffer;
}
serial.Close();
cout << "\n";
cout << "=========================================================\n";
cout << "getTristarLog aborted. \n\n";
return 0;
}
int WINAPI _tWinMain (HINSTANCE /*hInst*/, HINSTANCE /*hInstPrev*/, LPTSTR /*lptszCmdLine*/, int /*nCmdShow*/)
{
CSerial serial;
LONG lLastError = ERROR_SUCCESS;
// Attempt to open the serial port (COM1)
lLastError = serial.Open(_T("COM1"),0,0,false);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port"));
// Setup the serial port (9600,N81) using hardware handshaking
lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting"));
// Setup handshaking
lLastError = serial.SetupHandshaking(CSerial::EHandshakeHardware);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port handshaking"));
// The serial port is now ready and we can send/receive data. If
// the following call blocks, then the other side doesn't support
// hardware handshaking.
lLastError = serial.Write("1\n");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
// Close the port again
serial.Close();
return 0;
}
//------------------------------------------------------------------------------
// close debug log file
//------------------------------------------------------------------------------
SQRESULT SQ_debugLog_close (HSQUIRRELVM v)
{
if (serial.IsOpen())
{
//fclose(gpDebugLogFile);
serial.Close();
}
return SQ_OK;
}
int SendData()
{
int i = 0;
int count = 0;
char ioControl[3];
int bytesRecieved = 0;
while (true)
{
bytesRecieved = SerialPort.ReadData(ioControl, 1);
printf("%c %d\t", ioControl[0], bytesRecieved);
if (ioControl[0] == 'r')
{
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
SerialPort.SendData(Controller[i].passedValue, sizeof(Controller[i].passedValue));
if (leapConnected == true)
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
SerialPort.SendData(Leapvalues[i].passedValue, sizeof(Leapvalues[i].passedValue));
}
else
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
SerialPort.SendData("0", 1);
}
//Sleep(59);
count++;
if (count > 5)
{
SerialPort.Close();
Sleep(1000);
SerialPort.Open(PORT_NUM, BAUD);
return 0;
}
}
else if (ioControl[0] == 'q')
{
return 1;
}
else
{
/*printf("Error: Unknown codeword passed from arduino [%c], aborting send\n", ioControl[0]);
Sleep(60);*/
}
}
}
int main(char argc, char *argv[])
{
int port = 3, baudRate = 9600, dispType = 0;
if (argc >= 2)
port = atoi(argv[2]);
if (argc >= 3)
baudRate = atoi(argv[3]);
if (argc >= 4)
dispType = atoi(argv[4]);
CSerial serial;
if (!serial.Open(port, baudRate))
return 0;
int curT=0, oldT=0;
while (1)
{
curT = GetTickCount();
if (curT - oldT > 10)
{
char buffer[256];
int nBytesRead = serial.ReadData(buffer, sizeof(buffer));
if (nBytesRead > 0)
{
for (int i = 0; i < nBytesRead; ++i)
{
switch (dispType)
{
case 0: printf("%c", buffer[i]); break;
case 1: printf("%d", buffer[i]); break;
case 2: printf("%x", buffer[i]); break;
}
}
printf("\n");
}
oldT = curT;
}
}
serial.Close();
return 1;
}
int main(int argc , char* argv[]){
for (int portNo = 0; portNo<5 ; ++portNo){
if(serial.findAvailablePorts(portNo))
cout<<"COM"<<portNo<<" available!"<<endl;
}
if(serial.Open(3,115200)){
cout<<"Connection initialized!"<<endl;
}
char szMessage[3] = {0x75, 0x92 , 0xE7};
serial.SendData(szMessage,3);
char* lpBuffer = new char[100];
char setOn[6] = {0x52, 0x86 , 0xD4 , 0x5A , 0x80 , 0xDA};
Sleep(300);
serial.ReadData(lpBuffer , 30);
if(controlComms.checkValidity(lpBuffer,20)!=0){
cout<<"Device is ready!"<<endl;
} else{
cout<<"Device is not ready!"<<endl;
}
Sleep(1000);
//Sleep(1000);
serial.SendData(setOn,sizeof(setOn));
Sleep(300);
thread t1(readData,"");
t1.join();
serial.Close();
printf("\n");
return 0;
}
BOOL readData(string str){
char* lpBuffer = new char[100];
while(true){
if(serial.ReadDataWaiting()){
try{
int bytesRead = serial.ReadData(lpBuffer,30);
controlComms.printChars(lpBuffer);
Sleep(500);
delete []lpBuffer;
} catch(exception ex){
}
}
Sleep(100);
}
if(serial.IsOpened()) serial.Close();
}
int main(int argc, char* argv[]) {
//Initialize OpenCV
CvSize size640x480 = cvSize(640, 480);
CvCapture* p_capWebcam;
IplImage* p_imgOriginal;
IplImage* hsv;
//Initialize globals used throughout the program
ofstream dataFile;
int netTime = 0;
int intervalTime = 1000;
int hourCount = 1;
int avgCounter = 1; float avgH =0, avgS =0, avgV =0, avgH2 =0, avgS2 =0, avgV2 =0;
//More misc initialization for OpenCV and the program
CvMemStorage* p_strStorage;
char charCheckForEscKey;
p_capWebcam = cvCaptureFromCAM(0);
//Safety check to make sure the camera is there
if(p_capWebcam == NULL) {
printf("error: capture is NULL \n");
getchar();
return(-1);
}
//Declare video feed window
cvNamedWindow("Original", CV_WINDOW_AUTOSIZE);
//Create new image that will be the converted hsv values
hsv = cvCreateImage(size640x480, IPL_DEPTH_8U, 3);
//Infinite loop (until esc sequence)
while(1) {
//Get the current frame
p_imgOriginal = cvQueryFrame(p_capWebcam);
//Convert RGB to HSV in a separate image
cvCvtColor( p_imgOriginal, hsv, CV_BGR2HSV );
//Safety check to make sure camera actually captured stuff
if(p_imgOriginal == NULL) {
printf("error: frame is NULL \n");
getchar();
break;
}
//Initilization of vars for the average algorithm
float H=0, S=0, V=0, R=0, G=0, B=0, pixCount =1;
float H2=0, S2=0, V2=0, R2=0, G2=0, B2=0, pixCount2 = 1;
CvScalar curValHSV;
CvScalar curValRGB;
//Average algorithm that takes the average for 2 reactors and 2 sets of values (RGB and HSV)
for(int row=190; row<290; row++){
for(int col=150; col<250; col++){
curValHSV = cvGet2D(hsv,row,col);
curValRGB = cvGet2D(p_imgOriginal,row,col);
H=(H*(pixCount-1)+curValHSV.val[0])/pixCount;
S=(S*(pixCount-1)+curValHSV.val[1])/pixCount;
V=(V*(pixCount-1)+curValHSV.val[2])/pixCount;
B=(B*(pixCount-1)+curValRGB.val[0])/pixCount;
G=(G*(pixCount-1)+curValRGB.val[1])/pixCount;
R=(R*(pixCount-1)+curValRGB.val[2])/pixCount;
pixCount++;
}
for(int col=390; col<490; col++){
curValHSV = cvGet2D(hsv,row,col);
curValRGB = cvGet2D(p_imgOriginal,row,col);
H2=(H2*(pixCount-1)+curValHSV.val[0])/pixCount;
S2=(S2*(pixCount-1)+curValHSV.val[1])/pixCount;
V2=(V2*(pixCount-1)+curValHSV.val[2])/pixCount;
B2=(B2*(pixCount-1)+curValRGB.val[0])/pixCount;
G2=(G2*(pixCount-1)+curValRGB.val[1])/pixCount;
R2=(R2*(pixCount-1)+curValRGB.val[2])/pixCount;
pixCount2++;
}
}
printf("H1=%f S1=%f V1=%f\n", H, S, V);
printf("H2=%f S2=%f V2=%f\n", H2, S2, V2);
//Draw circles of the average colour of each reactor
cvCircle(p_imgOriginal, cvPoint(213,500), 50, CV_RGB(R, G, B), CV_FILLED);
cvCircle(p_imgOriginal, cvPoint(426,500), 50, CV_RGB(R2, G2, B2), CV_FILLED);
//Draw helper "cross hair" rectangles
cvRectangle(p_imgOriginal, cvPoint(150,190), cvPoint(250,290), CV_RGB(255,0,0), 3);
cvRectangle(p_imgOriginal, cvPoint(390,190), cvPoint(490,290), CV_RGB(255,0,0), 3);
//Allocate necessary memory
p_strStorage = cvCreateMemStorage(0);
//Display video feed
cvShowImage("Original", p_imgOriginal);
//Start serial communication
CSerial serial;
//If opening the serial port is succesful
if (serial.Open(3, 9600))
{
//If the algae are green and are unsaturated/dying
if( (H>28 && H<44) && S<230){
//Send the microcontroller the message to turn the motor
static char szMessage = '1';
serial.SendData(&szMessage, 1);
serial.Close();
char * outputc = new char[6];
outputc[6]=0;
//Wait for servo success code and temperature
for(int i=0; i<6; i++){
outputc[i]=readCharLoop();
}
printf("Temperature: %s degrees C \n\n", outputc);
int tens = outputc[2]-48;
int ones = outputc[3]-48;
int dec = outputc[5]-48;
float num = tens*10+ones+0.1*dec;
}
else{
static char szMessage = '0';
serial.SendData(&szMessage, 1);
serial.Close();
char * outputc = new char[5];
outputc[5]=0;
//Wait for temperature to come in
for(int i=0; i<5; i++){
outputc[i]=readCharLoop();
}
printf("Temperature: %s degrees C \n\n", outputc);
int tens = outputc[1]-48;
int ones = outputc[2]-48;
int dec = outputc[4]-48;
float num = tens*10+ones+0.1*dec;
}
}
else
printf("Failed to open port!\n\n\n");
//Release memory
cvReleaseMemStorage(&p_strStorage);
//If esc key pressed, then break loop
charCheckForEscKey = cvWaitKey(10);
if(charCheckForEscKey == 27) break;
//Placeholder time interval component (replace with time.h functions)
Sleep(intervalTime);
netTime += intervalTime;
//Save data locally to a .txt file
if(netTime/1000/60/60 >= hourCount)
{
avgH=(avgH*(avgCounter-1)+H)/avgCounter;
avgS=(avgS*(avgCounter-1)+S)/avgCounter;
avgV=(avgV*(avgCounter-1)+V)/avgCounter;
avgH2=(avgH2*(avgCounter-1)+H2)/avgCounter;
avgS2=(avgS2*(avgCounter-1)+S2)/avgCounter;
avgV2=(avgV2*(avgCounter-1)+V2)/avgCounter;
if (avgCounter >=5){
dataFile.open("data.txt", std::ios_base::app);
dataFile << "Time since beginning: "<< hourCount << ":00 hours \n" ;
dataFile << "HSV (SD): " << avgH << ", " << avgS << ", "<< avgV << "\n";
dataFile << "HSV (C): " << avgH2 << ", " << avgS2 << ", "<< avgV2 << "\n\n";
dataFile.close();
hourCount +=1;
avgCounter=0;
avgH =0; avgS =0; avgV =0; avgH2 =0; avgS2 =0; avgV2 =0;
}
avgCounter+=1;
}
} // end while
//Release more memory and destroy the window when done
cvReleaseCapture(&p_capWebcam);
cvDestroyWindow("Original");
return(0);
}
int __cdecl _tmain (int argc, char** argv)
{
//------------ I N I C I O M A I N D E L P R O G R A M A D E L R O B O T-----------//
//inicializaion de variables
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
ActionGos go(500, 350);
robot.addAction(&go, 48);
ActionTurns turn(400, 110);
robot.addAction(&turn, 49);
ActionTurns turn2(400, 110);
robot.addAction(&turn2, 49);
// presionar tecla escape para salir del programa
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("Presionar ESC para salir\n");
// uso de sonares para evitar colisiones con las paredes u
// obstaculos grandes, mayores a 8cm de alto
ArActionLimiterForwards limiterAction("limitador velocidad cerca", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("limitador velocidad lejos", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Inicializon la funcion de goto
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Finaliza el goto si es que no hace nada
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parser del CLI
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Conexion del robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// enciende motores, apaga sonidos
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);
// ============================ INICIO CONFIG COM =================================//
CSerial serial;
LONG lLastError = ERROR_SUCCESS;
// Trata de abrir el com seleccionado
lLastError = serial.Open(_T("COM3"),0,0,false);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible abrir el COM"));
// Inicia el puerto serial (9600,8N1)
lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Imposible setear la config del COM"));
// Register only for the receive event
lLastError = serial.SetMask(CSerial::EEventBreak |
CSerial::EEventCTS |
CSerial::EEventDSR |
CSerial::EEventError |
CSerial::EEventRing |
CSerial::EEventRLSD |
CSerial::EEventRecv);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));
// Use 'non-blocking' reads, because we don't know how many bytes
// will be received. This is normally the most convenient mode
// (and also the default mode for reading data).
lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
// ============================ FIN CONFIG COM =================================//
bool first = true;
int goalNum = 0;
int color = 3;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// inicia el primer punto
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++; //cambia de 0 a 1 el contador
printf("El contador esta en: --> %d <---\n",goalNum);
if (goalNum > 20)
goalNum = 1;
//comienza la secuencia de puntos
if (goalNum == 1)
{
gotoPoseAction.setGoal(ArPose(1150, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
// Create the sound queue.
ArSoundsQueue soundQueue;
// Run the sound queue in a new thread
soundQueue.runAsync();
std::vector<const char*> filenames;
filenames.push_back("sound-r2a.wav");
soundQueue.play(filenames[0]);
}
else if (goalNum == 2)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn.myActivate = 1;
turn.myDirection = 1;
turn.activate();
ArUtil::sleep(1000);
turn.deactivate();
turn.myActivate = 0;
turn.myDirection = 0;
robot.lock();
}
else if (goalNum == 3)
{
gotoPoseAction.setGoal(ArPose(1150, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
// Imprimo algunos datos del robot como posicion velocidad y bateria
robot.lock();
ArLog::log(ArLog::Normal, "Posicion=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Bateria=%.2fV",
robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getBatteryVoltage());
robot.unlock();
}
else if (goalNum == 4)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 5)
{
gotoPoseAction.setGoal(ArPose(650, 2670));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 6)
{
printf("Gira 90 grados izquierda\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 1;
turn2.activate();
ArUtil::sleep(1000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 7)
{
gotoPoseAction.setGoal(ArPose(650, 0));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 8)
{
gotoPoseAction.setGoal(ArPose(1800,1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 9)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 10)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 850));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
gotoPoseAction.setGoal(ArPose(3500, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1550));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 11)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 613));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 2)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
goalNum = 19;
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1785));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 12)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 13)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3500, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3500, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 14)
{
robot.unlock();
//Valor para el while
bool fContinue = true;
// <<<<<<------------- 1 Parte Secuencia: BAJA BRAZO ------------->>>>>> //
lLastError = serial.Write("b");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
//-------------------------E S C U C H A C O M ----------------------------//
do
{
// Wait for an event
lLastError = serial.WaitEvent();
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));
// Save event
const CSerial::EEvent eEvent = serial.GetEventType();
// Handle break event
if (eEvent & CSerial::EEventBreak)
{
printf("\n### BREAK received ###\n");
}
// Handle CTS event
if (eEvent & CSerial::EEventCTS)
{
printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
}
// Handle DSR event
if (eEvent & CSerial::EEventDSR)
{
printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
}
// Handle error event
if (eEvent & CSerial::EEventError)
{
printf("\n### ERROR: ");
switch (serial.GetError())
{
case CSerial::EErrorBreak: printf("Break condition"); break;
case CSerial::EErrorFrame: printf("Framing error"); break;
case CSerial::EErrorIOE: printf("IO device error"); break;
case CSerial::EErrorMode: printf("Unsupported mode"); break;
case CSerial::EErrorOverrun: printf("Buffer overrun"); break;
case CSerial::EErrorRxOver: printf("Input buffer overflow"); break;
case CSerial::EErrorParity: printf("Input parity error"); break;
case CSerial::EErrorTxFull: printf("Output buffer full"); break;
default: printf("Unknown"); break;
}
printf(" ###\n");
}
// Handle ring event
if (eEvent & CSerial::EEventRing)
{
printf("\n### RING ###\n");
}
// Handle RLSD/CD event
if (eEvent & CSerial::EEventRLSD)
{
printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
}
// Handle data receive event
if (eEvent & CSerial::EEventRecv)
{
// Read data, until there is nothing left
DWORD dwBytesRead = 0;
char szBuffer[101];
do
{
// Lee datos del Puerto COM
lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));
if (dwBytesRead > 0)
{
//Preseteo color
int color = 0;
// Finaliza el dato, asi que sea una string valida
szBuffer[dwBytesRead] = '\0';
// Display the data
printf("%s", szBuffer);
// <<<<<<----------- 2 Parte Secuencia: CIERRA GRIPPER ----------->>>>>> //
if (strchr(szBuffer,76))
{
lLastError = serial.Write("c");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 3 Parte Secuencia: SUBE BRAZO ------------->>>>>> //
if (strchr(szBuffer,117))
{
lLastError = serial.Write("s");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<------------- 4 Parte Secuencia: COLOR ------------->>>>>> //
if (strchr(szBuffer,72))
{
lLastError = serial.Write("C");
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
}
// <<<<<<---------- 5.1 Parte Secuencia: COLOR ROJO---------->>>>>> //
if (strchr(szBuffer,82))
{
color = 1;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.2 Parte Secuencia: COLOR AZUL ---------->>>>>> //
if (strchr(szBuffer,66))
{
color = 2;
//salir del bucle
fContinue = false;
}
// <<<<<<---------- 5.3 Parte Secuencia: COLOR VERDE ---------->>>>>> //
if (strchr(szBuffer,71))
{
color = 3;
//salir del bucle
fContinue = false;
}
}
}
while (dwBytesRead == sizeof(szBuffer)-1);
}
}
while (fContinue);
// Close the port again
serial.Close();
robot.lock();
}
else if (goalNum == 15)
{
printf("Gira 180 grados derecha\n");
robot.unlock();
turn2.myActivate = 1;
turn2.myDirection = 2;
turn2.activate();
ArUtil::sleep(2000);
turn2.deactivate();
turn2.myActivate = 0;
turn2.myDirection = 0;
robot.lock();
}
else if (goalNum == 16)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(3300, 413));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(3300, 1985));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 17)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 603));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1795));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 18)
{
if (color == 1)
{
gotoPoseAction.setGoal(ArPose(2800, 860));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if (color == 3)
{
gotoPoseAction.setGoal(ArPose(2800, 1540));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
else if (goalNum == 19)
{
gotoPoseAction.setGoal(ArPose(2600, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
else if (goalNum == 20)
{
gotoPoseAction.setGoal(ArPose(1800, 1199));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "No puede llegar al punto, y la aplicacion saldra en %d", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(10);
}
// Robot desconectado al terminal el sleep
Aria::shutdown();
//------------ F I N M A I N D E L P R O G R A M A D E L R O B O T-----------//
return 0;
}
int __cdecl _tmain (int /*argc*/, char** /*argv*/)
{
CSerial serial;
LONG lLastError = ERROR_SUCCESS;
// Attempt to open the serial port (COM1)
lLastError = serial.Open(_T("COM1"),0,0,false);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port"));
// Setup the serial port (9600,8N1, which is the default setting)
lLastError = serial.Setup(CSerial::EBaud9600,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting"));
// Register only for the receive event
lLastError = serial.SetMask(CSerial::EEventBreak |
CSerial::EEventCTS |
CSerial::EEventDSR |
CSerial::EEventError |
CSerial::EEventRing |
CSerial::EEventRLSD |
CSerial::EEventRecv);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port event mask"));
// Use 'non-blocking' reads, because we don't know how many bytes
// will be received. This is normally the most convenient mode
// (and also the default mode for reading data).
lLastError = serial.SetupReadTimeouts(CSerial::EReadTimeoutNonblocking);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port read timeout."));
// Keep reading data, until an EOF (CTRL-Z) has been received
bool fContinue = true;
do
{
// Wait for an event
lLastError = serial.WaitEvent();
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to wait for a COM-port event."));
// Save event
const CSerial::EEvent eEvent = serial.GetEventType();
// Handle break event
if (eEvent & CSerial::EEventBreak)
{
printf("\n### BREAK received ###\n");
}
// Handle CTS event
if (eEvent & CSerial::EEventCTS)
{
printf("\n### Clear to send %s ###\n", serial.GetCTS()?"on":"off");
}
// Handle DSR event
if (eEvent & CSerial::EEventDSR)
{
printf("\n### Data set ready %s ###\n", serial.GetDSR()?"on":"off");
}
// Handle error event
if (eEvent & CSerial::EEventError)
{
printf("\n### ERROR: ");
switch (serial.GetError())
{
case CSerial::EErrorBreak: printf("Break condition"); break;
case CSerial::EErrorFrame: printf("Framing error"); break;
case CSerial::EErrorIOE: printf("IO device error"); break;
case CSerial::EErrorMode: printf("Unsupported mode"); break;
case CSerial::EErrorOverrun: printf("Buffer overrun"); break;
case CSerial::EErrorRxOver: printf("Input buffer overflow"); break;
case CSerial::EErrorParity: printf("Input parity error"); break;
case CSerial::EErrorTxFull: printf("Output buffer full"); break;
default: printf("Unknown"); break;
}
printf(" ###\n");
}
// Handle ring event
if (eEvent & CSerial::EEventRing)
{
printf("\n### RING ###\n");
}
// Handle RLSD/CD event
if (eEvent & CSerial::EEventRLSD)
{
printf("\n### RLSD/CD %s ###\n", serial.GetRLSD()?"on":"off");
}
// Handle data receive event
if (eEvent & CSerial::EEventRecv)
{
// Read data, until there is nothing left
DWORD dwBytesRead = 0;
char szBuffer[101];
do
{
// Read data from the COM-port
lLastError = serial.Read(szBuffer,sizeof(szBuffer)-1,&dwBytesRead);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to read from COM-port."));
if (dwBytesRead > 0)
{
// Finalize the data, so it is a valid string
szBuffer[dwBytesRead] = '\0';
// Display the data
printf("%s", szBuffer);
// Check if EOF (CTRL+'[') has been specified
if (strchr(szBuffer,EOF_Char))
fContinue = false;
}
}
while (dwBytesRead == sizeof(szBuffer)-1);
}
}
while (fContinue);
// Close the port again
serial.Close();
return 0;
}
int _tmain(int argc, _TCHAR* argv[])
{
printf("Camera Tracker\n");
#ifdef ENABLE_SERIAL
serial.Open(4, 57600);
resetGyro = true;
SendQuadcopterData();
#endif
VideoCapture cap(0);
Mat img;
bgImg = imread(bgFilename);
if (!bgImg.empty()) {
printf("Loaded BG img\n");
frameCounter = 10;
}
char * imageWindowName = "Image";
char * diffWindowName = "Diff";
namedWindow(imageWindowName);
namedWindow(diffWindowName);
createTrackbar("Lift", imageWindowName, &lift, 255, onLiftChange);
createTrackbar("Yaw", imageWindowName, &yaw, 255, onYawChange);
createTrackbar("Pitch", imageWindowName, &pitch, 255, onPitchChange);
createTrackbar("Roll", imageWindowName, &roll, 255, onRollChange);
createTrackbar("GyroDiv", imageWindowName, &gyroDiv, 255, onGyroDivChange);
while (1) {
cap.read(img);
if (img.empty()) {
printf("Unable to grab frame\n");
return 0;
}
if (frameCounter < 5)
img.copyTo(bgImg);
subtract(img, bgImg, fgImg);
threshold(fgImg, fgImg, 150, 255, CV_THRESH_BINARY);
imshow(imageWindowName, img);
imshow(diffWindowName, fgImg);
moveWindow(imageWindowName, 0, 0);
moveWindow(diffWindowName, 0, img.rows + 300);
char c = cvWaitKey(5);
if (c == 27)
break;
if (c == 'r')
frameCounter = 0;
if (c == 'g') {
resetGyro = true;
SendQuadcopterData();
}
if (c == ' ') {
lift = 0;
SendQuadcopterData();
}
if (c == 's') {
printf("Saving background image\n");
imwrite(bgFilename, img);
}
//heartbeat
if ((frameCounter % 15) == 0)
SendQuadcopterData();
frameCounter++;
}
#ifdef ENABLE_SERIAL
serial.Close();
#endif
return 0;
}
/**
<policy-file-request/>: Send flex policy
TT: Test
ET: Exit
OCx: Open COMx for reading
CC: Close COM
**/
void OnRead(SOCKET client)
{
char buf[BUFFER_SIZE];
memset(buf, 0, BUFFER_SIZE);
recv(client, buf, BUFFER_SIZE, 0);
// MessageBox(NULL, buf, "Socket Receive", MB_ICONINFORMATION);
if (strstr(buf, CMD_POLICY) == buf)
{
sprintf(buf, FLEX_DOMAIN_POLICY);
send(client, buf, strlen(buf) + 1, 0);
}
else if (strstr(buf, CMD_TEST) == buf)
{
sprintf(buf, "Welcome to Flex-Aide!\0");
send(client, buf, strlen(buf) + 1, 0);
}
else if (strstr(buf, CMD_EXIT) == buf)
{
PostQuitMessage(0);
}
else if (strstr(buf, CMD_OPENCOM) == buf)
{
int port = (buf[2] == 0) ? 1 : buf[2] - '0';
if (port < SERIAL_MIN || port > SERIAL_MAX) port = SERIAL_MIN;
if (!com.Open(port, 57600))
{
send(client, SERIAL_ERR, strlen(SERIAL_ERR) + 1, 0);
}
else
{
send(client, SERIAL_OK, strlen(SERIAL_OK) + 1, 0);
}
SetTimer(wNoop, TIMER_ID, 1000, ReadCOM);
}
else if (strstr(buf, CMD_CLOSECOM) == buf)
{
com.Close();
KillTimer(wNoop, TIMER_ID);
}
else if (strstr(buf, CMD_EXECUTE) == buf)
{
UINT uCmdShow = (buf[2] == 'H') ? SW_HIDE : SW_SHOWNORMAL;
char cmd[MAX_PATH], fcmd[MAX_PATH], ncmd[MAX_PATH];
memset(cmd, 0, MAX_PATH);
memset(fcmd, 0, MAX_PATH);
memset(ncmd, 0, MAX_PATH);
strncpy(cmd, buf + 3, strlen(buf) - 3);
LPCSTR lpCmdLine;
if (strstr(cmd, "..\\") == cmd)
{
strncpy(ncmd, cmd + 3, strlen(cmd) - 3);
sprintf(fcmd, "%s\\%s", szParentPath, ncmd);
lpCmdLine = fcmd;
}
else
{
lpCmdLine = cmd;
}
// WinExec(lpCmdLine, uCmdShow);
SHELLEXECUTEINFO exec = {0};
exec.cbSize = sizeof(SHELLEXECUTEINFO);
exec.fMask = SEE_MASK_NOCLOSEPROCESS;
exec.hwnd = NULL;
exec.lpVerb = NULL;
exec.lpFile = lpCmdLine;
exec.lpParameters = "";
exec.lpDirectory = NULL;
exec.nShow = uCmdShow;
exec.hInstApp = NULL;
ShellExecuteEx(&exec);
// WaitForSingleObject(exec.hProcess,INFINITE);
}
else
{
send(client, buf, strlen(buf) + 1, 0);
}
// MessageBox(NULL, buf, "Socket Send", MB_ICONINFORMATION);
}
bool transmit::changelamp(bool flag, int id, bool status, bool oldstatus){
char* buffer = new char[4];
bool tmp;
if(flag){
buffer[0] = '?';
}
else{
buffer[0] = '!';
}
if(true){
switch(id+1){
case 1:
buffer[1] = '1';
break;
case 2:
buffer[1] = '2';
break;
case 3:
buffer[1] = '3';
break;
case 4:
buffer[1] = '4';
break;
case 5:
buffer[1] = '5';
break;
case 6:
buffer[1] = '6';
break;
case 7:
buffer[1] = '7';
break;
case 8:
buffer[1] = '8';
break;
case 9:
buffer[1] = '9';
break;
case 10:
buffer[1] = 'a';
break;
case 11:
buffer[1] = 'b';
break;
case 12:
buffer[1] = 'c';
break;
case 13:
buffer[1] = 'd';
break;
case 14:
buffer[1] = 'e';
break;
case 15:
buffer[1] = 'f';
break;
default:
break;
}
}
if(status == oldstatus){
tmp = false;
return tmp;
}
if(status && status != oldstatus){
buffer[2] = 'f';
tmp = true;
}
if(!status && status != oldstatus){
buffer[2] = '0';
tmp = true;
}
buffer[3] = char(13);
if(true){
CSerial* serial = new CSerial();
if(!serial->Open(2, 9600))
{
qDebug() << "Could not open COM";
}
else{
serial->SendData(buffer, 4);
serial->Close();
}
delete serial;
}
qDebug() << buffer[0];
qDebug() << buffer[1];
qDebug() << buffer[2];
qDebug() << buffer[3];
return tmp;
}
