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;
}
void sentMapToMSP(){
cout << "[Serial] Opening Serial Port Comm. . . . " <<endl;
if (serial.Open(4, 9600)){
cout << "[Serial] Port opened successfully" << endl;
}else{
cout << "[Serial] Failed to open port!" << endl;
return ;
}
// send SOM_MAP to MSP 430
for(int i = 0 ; i < ROW ; i++ ){
for(int j = 0 ; j < COL ; j++ ){
for(int e = 0 ; e < element_count ; e++){
cout << "["<<i<<"]["<<j<<"]["<<e<<"]";
serial_sent_double(som_map[i][j].weights[e]);
}
}
}
for(int i = 0 ; i < ROW ; i++ ){
for(int j = 0 ; j < COL ; j++ ){
cout << "["<<i<<"]["<<j<<"]";
serial_sent_int(plotter[i][j]);
}
}
}
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;
}
void FlashTool::enterBootloader()
{
CSerial serial;
if(!serial.Open(ui->ucPortBox->itemData(ui->ucPortBox->currentIndex()).toString()))
{
QMessageBox::critical(this, "Error", "Could not open serial port.");
return;
}
serial.Setup(CSerial::EBaud115200);
serial.SetupHandshaking(CSerial::EHandshakeOff);
serial.SetEventChar(0x0D);
int version;
proto::SimplePacket<proto::CMD_INIT> init;
for(version = 0; version <= proto::VERSION+10; ++version)
{
init.header.version = version;
init.checksum = proto::packetChecksum(init.header, 0);
serial.write(&init, sizeof(init));
if(serial.WaitEvent(200) == ERROR_SUCCESS)
break;
}
proto::Packet<proto::CMD_RESET, proto::Reset> reset;
reset.header.version = version;
memcpy(&reset.payload.key, proto::RESET_KEY, sizeof(proto::RESET_KEY));
reset.updateChecksum();
serial.write(&reset, sizeof(reset));
}
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(int argc, char *argv[])
{
filename = "myFile.txt";
port = 5;
baudRate = 9600;
if(argc == 1)
{
cout << "No parameters specified, using defaults." << endl;
cout << "To specify use DantecTraverse.exe [port [filename [baudRate]]]" << endl;
}
else if(argc >= 2)
{
port = fromString<int>(argv[1]);
}
else if(argc >= 3)
{
filename = argv[2];
}
else if(argc >= 4)
{
baudRate = fromString<int>(argv[3]);
}
cout << "Parameters are: COM " << port << "; filename: \"" << filename << "\"; Baud Rate: " << baudRate << endl;
if(!serialPort.Open(port, baudRate))
{
cout << "Cannot open serial port " << port << " with Baud Rate " << baudRate << endl;
throw 1;
}
while(1)
{
//checks if the file ends with a full stop on a seperate line, this signifies that YAPP is finished writing to the file
ifstream iFile(filename.c_str());
string content((istreambuf_iterator<char>(iFile)), istreambuf_iterator<char>());
if(content.rfind("\n.") != string::npos)
{
cout << content << endl;
doCommand(content);
ofstream oFile(filename.c_str(), fstream::trunc);
}
int waiting = serialPort.ReadDataWaiting();
if(waiting)
{
char* buffer = new char[waiting];
serialPort.ReadData(buffer, waiting);
string incomming = buffer;
cout << "Incoming: " << buffer << endl;
delete buffer;
}
}
}
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;
}
void FlashTool::updatePorts()
{
CSerial serial;
ui->progPortBox->clear();
ui->ucPortBox->clear();
for(int i = 1; i < 50; ++i)
{
QString path = "\\\\.\\COM" + QString::number(i);
if(serial.Open(path))
{
ui->progPortBox->addItem("COM" + QString::number(i), path);
ui->ucPortBox->addItem("COM" + QString::number(i), path);
serial.close();
}
}
ui->progPortBox->addItem("usb (e.g. AVRISP mkII)", "usb");
}
int SerialSetup (CSerial::EBaudrate baud, CSerial::EDataBits data, CSerial::EParity par, CSerial::EStopBits stop, CSerial::EHandshake handshake)
{
// 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(baud, data, par, stop);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting"));
// Setup handshaking
lLastError = serial.SetupHandshaking(handshake);
if (lLastError != ERROR_SUCCESS)
return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port handshaking"));
// if no error
return 0;
}
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;
}
void serial_receive(){
int nBytesRead = 0,buf_idx =0,found = 0;
while(1){
if (serial.Open(4, 9600)&&serialEnable==1){
char* lpBuffer = new char[500];
char myrightBuffer[500];
while(found == 0){
nBytesRead = serial.ReadData(lpBuffer, 500);
char *start = strstr(lpBuffer, "R");
char *endptr = strstr(lpBuffer,"+");
if(start) {
strncpy(myrightBuffer, start, endptr-start);
break;
}
}
cout <<"[MSP430]> "<< myrightBuffer << endl;
serialEnable = 0;
delete []lpBuffer;
}
}
}
void sensorFeedback(CPipe motorDone, CPipe motor, MotorData ST400, CSerial serial){
while(motorDone.TestForData() == 0){
//Check if a sensor detects something, otherwise wait until
//motor indicates it has completed current instructions
/*Insert sensor logic here (maybe a separate thread or new class?)*/
//Make sure ST400.stop is set to 1 if a sensor interrupt occurs
//and write to ST400Control
if (serial.Open(4, 9600))
{
char* lpBuffer = new char[100];
int nBytesRead = serial.ReadData(lpBuffer, 100);
strtok(lpBuffer,"\n");
if(atoi(lpBuffer) == 1){
ST400.stop=1;
cout<<"HIT"<<endl;
//cout<<"YAY I READ FROM ARDRUINO!"<<endl;
motor.Write(&ST400, sizeof(ST400));
ST400.stop=0;
ST400.leftWheel=0; //Prevent rover from executing remaining steps
ST400.rightWheel=0;
break;
}
else{
ST400.stop=0;
cout<<"MISS"<<endl;
}
}
}
};
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;
}
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);
}
int main (){
CProcess p1("C:\\March01\\Debug\\ColourDetection.exe", // pathlist to child program executable
NORMAL_PRIORITY_CLASS, // priority
OWN_WINDOW, // process has its own window
ACTIVE // process is active immediately
);
CProcess p3("C:\\March01\\Debug\\ST400Control.exe", // pathlist to child program executable
NORMAL_PRIORITY_CLASS, // priority
OWN_WINDOW, // process has its own window
ACTIVE // process is active immediately
);
double debugging;
double obstacleSensor = 0;
double steps = 400; // Set to 400 half-steps per Revolution.
string sensorVoltage;
double sensorVolt;
struct botData roverData; // For Reading data from Image Processor.
roverData.posXbot = 0.0;
roverData.posYbot = 0.0;
roverData.g_angle = 0.0;
CSerial serial; // CREATE A SERIAL Object
if (serial.Open(8, 9600))
printf("Port opened successfully");
else
printf("Failed to open port!");
cout << "Please Enter Debugging Mode \n";
cout << "0 - Send A Number of Steps to Left and Right Wheels. \n";
cout << "1 - Rover Travels to a X-Y Coordinate we type in. \n";
cout << "2 - Rover Follows One Ball autonomously. \n";
cout << "3 - Find all balls autonomously. \n";
cin >> debugging;
Rover rover1;
if(debugging==0){
cout << "Entered Debugging Mode 0: Enter Left followed by Right Wheel Motor Steps \n";
CProcess p("C:\\March01\\Debug\\KeyboardInput.exe", // pathlist to child program executable
NORMAL_PRIORITY_CLASS, // priority
OWN_WINDOW, // process has its own window
ACTIVE // process is active immediately
);
cout << "\n Right after Process created";
CPipe pleft("leftMotorPipe",1024);
CPipe pright("rightMotorPipe",1024);
cout << "\n After pleft and pright pipes created\n\n";
double leftWheelRead=0;
double rightWheelRead=0;
double leftFirst=0;
double rightFirst=0;
ST400.leftWheel=0;
ST400.rightWheel=0;
ST400.stop = 0;
CPipe motor("MotorData",1024); // WRITE-ONLY pipeline to ST400Control
CPipe motorDone("Stopped", 1024); // READ-ONLY pipeline from ST400Control
int status = 0; // =1 if robot motion is complete
while(true){
//Check if the user has input values for both the left and right wheels and read the data if they have.
if(pleft.TestForData()>=sizeof(leftWheelRead) && pright.TestForData()>=sizeof(rightWheelRead))
{
pleft.Read(&leftWheelRead,sizeof(leftWheelRead));
cout << "leftWheel is: " <<leftWheelRead << "\n";
pright.Read(&rightWheelRead,sizeof(rightWheelRead));
cout << "rightWheel is: " <<rightWheelRead << "\n";
}//
//Nested while loop - only executes when there are steps to be travelled
while(leftWheelRead!=0 && rightWheelRead!=0)
{
//If the steps are not equal for each motor, turn rover
if(abs(leftWheelRead) != abs(rightWheelRead)){
//Turn Left
if(leftWheelRead>rightWheelRead)
{
leftFirst = (leftWheelRead - rightWheelRead)/2.0;
rightFirst = -leftFirst;
leftWheelRead = rightWheelRead;
printf("Turning left...\n");
}
//Turn right
else{
rightFirst = (rightWheelRead - leftWheelRead)/2.0;
leftFirst = -rightFirst;
rightWheelRead = leftWheelRead;
printf("Turning right...\n");
}
ST400.leftWheel = (leftFirst<5000) ? leftFirst : 5000;
ST400.rightWheel = (rightFirst<5000) ? rightFirst : 5000;
//Send the instructions to ST400Control
motor.Write(&ST400, sizeof(ST400));
//sensorFeedback(motorDone, motor, ST400, serial); // THE WHILE LOOP REPLACED HERE.
motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop
if(status==1)
{
printf("Motion completed...\n");
}
}
//Otherwise move straight ahead
else{
ST400.rightWheel= (rightWheelRead < 5000) ? rightWheelRead : 5000;
ST400.leftWheel= (leftWheelRead < 5000) ? leftWheelRead : 5000;
leftWheelRead=0;
rightWheelRead=0;
motor.Write(&ST400,sizeof(ST400));
printf("Driving straight ahead...\n");
//sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE.
motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop
if(status==1)
{
printf("Motion completed...\n");
}
}
}
char cc = cvWaitKey(1);
if(cc == 27) break;
}
} // End of Debugging 0.
else if (debugging == 1){
//double xInput;
//ifstream inFile;
//
//inFile.open("c:\\xcoord.txt");
//if (!inFile) {
// cout << "Unable to open file";
// exit(1); // terminate with error
//}
//
//while (inFile >> xInput) {
// cout << "X coordinate is: " << xInput << endl;
//}
//
//inFile.close();
//
//double yInput;
//ifstream inFile1;
//
//inFile1.open("c:\\ycoord.txt");
//if (!inFile1) {
// cout << "Unable to open file";
// exit(1); // terminate with error
//}
//
//while (inFile1 >> yInput) {
// cout << "Y coordinate is: " << yInput << endl;
//}
//
//inFile1.close();
CPipe motor("MotorData",1024); //WRITE-ONLY pipeline to ST400Control
CPipe motorDone("Stopped",1024); //READ-ONLY pipeline from ST400Control
ST400.leftWheel=0;
ST400.rightWheel=0;
ST400.stop=0;
int status = 0; //=1 if robot motion is complete
CPipe pMainPipe("MainPipe",1024);
while(true){
cout << "Entered Debugging Mode 1: Enter Coordinates for Rover to Go to. \n" ;
double xInput, yInput;
cout << "Please Enter Destination X Coordinate: [0 to 720] \n" ;
cin >> xInput;
cout << "Please Enter Destination Y Coordinate: [0 to 480] \n" ;
cin >> yInput;
//char c = cvWaitKey(1);
//if(c == 27) break;
while(roverData.posXbot < 170 || roverData.posXbot > 570
|| roverData.posYbot < 2 || roverData.posYbot > 474){
pMainPipe.Read(&roverData,sizeof(roverData));
}
rover1.coordinate.x = roverData.posXbot; // RANDY: Display rover x coordinate from this variable.
rover1.coordinate.y = roverData.posYbot; // RANDY: Display rover y coordinate from this variable.
rover1.direction = roverData.g_angle; // RANDY: Display rover angle from this variable.
Point2D ball(xInput, yInput);
double ballxUpper = ball.x+30;
double ballxLower = ball.x-30;
double ballyUpper = ball.y+30;
double ballyLower = ball.y-30;
if((rover1.coordinate.x < ballxUpper)
&& (rover1.coordinate.x > ballxLower)
&& (rover1.coordinate.y < ballyUpper)
&& (rover1.coordinate.y > ballyLower)){
//return 0;
}
// Calculate Heading.
rover1.headingRange = rover1.calcHeading(ball,rover1.coordinate,rover1.direction);
double headingInRevolutions = rover1.headingRange.heading*980/360;// * (1/(10*PI))*((16*PI)/360); // RANDY: Display headingInRevolutions and rename to turning steps.
// Calculate Range.
double rangeInRevolutions = rover1.headingRange.range*6.04;// * 0.508/(10*PI); // RANDY: Display rangeinRevolutions and rename to range steps.
/* Motor control starts here ************************************************************************/
if(headingInRevolutions < -50){ // If ClockWise. LEFTWHEEL = MORE.
// double turnRate;
ST400.leftWheel = headingInRevolutions;
ST400.rightWheel = -headingInRevolutions;
//ST400.leftWheel = ST400.leftWheel/2.0;
//ST400.rightWheel = -ST400.leftWheel;
motor.Write(&ST400, sizeof(ST400));
headingInRevolutions = 0; // Reset Turning to 0 so that rover will now go straight.
//sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE.
motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop
while(motorDone.TestForData() == 0){ // Wait until done. - Added.
}
}
else if(headingInRevolutions > 50){ // If CounterClockWise. RIGHTWHEEL = MORE.
//ST400.leftWheel = (headingInRevolutions<5000) ? -headingInRevolutions/2 : 5000;
//ST400.rightWheel = (headingInRevolutions<5000) ? headingInRevolutions/2 : 5000;
ST400.leftWheel = -headingInRevolutions;
ST400.rightWheel = headingInRevolutions;
motor.Write(&ST400, sizeof(ST400));
headingInRevolutions = 0; // Reset Turning to 0 so that rover will now go straight.
//sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE.
motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop
while(motorDone.TestForData() != 0){ // Wait until done. - Added.
}
}
//if(headingInRevolutions==0){ // If Straight.
ST400.leftWheel = rangeInRevolutions;
ST400.rightWheel = rangeInRevolutions;
if(motor.TestForData() == 0 && ST400.rightWheel!=0){
motor.Write(&ST400,sizeof(ST400));
printf("Going Straight: wrote identical data to both wheels\n");
//Reset these commands to 0 after sending the instruction
//so the motors run for the input steps only once
ST400.leftWheel=0;
ST400.rightWheel=0;
//sensorFeedback(motorDone, motor, ST400, serial);// THE WHILE LOOP REPLACED HERE.
motorDone.Read(&status,sizeof(status)); //Empty pipe to prevent infinite loop
while(motorDone.TestForData() != 0){ // Wait until done. - Added.
}
}
if(motor.TestForData()==0)
{
printf("Pipe empty\n");
}
//char cc = cvWaitKey(1);
//if(cc == 27) break;
//} // End of While Loop.
//serial.Close();
}
}// End of Debugging Mode 1.
int main()
{
//Leap Motion Vairables
Leap::Controller controller;
Leap::Frame frame;
Leap::HandList hands;
Leap::Hand h1;
Leap::FingerList fingers;
Leap::Finger index;
Leap::Finger thumb;
Leap::PointableList pointables;
float indexX = 0, indexY = 0, indexZ = 0, thumbX = 0, thumbY = 0, thumbZ = 0, sum = 0;
unsigned long cycles = 0;
// TCP Variables
WSADATA wsaData;
SOCKET connectSocket = INVALID_SOCKET;
struct addrinfo* result = NULL;
struct addrinfo* ptr = NULL;
struct addrinfo hints;
char cSendBuf[512][512];
char sSendBuf[512];
int iResult;
int recvBufLen = DEFAULT_BUFLEN;
int i = 0;
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0) {
printf("WSAStartup failed with error: %d\n", iResult);
return 1;
}
// Initialize all address info to 0 to start.
SecureZeroMemory(&hints, sizeof(hints));
hints.ai_family = AF_UNSPEC; // Doesn't matter if we use IPV4 or IPV6
hints.ai_socktype = SOCK_STREAM; // TCP Stream sockets
hints.ai_protocol = IPPROTO_TCP;
// Resolve the server address and port
iResult = getaddrinfo("127.0.0.1", DEFAULT_PORT, &hints, &result);
if (iResult != 0) {
printf("getaddrinfo failed with error: %d\n", iResult);
WSACleanup();
return 1;
}
// Attempt to connect to an address until one succeeds
for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
// create a socket for connecting to the server
connectSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol);
if (connectSocket == INVALID_SOCKET) {
printf("socket failed with error: %ld\n", WSAGetLastError());
WSACleanup();
return 1;
}
// Connect to the server
iResult = connect(connectSocket, ptr->ai_addr, (int)ptr->ai_addrlen);
if (iResult == SOCKET_ERROR) {
closesocket(connectSocket);
connectSocket = INVALID_SOCKET;
continue;
}
break;
}
// Deallocate the address info
freeaddrinfo(result);
if (connectSocket == INVALID_SOCKET) {
printf("Unable to connect to server!\n");
WSACleanup();
return 1;
}
// Setup serial port connection and needed variables.
SerialPort.Open(PORT_NUM, BAUD);
Controller[20].value = 9; //Verification Byte sent to make sure everything else ends up in the right location
FillByteSize();
while (true)
{
cycles++;
UpdateControllerState(); //Updates all values on the controller
WORD wButtons = g_Controllers[CONTROLLER1].state.Gamepad.wButtons;
//Stores all of the values from the controller into the controller structure
Controller[0].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRX;
Controller[1].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRY;
Controller[2].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLX;
Controller[3].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLY;
Controller[4].value = (g_Controllers[CONTROLLER1].state.Gamepad.bRightTrigger);
Controller[5].value = (g_Controllers[CONTROLLER1].state.Gamepad.bLeftTrigger);
Controller[6].value = (wButtons & XINPUT_GAMEPAD_RIGHT_THUMB);
Controller[7].value = (wButtons & XINPUT_GAMEPAD_LEFT_THUMB);
Controller[8].value = (wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER);
Controller[9].value = (wButtons & XINPUT_GAMEPAD_LEFT_SHOULDER);
Controller[10].value = (wButtons & XINPUT_GAMEPAD_DPAD_UP);
Controller[11].value = (wButtons & XINPUT_GAMEPAD_DPAD_DOWN);
Controller[12].value = (wButtons & XINPUT_GAMEPAD_DPAD_LEFT);
Controller[13].value = (wButtons & XINPUT_GAMEPAD_DPAD_RIGHT);
Controller[14].value = (wButtons & XINPUT_GAMEPAD_A);
Controller[15].value = (wButtons & XINPUT_GAMEPAD_B);
Controller[16].value = (wButtons & XINPUT_GAMEPAD_Y);
Controller[17].value = (wButtons & XINPUT_GAMEPAD_X);
Controller[18].value = (wButtons & XINPUT_GAMEPAD_START);
Controller[19].value = (wButtons & XINPUT_GAMEPAD_BACK);
CheckDeadZone();
if (controller.isConnected() == true)
{
sum = 0;
frame = controller.frame();
hands = frame.hands();
h1 = hands[0];
fingers = frame.fingers();
thumb = fingers[0];
index = fingers[1];
pointables = frame.pointables();
Leapvalues[0].value = h1.palmVelocity().x;
Leapvalues[1].value = h1.palmVelocity().y;
Leapvalues[2].value = h1.palmVelocity().z;
Leapvalues[3].value = h1.direction().pitch()*Leap::RAD_TO_DEG;
Leapvalues[4].value = h1.direction().yaw()*Leap::RAD_TO_DEG;
Leapvalues[5].value = h1.direction().roll()*Leap::RAD_TO_DEG;
indexX = index.tipPosition().x;
indexY = index.tipPosition().y;
indexZ = index.tipPosition().z;
thumbX = thumb.tipPosition().x;
thumbY = thumb.tipPosition().y;
thumbZ = thumb.tipPosition().z;
Leapvalues[6].value = sqrt(pow((indexX - thumbX), 2) + pow((indexY - thumbY), 2) + pow((indexZ - thumbZ), 2));
leapConnected = true;
CheckLeapDeadZone();
}
for (i = 6; i < NUMBER_OF_BUTTONS; i++) //DO NOT SET TO <= NUMBER_OF_BUTTONS, NOT A MISTAKE. Verification bit should always keep its value
{
{
Controller[i].value = AnalogToDigital(Controller[i].value); //converts all of the button presses on the controller to a binary value
}
}
//turns all of the numerical values into buffers that can be passed to the arduino
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
{
_itoa_s(Controller[i].value, Controller[i].passedValue, 10);
}
/*
for (i = 0; i < NUMBER_OF_BUTTONS; i++) {
_itoa_s(Controller[i].value, cSendBuf[i], 10);
cSendBuf[i][strlen(cSendBuf[i])] = '\0';
iResult = send(connectSocket, cSendBuf[0], (int)strlen(cSendBuf[0]), 0);
printf("String sent: %s\n", cSendBuf[0]);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
}
// Try to send the packet
iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);
//printf("String sent: %s\n", sendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}*/
if (leapConnected = true)
{
for (i = 0; i < NUMBER_OF_LEAP_INPUTS; i++)
{
_itoa_s(Leapvalues[i].value, Leapvalues[i].passedValue, 10);
}
}
/*Values recieved in this order:
0) YAW
1) PITCH
2) ROLL
3) ACCELERATION ON X AXIS
4) ACCELERATION ON Y AXIS
5) ACCELERATION ON Z AXIS
6) SONAR SENSOR DISTANCE (IN METERS)
*/
if (SendData() == 1)
{
for (i = 0; i < 7; i++)
{
while (SerialPort.ReadDataWaiting() < 3)
{
}
if (i > 2 && i < 6)
SerialPort.ReadData(received[i], 5);
else
SerialPort.ReadData(received[i], 4);
std::cout << received[i] << ' ';
// Added this 6.25.14
strcpy(sSendBuf, received[i]);
sSendBuf[strlen(sSendBuf)] = '\0';
iResult = send(connectSocket, sSendBuf, (int)strlen(sSendBuf), 0);
printf("String sent: %s\n", sSendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
// Try to send the packet
iResult = send(connectSocket, "\n", (int)strlen("\n"), 0);
//printf("String sent: %s\n", sendBuf);
// Check for errors
if (iResult == SOCKET_ERROR) {
printf("send failed with error: %d\n", WSAGetLastError());
closesocket(connectSocket);
WSACleanup();
return 1;
}
}
}
//std::cout << recieved[0];
printf("\t%d", cycles);
printf("\n");
//Sleep(500); <<'\t' << recieved[1]
}
closesocket(connectSocket);
WSACleanup();
return 0;
}
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*/)
{
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 __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 main()
{
unsigned long cycles = 0;
int i = 0;
// ********************************************************************************************************************
// IMPORTANT: The serial port numbers here must correspond to those of the arduino on the ROV. The first number is the
// COM # and the second number is the baud rate set on the arduino.
SerialPort.Open(8, 115200);
// ********************************************************************************************************************
Controller[20].value = 9; //Verification Byte sent to make sure everything else ends up in the right location
FillByteSize();
while (true)
{
cycles++;
UpdateControllerState(); //Updates all values on the controller
WORD wButtons = g_Controllers[CONTROLLER1].state.Gamepad.wButtons;
//Stores all of the values from the controller into the controller structure
Controller[0].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRX;
Controller[1].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbRY;
Controller[2].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLX;
Controller[3].value = g_Controllers[CONTROLLER1].state.Gamepad.sThumbLY;
Controller[4].value = (g_Controllers[CONTROLLER1].state.Gamepad.bRightTrigger);
Controller[5].value = (g_Controllers[CONTROLLER1].state.Gamepad.bLeftTrigger);
Controller[6].value = (wButtons & XINPUT_GAMEPAD_RIGHT_THUMB);
Controller[7].value = (wButtons & XINPUT_GAMEPAD_LEFT_THUMB);
Controller[8].value = (wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER);
Controller[9].value = (wButtons & XINPUT_GAMEPAD_LEFT_SHOULDER);
Controller[10].value = (wButtons & XINPUT_GAMEPAD_DPAD_UP);
Controller[11].value = (wButtons & XINPUT_GAMEPAD_DPAD_DOWN);
Controller[12].value = (wButtons & XINPUT_GAMEPAD_DPAD_LEFT);
Controller[13].value = (wButtons & XINPUT_GAMEPAD_DPAD_RIGHT);
Controller[14].value = (wButtons & XINPUT_GAMEPAD_A);
Controller[15].value = (wButtons & XINPUT_GAMEPAD_B);
Controller[16].value = (wButtons & XINPUT_GAMEPAD_Y);
Controller[17].value = (wButtons & XINPUT_GAMEPAD_X);
Controller[18].value = (wButtons & XINPUT_GAMEPAD_START);
Controller[19].value = (wButtons & XINPUT_GAMEPAD_BACK);
CheckDeadZone();
for (i = 6; i < NUMBER_OF_BUTTONS; i++) //DO NOT SET TO <= NUMBER_OF_BUTTONS, NOT A MISTAKE. Verification bit should always keep its value
{
{
Controller[i].value = AnalogToDigital(Controller[i].value); //converts all of the button presses on the controller to a binary value
}
}
//turns all of the numerical values into buffers that can be passed to the arduino
for (i = 0; i <= NUMBER_OF_BUTTONS; i++)
{
_itoa_s(Controller[i].value, Controller[i].passedValue, 10);
}
if (SendData() == 1)
{
//SerialPort.ReadData(recieved[0], 3);
}
std::cout << Controller[5].value << Controller[4].value << Controller[1].value << std::endl; // This shows the value being passed to the arduino from the selected controller button (#3 = Left stick Y position)
}
return 0;
}
