//-----------------------------------------------------------------------------
int _tmain(int argc, _TCHAR* argv[])
{
Serial* SP = new Serial(COM_PORT); // Инициализация соединения
if (SP->IsConnected())
printf("We're connected\n\n");
else
printf("Connection refused\n\n");
char incomingData[BUF_SIZE] = ""; // Строка для посылки (данных)
int readResult = 0; // Каков размер нашей посылки
while (SP->IsConnected())
{
readResult = SP->ReadData(incomingData, BUF_SIZE-1);
// Если ничего нет readResult будет равно -1
if (DEBUG) printf("Bytes read: %i\n",readResult);
// Выводим для отладки кол-во байт переданных данных (или -1)
incomingData[readResult] = 0;
// Записываем конец строки после всех нужных данных
if (readResult > 0) printf("%s",incomingData);
// Eсли в посылке что-то было (и она была), то выводим ее
Sleep(UPD_TIME);
// Подождем пока, чтобы не вывести одно и тоже несколько раз
}
delete SP;
Sleep(2000);
return 0;
} // By SnipGhost 06.08.2016
// application reads from the specified serial port and reports the collected data
int _tmain(int argc, _TCHAR* argv[])
{
printf("Welcome to the serial test app!\n\n");
Serial* SP = new Serial("\\\\.\\COM13"); // adjust as needed
if (SP->IsConnected())
printf("We're connected");
char incomingData[2000] = ""; // don't forget to pre-allocate memory
//printf("%s\n",incomingData);
int dataLength = 1500;
int readResult = 0;
while(SP->IsConnected())
{
readResult = SP->ReadData(incomingData,dataLength);
// printf("Bytes read: (-1 means no data available) %i\n",readResult);
if (readResult == -1) continue;
std::string test(incomingData);
for (int i = 0; i < readResult; i++)
{
printf("%c", incomingData[i]);
}
// printf("%s",incomingData);
test = "";
Sleep(50);
}
return 0;
}
// application reads from the specified serial port and reports the collected data
int _tmain(int argc, _TCHAR* argv[])
{
printf("Welcome to the serial test app!\n\n");
Serial* SP = new Serial("COM4"); // adjust as needed
if (SP->IsConnected())
printf("We're connected\n");
char incomingData[256] = ""; // don't forget to pre-allocate memory
//printf("%s\n",incomingData);
int dataLength = 256;
int readResult = 0;
while(SP->IsConnected())
{
readResult = SP->ReadData(incomingData,dataLength);
// printf("Bytes read: (-1 means no data available) %i\n",readResult);
std::string test(incomingData);
printf("%s",incomingData);
test = "";
Sleep(500);
}
return 0;
}
int _tmain(int argc, _TCHAR* argv[])
{
cout << "Connecting to arduino..." << endl;
Serial* SP = new Serial("\\\\.\\COM7"); // adjust as needed
if (SP->IsConnected())
cout << "Connection established with Arduino!!" << endl; // Let us know the serial is connected
char incomingData[256] = ""; // don't forget to pre-allocate memory
int dataLength = 256;
int readResult = 0;
while(SP->IsConnected())
{
readResult = SP->ReadData(incomingData,dataLength);
//cout << "Bytes read: " << readResult << endl;
string test(incomingData);
if (readResult > 0) {
cout << incomingData << endl;
test = "";
}
Sleep(300);
}
cin >> argc;
return 0;
}
int main(int argc, char* argv[])
{
Serial * Arduino = new Serial("COM8");
cout << "Communicating with COM7 enter data to be sent\n";
char data[256] = "";
int nchar = 256;
char incomingData[256] = "";
while (Arduino->IsConnected())
{
cin >> data;
Arduino->WriteData(data, nchar);
Arduino->ReadData(incomingData, nchar);
cout << incomingData << endl;
}
return 0;
}
int main() {
Serial* port = new Serial("COM4");
if (port->IsConnected()) cout << "Connection established!!!" << endl;
int tempX = 1111;
int tempY = 1111;
bool automatic = true;
sendCommandToArduino(port, automatic, 'w', tempX, tempY);
//char data[256] = "";
//int datalength = 256;
//int readResult = 0;
//for (int i = 0; i < 256; ++i) { data[i] = 0; }
//string str;
//char command[] = "i";
/*char sendMe[1];
sendMe[0] = '1';
sendMe[0] = '0';
int msglen = strlen(sendMe);
while(true){
port->WriteData(sendMe, 2);
cout << "Please write two digit number (00 = turn off, 01 = turn on, 11 = blink): ";
cin >> sendMe[0];
cin >> sendMe[1];
system("pause");
if(sendMe[0] == '1')
sendMe[0] = '0';
else
sendMe[0] = '1';
}
//while (1) {
// while (port->ReadData(data, 256) != -1) {
// printf("%s", data);
// }
//} */
system("pause");
return 0;
}
Serial* connectToPort(){
string lPort;
cout << "Port: ";
cin >> lPort;
#ifdef __WIN32__
string lfilePath = "\\\\.\\COM"+ lPort;
#endif
#ifdef __linux__
string lfilePath = "/dev/"+ lPort;
#endif
Serial * pSerial = new Serial(lfilePath.c_str());
if(!pSerial->IsConnected()){
cout << "Unable to connect to Serial Port :" + lPort << endl;
return NULL;
}
else{
cout << "Connection established" << endl;
return pSerial;
}
}
int *LedAmountTest(char *Comm)
{
Serial* SP = new Serial(Comm);
if (SP->IsConnected())
std::cout << "Connected with COM5" << std::endl;
bool exit = false;
//je kan hier op escape drukken om het programma af ste sluiten hier
std::cout << "Waiting for Arduino. Press ESC to quit" << std::endl;
char Rx_buffer[2] = "";
while (Rx_buffer[0] != '0') //blijf hier hangen tot de arduino klaar is
{
Sleep(100);
SP->ReadData(Rx_buffer, 2);
if (GetAsyncKeyState(VK_ESCAPE))
{
exit = true;
}
if (exit == true)
{
std::cout << "Something went wrong with communication. Check baudrate settings and COM port" << std::endl;
}
}
Rx_buffer[0] = '0';
std::cout << "Got response from arduino sending amount of leds" << std::endl;
//Stuur de hoeveelheid leds naar de arduino
UINT8 Tx_buffer[600 * 3];
ZeroMemory(Tx_buffer, 600 * 3);
Tx_buffer[0] = 600 >> 8 & 0x00FF;
Tx_buffer[1] = 600 & 0x00FF;
SP->WriteData((char*)Tx_buffer, 2);
std::cout << "Press up to add a LED and press down to remove one." << std::endl;
std::cout << "When you are satisfied press SPACEBAR to confirm" << std::endl;
std::cout << "Press ESC to quit" << std::endl;
//onderstaande stukje code zal blijven draaien tot je op ESC drukt
static int leds[4] = { 0 }, i = 0;
int offset = 0;
leds[0]++;
while (i <4)
{
if (GetAsyncKeyState(VK_SPACE)) //Als escape is ingedrukt zet exit true
{
offset += leds[i];
i++;
Sleep(500);
}
if (GetAsyncKeyState(VK_UP))
{
leds[i]++;
ZeroMemory(Tx_buffer, 600 * 3);
for (int j = 0; j < leds[i]; j++)
{
for (int k = 0; k < 3; k++)
{
Tx_buffer[(j + offset) * 3 + k] = 0xff;
}
}
SP->WriteData((char*)Tx_buffer, 1800);
Sleep(50);
}
if (GetAsyncKeyState(VK_DOWN) && leds[i] > 1)
{
leds[i]--;
ZeroMemory(Tx_buffer, 600 * 3);
for (int j = 0; j < leds[i]; j++)
{
for (int k = 0; k < 3; k++)
{
Tx_buffer[(j + offset) * 3 + k] = 0xff;
}
}
SP->WriteData((char*)Tx_buffer, 1800);
Sleep(50);
}
}
SP->~Serial();
return leds;
}
int main()
{
std::cout << "Aero will be disabled for performance reason!" << std::endl;
DisableAeroTheme();
bool exit = false; //dit is voor later een escape variable
int cap_method = D3D_CAP;
float gamma = 0.6;
GDICap Cap;
Direct3DCap D3DCap; //init directx9
JackFFT JACK;
std::ifstream myinfile;
myinfile.open("./Config.txt");
//Als het bestand is geopend bestaat die al dus sla je het maken over
if (!myinfile.is_open())
{
myinfile.close();
std::ofstream myfile;
myfile.open("./Config.txt");
CreateConfig(myfile, D3DCap);
myfile.close();
myinfile.open("./Config.txt");
}
std::string STRING;
int Config[9] = { -1 };
for (int i = 0; i < 9; i++)
{
if (myinfile.eof())
break;
std::getline(myinfile, STRING);
Config[i] = atoi(STRING.c_str());
}
myinfile.close();
//Check of config laden goed is gelukt
int i = 0;
while (Config[i] != -1 && i < 9)
{
i++;
}
if (i < 9)
{
std::cout << "Config Loading went wrong! Please delete Config.txt and run this software again!" << std::endl;
return 0;
}
std::cout << "Using screen: " << Config[0] << " for capturing" << std::endl;
D3DCap.init(Config[0]);
Cap.init(Config[0]); //
UINT32 *pBits;
switch (cap_method)
{
case GDI_CAP:
pBits = Cap.pBits;
break;
case D3D_CAP:
pBits = D3DCap.pBits;
break;
case JAC_CAP:
pBits = JACK.pBits;
}
ScreenCalc Scherm(105, //init de kleur bereken functies
pBits, //De PixelData
D3DCap.return_hres(), //De Hori Resolutie
D3DCap.return_vres(), //De Verti Resolutie
Config[1], //Hoeveel procent die moet nemen aan de bovenkant/onderkant
Config[2], //Hoeveel procent die aan de zijkant moet nemen
Config[3], //Leds Boven
Config[5], //Leds Onder
Config[4], //Leds Links
Config[6], //Leds Rechts
Config[7]);
Scherm.Bereken_Grid(); //stel de hoeveelheid leds in die worden gebruikt en bereken Grid Grootte
Scherm.set_Gamma(gamma);
//Het programma moet eerst 0xff binnen krijgen als dat het geval is dan mag die beginnen met het oversturen
//van de hoeveelheid leds
//Als die hoeveelheden overeenkomen mag die beginnen met het zenden van led data
std::string String;
String = "\\\\.\\COM";
String += std::to_string(Config[8]);
char *temp = new char[String.size() + 1];
std::copy(String.begin(), String.end(), temp);
temp[String.size()] = '\0';
Serial* SP = new Serial(temp);
delete[] temp;
temp = nullptr;
if (SP->IsConnected())
std::cout << "Connected with COM5" << std::endl;
else
{
std::cout << "Communication Error. Exiting" << std::endl;
return 0;
}
char Rx_buffer[100] = ""; //Dit is de Rx_buffer deze moet een char zijn
//je kan hier op escape drukken om het programma af ste sluiten hier
std::cout << "Waiting for Arduino. Press ESC to quit" << std::endl;
SP->ReadData(Rx_buffer, 2);
while (Rx_buffer[0] != '0') //blijf hier hangen tot de arduino klaar is
{
Sleep(100);
SP->ReadData(Rx_buffer, 2);
if (GetAsyncKeyState(VK_ESCAPE))
{
exit = true;
}
if (exit == true)
{
std::cout << "Something went wrong with communication. Check baudrate settings and COM port" << std::endl;
return 0; //beeindig de software
}
}
Rx_buffer[0] = '0';
std::cout << "Got response from arduino sending amount of leds" << std::endl;
//Stuur de hoeveelheid leds naar de arduino
UINT8 Tx_buffer[600 * 3];
ZeroMemory(Tx_buffer, 600 * 3);
Tx_buffer[0] = Scherm.geefLeds() >> 8 & 0x00FF;
Tx_buffer[1] = Scherm.geefLeds() & 0x00FF;
SP->WriteData((char*)Tx_buffer, 2);
Sleep(1000); //Wacht 1 seconde op de arduino
std::cout << "Press END to quit capturing" << std::endl;
UINT8 *pointer; //Pointer voor de led bitstream
clock_t klok1;
clock_t klok2 = 50;
pointer = Scherm.GeefLedPointer(); //Koppel de led bitstream aan de pointer
// maak een thread die nu nog niks doet
std::thread uart(send_data,SP,Rx_buffer,Scherm,pointer);
while (exit == false)
{
//std::cout << " " << "\r";
//std::cout << "FPS: " << ((1 * CLOCKS_PER_SEC) / (clock() - klok2)) << "\r";
klok2 = clock();
if (GetAsyncKeyState(VK_END)) //Als escape is ingedrukt zet exit true
{
exit = true;
}
else if (GetAsyncKeyState(VK_F8))
{
JACK.stop();
pBits = Cap.pBits;
Scherm.set_data(pBits);
cap_method = GDI_CAP;
std::cout << "Changed capture method to GDI " << std::endl;
Sleep(100);
}
else if (GetAsyncKeyState(VK_F9))
{
JACK.stop();
pBits = D3DCap.pBits;
Scherm.set_data(pBits);
cap_method = D3D_CAP;
std::cout << "Changed capture method to D3D " << std::endl;
Sleep(100);
}
else if (GetAsyncKeyState(VK_F7))
{
cap_method = JAC_CAP;
JACK.init(pointer);
std::cout << "Changed capture method to Jack Audio " << std::endl;
JACK.start();
Sleep(100);
}
else if (GetAsyncKeyState(VK_F12))
{
gamma += 0.01;
Scherm.set_Gamma(gamma);
}
else if (GetAsyncKeyState(VK_F11))
{
gamma -= 0.01;
Scherm.set_Gamma(gamma);
}
else if (GetAsyncKeyState(VK_F10))
{
std::cout << "Gamma : " << gamma << std::endl;
}
//start een thread die de data stuurt
//Scherm.Calc_Aspect_ratio();
//Maak screenshot en sla die op
switch (cap_method)
{
case GDI_CAP:
Cap.capture();
break;
case D3D_CAP:
D3DCap.capture();
break;
case JAC_CAP:
Sleep(10);
//when audio is selected everything will be handled in a different thread
break;
}
if (cap_method != JAC_CAP)
{
Scherm.Bereken();
Scherm.Calc_Aspect_ratio();
}
//wacht tot alle data verzonden is en we weer antwoord hebben gehad dat alles in orde is voordat we weer verder gaan
}
Thread_comm = 0;
uart.join();
return 0;
}
void MpuReader::run()
{
//連線藍芽
#ifndef USE_WIRE_CONNECTION
blue_connector = new cwz_c_blue();
int result = blue_connector->connect(this->stopFlag);
if(result == 1)//如果成功連線
emit connectionCreated();
#endif
//********************************************************//
// 變數宣告
//********************************************************//
//Handle Serial Buffer
const int incomming_buf_len = 2048;
char incomingData[incomming_buf_len] = ""; // don't forget to pre-allocate memory
int dataLength = 256;
const int bSize = 10000;
char buffer[bSize] = "";
int bLen = 0;
//Decode information from buffer string
int count = 0;
int flag = -1; //-1等於上次是不合法結束的情況
int accl[3]; // x, y, z
int gyro[3]; // x, y, z
float gravity[3];
float quatern[4];
int buttons[2];
int period;
//中位值濾波
int MFLen = 5; //濾波器長度為5
int **Accls = allcIntDArray(MFLen, 3);
int **Gyros = allcIntDArray(MFLen, 3);
int MFCount = 0; // 只用於前五次, 確認filter的data set是不是被裝滿了, 裝滿了才開始濾波
//Symbol record
//SymbolRecorder SR = SymbolRecorder();
//fileNameSStream << racketFilePath << date_string << "_" << type_string << "_" << person_string << "_" << racket_file_count << ".txt";
//********************************************************//
// 連接Com port並確認連線
//********************************************************//
#ifdef USE_WIRE_CONNECTION
std::cout << "Arduino with GY521 board and chip MPU6050!" << std::endl;
char *COM_NUM = "COM5";
Serial* SP = new Serial(COM_NUM); // adjust as needed
if (SP->IsConnected()){
emit connectionCreated();
std::cout << "We're connected" << std::endl;
}else{
stopFlag = true;
}
//read few set of data until it's getting stable
waitUntilSerialStable(SP, incomingData, dataLength);
std::cout << "Data should be stable, start to read MPU6050" << std::endl;
#else
blue_connector->send("Start to read mpu6050 data from bluetooth.\n");
#endif
while(!stopFlag){
#ifdef USE_WIRE_CONNECTION
if(!SP->IsConnected()){
std::cout << "Failed to read " << COM_NUM << " Mpu6050 reader is off." << std::endl;
emit readingEnded();
break;
}
if( !readSerialIntoBuffer(SP, incomingData, dataLength, readResult, bLen, bSize, buffer) )
continue;//If no valid serial data are read or buffer overflow, skip this round
#else
dataLength = blue_connector->receive(incomingData, incomming_buf_len);
if(dataLength > 0){//有收到東西
incomingData[dataLength] = '\0';
strcpy_s(&buffer[bLen], bSize, incomingData);
bLen += dataLength;
}
#endif
//decode mpu6050 data from buffer str, take off processed part from buffer
strcpy_s(buffer, bSize, getAccelAndGyro(&count, &flag, &bLen, buffer, accl, gyro, quatern, buttons, &period, bSize));
// 每一組合法數值為 (acclX,acclY,acclZ,gyroX,gyroY,gyroZ,quatern1,quatern2,quatern3,quatern4,buttonRight,buttonLeft,readperiod)
if( flag != GETAANDG_NEW_VALID_DATA )
continue;//尚未讀到整組完整資料 不處理 繼續讀
SHORT downKeyState = GetAsyncKeyState( VK_DOWN );
//std::cout << "gravity: " << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << std::endl;
if( writeRawToFile )
{//按ctrl鍵
std::cout << "period: " << period << std::endl;
if( ( 1 << 16 ) & downKeyState ){
//writeAcclAndGyroAndGravity(fileNameSStream.str().c_str(), accl, gyro, gravity, 1);
writeMpu6050RawToFile(fileNameSStream.str().c_str(), accl, gyro, quatern, period, 1);
//std::cout << "Down Key State: " << 1 << std::endl;
}else{
//writeAcclAndGyroAndGravity(fileNameSStream.str().c_str(), accl, gyro, gravity, 0);
writeMpu6050RawToFile(fileNameSStream.str().c_str(), accl, gyro, quatern, period, 0);
//std::cout << "Down Key State: " << 0 << std::endl;
}
}
//********************************************************//
// 處理加速度計與陀螺儀Raw Data
//********************************************************//
//get gravity
QtoGravity(quatern, gravity);
//std::cout << "Gyro: " << gyro[0] << "," << gyro[1] << "," << gyro[2] << std::endl;
//去重力
removeGravity(accl, gravity);
double total_q = sqrt(gravity[0]*gravity[0] + gravity[1]*gravity[1] + gravity[2]*gravity[2]);
this->my_glwidget->setNewZ(gravity[0], gravity[1], gravity[2]);
//check if gravity value is invalid
if( total_q >= 1.1 ){
std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
std::cout << "total_q: " << total_q << std::endl;
std::cout << "Wrong gravity ............................................" << std::endl;
std::cout << "quatern: " << quatern[0] << "," << quatern[1] << "," << quatern[2] << "," << quatern[3] << std::endl;
std::cout << "gravity: " << gravity[0] << "," << gravity[1] << "," << gravity[2] << std::endl;
std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
system("PAUSE");
}
//Median Filter(中位值濾波)
agMedianFilter(Accls, Gyros, accl, gyro, MFCount, MFLen, true, true); //是否做濾波 accl=true gyro=true
}
#ifdef USE_WIRE_CONNECTION
SP->~Serial();
#else
blue_connector->disconnect();
#endif
//釋放中位值濾波使用的記憶體空間
freeIntDArray(Accls, MFLen);
freeIntDArray(Gyros, MFLen);
}
int main()
{
FILE * xFile;
xFile = fopen("motor1x.csv", "w");
FILE * yFile;
yFile = fopen("motor1y.csv", "w");
FILE * thetaFile;
thetaFile = fopen("motor1theta.csv", "w");
FILE * refFile;
refFile = fopen("ref.csv", "w");
FILE * f1File;
f1File = fopen("f1.csv", "w");
FILE * f2File;
f2File = fopen("f2.csv", "w");
FILE * gFile;
gFile = fopen("total.csv", "w");
cv::Mat pesos;
IplImage* pesos3;
pesos3 = cvCreateImage(cvSize(1, 1), 8, 3);
// Config Debug
clock_t visionI, visionF, estraI, estraF, controlI, controlF, commI, commF, totalI, totalF, debugI, debugF, start;
vector<robot> whishPath, followedPath;
int p = 0;
float counter = 0;
char char_pressed;
int time = 1;
int side = 1;
bool paused = true;
CvRect ROI = cvRect(0, 0, 0, 0);
robot canto_area1 = robot(-1, -1);
robot canto_area2 = robot(-1, -1);
robot canto_gol1 = robot(-1, -1);
robot canto_gol2 = robot(-1, -1);
int amin = 21;
int amax = 84;
int radius = 20;
int min_distance = 54;
int parametro[36] = { 94, 143, 80, 121, 253, 25,
23, 134, 126, 52, 255, 255,
33, 81, 154, 180, 255, 255,
55, 189, 91, 94, 229, 255,
180, 255, 255, 0, 0, 0,
4, 156, 102, 14, 255, 255 };
//CvRect ROI = cvRect(36, 46, 266, 162);
//robot canto_area1 = robot(65, 67);
//robot canto_area2 = robot(244, 167);
//robot canto_gol1 = robot(43, 66);
//robot canto_gol2 = robot(282, 173);
//int amin = 20;
//int amax = 250;
//int radius = 20;
//int min_distance = 50;
//int parametro[36] = { 88, 139, 80, 121, 253, 255,
// 22, 59, 141, 55, 204, 255,
// 117, 58, 113, 141, 113, 255,
// 52, 82, 89, 97, 168, 169,
// 0, 79, 123, 19, 149, 191,
// 0, 129, 182, 18, 251, 255 };
int debug_colors[7][3] = { { 128, 0, 128 }, // Roxo
{ 0, 64, 0 }, // Verde
{ 117, 58, 0 }, // Marrom
{ 255, 255, 0 }, // Amarelo
{ 255, 255, 0 }, // Amarelo
{ 255, 255, 0 }, // Amarelo
{ 255, 128, 0 } }; // Laranja
if (time == 1)
{
for (int k = 0; k < 3; k++)
{
debug_colors[3 + k][0] = 0; // Azul
debug_colors[3 + k][1] = 0; // Azul
debug_colors[3 + k][2] = 255; // Azul
}
}
// Config Controle
// Parametros do PID na seguinte ordem: PF, IF, DF, PT, IT, DT, PFR, IFR, DFR, PTR, ITR, DTR
float param0[12] = { 11.45, 1.49, 26.9, 13.59, 1.63, 25.86, 1.03, 0.14, 9.36, 1.4, 0.23, 9.44 };
float param1[12] = { 25.72, 1.01, 20.07, 22.34, 0.94, 21.1, 10, 0.64, 4.92, 10, 0.61, 6.92 };
float param2[12] = { 21.86, 1.06, 18, 22, 1.03, 25.03, 3.59, 0.32, 4.94, 2.9, 0.89, 3.79 };
CvFont fonte;
cvInitFont(&fonte, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, 8);
cvNamedWindow("Live", CV_WINDOW_AUTOSIZE);
//cvNamedWindow("Filtro Atual");
//cvNamedWindow("Estrategia");
// Inicialização da Visão
vision Camera;
Camera.createCamera(0);
Camera.getImage(ROI);
Camera.configParam(parametro, amin, amax, time, min_distance, ROI);
// Inicialização da Estratégia
estrategia Estrategia(Camera.h, Camera.w, side, canto_gol1, canto_gol2, canto_area1, canto_area2);
Estrategia.Config(Camera.h, Camera.w);
pesos = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, 1);
Estrategia.ConfigAreas(canto_gol1, canto_gol2, canto_area1, canto_area2);
// Inicialização do Controle
controle_bili Controle_bili;
// Inicialização do Serial
Serial* SP = new Serial("COM4");
if (SP->IsConnected())
printf("We're connected\n");
//============================== VARIÁVEIS DE TESTE ============================//
int contador = 0;
float t;
float ref = 0.0, T = 40000.0, f1 = 0.0, f2 = 0.0, torque = 0.0, fx_B = 0.0, d = 3.75, KP_w = 0.0, KI_w = 0.0, flag = 0.0, K, KP_u, R = 55.0, a, dist = 0.0;
//float x = Camera.players[0].x,e =0.0;
//float y = Camera.players[0].y;
Camera.players[0].theta_acumulated = 0;
Camera.players[0].theta_old = 0;
//float theta = Camera.players[0].theta;
//float t = visionF - start;
//float v1 = 15;
//float v2 = -15;
float c, alpha = 0.0, biel = 0.0, v1_send = 0.0, v2_send = 0.0;
float dx0, dy0, Kp, /*v1 = 0.0, v2 = 0.0,*/ theta2 = 0.0, thetad = 0.0, Kt, refaux = 0.0, i = 0.0, o = 0.0, ang_error = 0.0;
float ct, st, erro_angulo = 0.0, x_bola = 0.0, y_bola = 0.0, thetad_old = 0.0, u_i = 0.0, KI_u = 0.0;
float w = 0.0, u = 0.0, w_i = 0.0, w_p = 0.0, h = 0.0, wc = 0.0, uc = 7.0, f = 300.0, u_p = 0.0, x_0 = 155, y_0 = 116;
h = 1.0 / 12.0;
int xd = 0, yd = 0;
char temp[2];
char tempread[2];
//char msg[12];
//byte sum;
float xd_old = 0.0, yd_old = 0.0;
float n_xd = Camera.players[0].x, n_yd = Camera.players[0].y;
start = clock();
Camera.players[0].refaux_acumulated = 0.0;
Camera.players[0].refaux_old = 0.0;
Camera.players[0].refaux = 0.0;
int contar = 0, inicio_voltas = 0.0;
char incomingData[256] = ""; // don't forget to pre-allocate memory
//printf("%s\n",incomingData);
int dataLength = 256;
int readResult = 0;
double t1 = 0;
double tChecksum_IN;
double tChecksum_OUT;
//double tRecieve_IN;
double tRecieve_OUT;
int loop = 0;
byte sum = 0;
byte recievedCheckSum = 0;
int left = 0;
int right = 0;
int recievedValue = 0;
float vADC = 0;
float voltagem = 0;
int ze = 255;
int newV1 = 0;
int newV2 = 0;
// int loop;
int change_loop = 1; // o valor dessea variavel fica entre 1 e 6, ela escolhe qual loop estaremos usando
char point = 'i'; // i-> ponto inicial do teste, f-> ponto final do teste
char param = '0'; // 0-> proporcional, 1-> integral, 2-> derivativo
int step_flag = 1; // 1-> passo alto, 2-> passo medio, 3-> passo baixo, 4-> achamos o valor "ótimo";
int analise = 0; // 0->análise do ts, 1->análise do overshoot, 2->análise do upertime
//=========================== INÍCIO DO LOOP ==============================//
while (true)
{
//====================Leitura Mensagem================================ //
readResult = SP->ReadData(incomingData, 11);
/*for (int i = 0; i<11; i++)
{
cout << dec << i << ": ";
cout << hex << (int)incomingData[i] << endl;
//cout << "haha" << endl;
//system("pause");
}*/
//O endereço do xbee de onde veio a mensagem é incomingData[4] e incomingData[5]
if (incomingData[0] == 0x7e && incomingData[1] == 0x00 && incomingData[2] == 0x07 && incomingData[3] == -127 && incomingData[4] == -86 && incomingData[5] == -86 && incomingData[7] == 0)
{
//cout << "_-_-_-_-_-_" << endl;
sum = 0;
recievedCheckSum = 0;
left = 0;
right = 0;
recievedValue = 0;
voltagem = 0;
vADC = 0;
//loop = 0;
for (int i = 0; i < 11; i++)
{
if (i>2 && i < 10)
{
sum = sum + incomingData[i];
}
}
sum = 0xFF - sum;
sum = 0xFF & sum;
if (incomingData[10] & 0x111111)
{
recievedCheckSum = (int)(incomingData[10] - 0xffffff00);
//cout << "HAHA" << endl;
}
else
{
recievedCheckSum = (int)(incomingData[10]);
}
if (recievedCheckSum == sum)
{
//cout << "Mensagem Valida" << endl;
//system("cls");
//cout << "left: " << hex << (int)incomingData[8] << endl;
//cout << "right: " << hex << (int)incomingData[9] << endl;
//cout << "check: " << dec << ((int)incomingData[9]) << endl;
if (incomingData[8] & 0x1111111)
{
//cout << "caso 1" << endl;
left = (int)(incomingData[8] - 0xffffff00);
left = left - 256;
}
else if (incomingData[8] & 0x111111)
{
//cout << "caso 2" << endl;
left = (int)(incomingData[8] - 0xffffff00);
//left = left -256;
}
else
{
//cout << "caso 3" << endl;
left = (int)(incomingData[8]);
}
//cout << dec << (int)(incomingData[9]) << endl;
if (incomingData[9] & 0x1111111)
{
right = (int)(incomingData[9] - 0xffffff00);
if ((int)incomingData[9] > 0)
{
right = right - 256;
}
//right = right - 256;
//cout << "caso 1" << endl;
}
else if (incomingData[9] & 0x111111)
{
right = (int)(incomingData[9] - 0xffffff00);
//left = left -256;
//cout << "caso 2" << endl;
}
else
{
right = (int)(incomingData[9]);
//cout << "caso 3" << endl;
}
/*if (incomingData[9] & 0x111111)
{
right = (int)(incomingData[9] - 0xffffff00);
}
else
{
right = (int)(incomingData[9]);
}*/
//cout <<"esquerda: " << left<< endl;
//cout << "direita: " << right << endl;
recievedValue = 768 + right;
vADC = recievedValue * 0.00489;
voltagem = 0.013414862*recievedValue;
/*
cout << "Total: " << dec << recievedValue << endl;
cout << "ADC: " << dec << vADC << endl;
printf("Voltagem: %0.2f V \n\n", voltagem);
cout << loop++ << endl;
*/
//cout << "Tempo receber: " << clock() - t1 << "ms" << endl;
//cout << "Recieve: " << tRecieve_OUT - t1 << "ms" << endl;
//cout << "Checksum: " << tChecksum_IN - tChecksum_IN << "ms" << endl;
//cout << loop++ << endl;
/*if (recievedValue < 1024)
{
system("cls");
voltagem = 0.0135*recievedValue;
cout << "Total: " << dec << recievedValue << endl;
printf("Voltagem: %0.2f V \n\n", voltagem);
cout << "Tempo receber: " << clock() - t1 << "ms" <<endl;
cout << "Recieve: " << tRecieve_OUT - t1 << "ms" << endl;
cout << "Checksum: " << tChecksum_IN - tChecksum_IN << "ms" << endl;
cout << loop++ << endl;
}*/
//cout << "Voltagem: " << dec << voltagem << endl;
}
}
else //=== Mensagem Inválida
{
char incomingData[256] = "";
}
/*for (int i = 0; i < 11; i++)
{
cout << i << ": " << hex << (int)incomingData[i] << endl;
}*/
//cout << endl << endl << endl << endl;
//cout << char(incomingData[6]) << endl;
//std::string test(incomingData);
//printf("%s", incomingData);
//test = "";
//==================================================================== //
//cout << "theta inicio = " << Camera.players[0].theta << endl;
counter++;
totalI = clock();
// =========================== VISÃO =========================== //
visionI = clock();
Camera.getImage();
Camera.dataAcquire();
Camera.players[1] = robot(-1, -1);
Camera.players[2] = robot(-1, -1);
visionF = clock();
// =========================== ESTRATEGIA =========================== //
estraI = clock();
//Atribuição de comportamentos
// Estrategia.update(Camera.players);
// Estrategia.comportamento3();
// Estrategia.Posse();
//Estrategia.players[0].comportamento = 3;
//Estrategia.players[1].comportamento = 3;
//Estrategia.players[2].comportamento = 2;
// Atribuição de objetivos
// Estrategia.goleiro4();
// Estrategia.lateral();
// Estrategia.atacante2();
// Estrategia.zagueiro();
//Estrategia.generalRouting();
//Estrategia.players[0].setGoal(23, 98);
//Estrategia.players[1].setGoal(23, 98);
estraF = clock();
// =========================== CONTROLE =========================== //
/*controlI = clock();
Controle00.controla(Camera.players[0], robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y), robot(Estrategia.players[0].objective_x, Estrategia.players[0].objective_y));
Controle01.controla(Camera.players[1], robot(Estrategia.players[1].goal_x, Estrategia.players[1].goal_y), robot(Estrategia.players[1].objective_x, Estrategia.players[1].objective_y));
Controle02.controla(Camera.players[2], robot(Estrategia.players[2].goal_x, Estrategia.players[2].goal_y), robot(Estrategia.players[2].objective_x, Estrategia.players[2].objective_y));
controlF = clock();*/
// =========================== COMUNICAÇÃO =========================== //
commI = clock();
//if (!clicked.stop)
//{
// xd = clicked.x -x_0;
// yd = -(clicked.y - y_0);
//}
//if (clicked.stop)
//{
// xd = Camera.players[0].x - x_0;
// yd = -(Camera.players[0].y- y_0);
//}
//xd = Camera.players[6].x - x_0;
//yd = -(Camera.players[6].y - y_0);
//yd = sin((6.28 / T) * t)*R;
//xd = cos((6.28 / T) * t)*R;
/*if (Camera.players[6].x == -1){
xd = xd_old;
yd = yd_old;
}
else{
xd_old = xd;
yd_old = yd;
}*/
///// -------------------------------------- MODIFICAÇÃO BIEL -----------------------------------------------------/////
/*
VERIFICAR PRECISAO DAS VARIAVEIS DE REFERÊNCIA -> PARA SABER SE AS CONDIÇÕES SERÃO SATISFEITAS
VERIFICAR QUAIS SÃO AS VARIAVEIS DE REFERÊNCIA
*/
// Apartir daqui procuraremos os ganho para o ajuste angular
if (change_loop == 1)
{
/*Loop para fazer o carro ir ate o ponto inicial do teste*/
xd = 120; //valor de x de um extremo do campo
yd = -9; //valor de y de um extremo do campo
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
thetad = 0.0;
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
{
change_loop = 2;
point = 'f';
}
}
}
else if (change_loop == 2)
{
if (point == 'f')
{
/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
thetad = 60.0;
// Nesse caso testaremos os ganhos de rotação
Camera.players[0].gain[0] = param0[3];
Camera.players[0].gain[1] = param0[4];
Camera.players[0].gain[2] = param0[5];
Camera.players[0].group_theta.push_back(Camera.players[0].theta);
if ((Camera.players[0].theta <= thetad + 0.05) && (Camera.players[0].theta <= thetad + 0.05))
{
Camera.players[0].condition_theta[0] = Controle_bili.find_ts('t', Camera.players[0], Camera.players[0].group_theta.size());
Camera.players[0].condition_theta[1] = Controle_bili.find_overshoot('t', Camera.players[0], Camera.players[0].group_theta.size());
Camera.players[0].condition_theta[2] = Controle_bili.find_upertime('t', Camera.players[0], Camera.players[0].group_theta.size());
point = 'i';
}
}
else if (point == 'i')
{
thetad = 0.0;
if ((Camera.players[0].x <= thetad + 0.09) && (Camera.players[0].x >= thetad - 0.09))
{
change_loop = 3;
point = 'f';
// O teste começara com o proporcional rotacional
param0[3] += 0.5;
Camera.players[0].group_theta.clear();
// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
Camera.players[0].old_gain[2] = Camera.players[0].gain[2];
Camera.players[0].gain[0] = param0[3];
Camera.players[0].gain[1] = param0[4];
Camera.players[0].gain[2] = param0[5];
}
}
}
else if (change_loop == 3)
{
if (point == 'f')
{
/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
thetad = 0.0;
Camera.players[0].group_theta.push_back(Camera.players[0].theta);
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta <= thetad + 0.09))
{
// Apartir daqui atribuiremos os valores anteriores de ts, overshoot e upertime
Camera.players[0].condition_theta[3] = Camera.players[0].condition_theta[0];
Camera.players[0].condition_theta[4] = Camera.players[0].condition_theta[1];
Camera.players[0].condition_theta[5] = Camera.players[0].condition_theta[2];
// Término das atribuições
//Começamos a medir os novos valores de ts, overshoot e upertime
Camera.players[0].condition_theta[0] = Controle_bili.find_ts('t', Camera.players[0], Camera.players[0].group_theta.size());
Camera.players[0].condition_theta[1] = Controle_bili.find_overshoot('t', Camera.players[0], Camera.players[0].group_theta.size());
Camera.players[0].condition_theta[2] = Controle_bili.find_upertime('t', Camera.players[0], Camera.players[0].group_theta.size());
// Término das novas atribuições
float wanted_ts = 50.0; // Não sei o valor de ts, chutei como 50
float wanted_overshoot = 50.0; // Não sei o valor de overshoot, chutei como 50
float wanted_upertime = 50.0; // Não sei o valor de upertime, chutei como 50
switch (param) // O switch é para sabermos qual ganho estamos analisando no momento
{
case '0':
// Ganho proporcional
if (analise == 0) //se igual a zero fazemos a analise de ts
{
step_flag = Controle_bili.improve_p_ts(Camera.players[0], 't', wanted_ts, step_flag);
}
else if (analise == 1) //se igual a 1 fazemos a anlise de overshoot
{
step_flag = Controle_bili.improve_p_overshoot(Camera.players[0], 't', wanted_overshoot, step_flag);
}
else if (analise == 2) // se igual a 2 fazemos a analise do tempo de subida (upertime)
{
step_flag = Controle_bili.improve_p_upertime(Camera.players[0], 't', wanted_upertime, step_flag);
}
if (step_flag == 4)
{
param = 1;
}
else
{
point = 'i';
}
param0[3] = Camera.players[0].gain[0];
break;
case '1':
// Ganho integral
if (analise == 0) //se igual a zero fazemos a analise de ts
{
step_flag = Controle_bili.improve_i_ts(Camera.players[0], 't', wanted_ts, step_flag);
}
else if (analise == 1) //se igual a 1 fazemos a anlise de overshoot
{
step_flag = Controle_bili.improve_i_overshoot(Camera.players[0], 't', wanted_overshoot, step_flag);
}
else if (analise == 2) // se igual a 2 fazemos a analise do tempo de subida (upertime)
{
step_flag = Controle_bili.improve_i_upertime(Camera.players[0], 't', wanted_upertime, step_flag);
}
if (step_flag == 4)
{
param = 2;
}
else
{
point = 'i';
}
param0[4] = Camera.players[0].gain[1];
break;
case '2':
// Ganho derivativo
step_flag = Controle_bili.improve_d_ts(Camera.players[0], 't', wanted_ts, step_flag);
if ((step_flag == 4) && (analise == 0))
{
param = 1;
analise = 1;
}
else if ((step_flag == 4) && (analise == 1))
{
param = 1;
analise = 2;
}
else if ((step_flag == 4) && (analise == 2))
{
change_loop = 4;
}
else
{
point = 'i';
}
param0[5] = Camera.players[0].gain[2];
break;
}
point = 'i';
}
}
else if (point == 'i')
{
thetad = 0.0;
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
{
point = 'f';
Camera.players[0].group_theta.clear();
// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
Camera.players[0].old_gain[2] = Camera.players[0].gain[2];
Camera.players[0].gain[0] = param0[0];
Camera.players[0].gain[1] = param0[1];
Camera.players[0].gain[2] = param0[2];
}
}
}
// Apartir daqui procuraremos os ganho para o ajuste linear
// O AJUSTE SERÁ FEITO POR UMA MOVIMENTAÇÃO NO EIXO X LOGO ALTERAREMOS OS GANHOS BASEADOS NESSE EIXO
else if (change_loop == 4)
{
/*Loop para fazer o carro ir ate o ponto inicial do teste*/
xd = 120; //valor de x de um extremo do campo
yd = -9; //valor de y de um extremo do campo
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
thetad = 0.0;
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
{
change_loop = 5;
point = 'f';
}
}
}
else if (change_loop == 5)
{
if (point == 'f')
{
/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
xd = 155; //valor de x do centro do campo
yd = -116; //valor de y do centro do campo
// Nesse caso testaremos os ganhos de deslocamento linear
Camera.players[0].gain[0] = param0[0];
Camera.players[0].gain[1] = param0[1];
Camera.players[0].gain[2] = param0[2];
Camera.players[0].group_x.push_back(Camera.players[0].x);
//Camera.players[0].group_y.push_back(Camera.players[0].y);
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
Camera.players[0].condition_x[0] = Controle_bili.find_ts('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[0] = control.find_ts('y', Camera.players[0], Camera.players[0].group_y.size());
Camera.players[0].condition_x[1] = Controle_bili.find_overshoot('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[1] = control.find_overshoot('y', Camera.players[0], Camera.players[0].group_y.size());
Camera.players[0].condition_x[2] = Controle_bili.find_upertime('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[2] = control.find_upertime('y', Camera.players[0], Camera.players[0].group_y.size());
point = 'i';
}
}
else if (point == 'i')
{
xd = 120; //valor de x de um extremo do campo
yd = -9; //valor de y de um extremo do campo
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
thetad = 0.0;
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
{
change_loop = 6;
point = 'f';
// O teste começara com o proporcional linear
param0[0] += 0.5;
Camera.players[0].group_x.clear();
//Camera.players[0].group_y.clear();
// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
Camera.players[0].old_gain[2] = Camera.players[0].gain[2];
Camera.players[0].gain[0] = param0[0];
Camera.players[0].gain[1] = param0[1];
Camera.players[0].gain[2] = param0[2];
}
}
}
}
else if (change_loop == 6)
{
if (point == 'f')
{
/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
xd = 155; //valor de x do centro do campo
yd = -116; //valor de y do centro do campo
Camera.players[0].group_x.push_back(Camera.players[0].x);
//Camera.players[0].group_y.push_back(Camera.players[0].y);
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
// Apartir daqui atribuiremos os valores anteriores de ts, overshoot e upertime
Camera.players[0].condition_x[3] = Camera.players[0].condition_x[0];
//Camera.players[0].condition_y[3] = Camera.players[0].condition_y[0];
Camera.players[0].condition_x[4] = Camera.players[0].condition_x[1];
//Camera.players[0].condition_y[4] = Camera.players[0].condition_y[1];
Camera.players[0].condition_x[5] = Camera.players[0].condition_x[2];
//Camera.players[0].condition_y[5] = Camera.players[0].condition_y[2];
// Término das atribuições
//Começamos a medir os novos valores de ts, overshoot e upertime
Camera.players[0].condition_x[0] = Controle_bili.find_ts('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[0] = control.find_ts('y', Camera.players[0], Camera.players[0].group_y.size());
Camera.players[0].condition_x[1] = Controle_bili.find_overshoot('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[1] = control.find_overshoot('y', Camera.players[0], Camera.players[0].group_y.size());
Camera.players[0].condition_x[2] = Controle_bili.find_upertime('x', Camera.players[0], Camera.players[0].group_x.size());
//Camera.players[0].condition_y[2] = control.find_upertime('y', Camera.players[0], Camera.players[0].group_y.size());
// Término das novas atribuições
float wanted_ts = 50.0; // Não sei o valor de ts, chutei como 50
float wanted_overshoot = 50.0;
float wanted_upertime = 50.0;
// OBS: COMO FAZER PARA ACHAR GANHOS DE X E Y E JUNTA-LOS NUM MOVIMENTO LINEAR?????????????
switch (param)// O switch é para sabermos qual ganho estamos analisando no momento
{
case '0':
// Ganho proporcional
if (analise == 0) //se igual a zero fazemos a analise de ts
{
step_flag = Controle_bili.improve_p_ts(Camera.players[0], 'X', wanted_ts, step_flag);
}
else if (analise == 1) //se igual a 1 fazemos a anlise de overshoot
{
step_flag = Controle_bili.improve_p_overshoot(Camera.players[0], 'X', wanted_overshoot, step_flag);
}
else if (analise == 2) // se igual a 2 fazemos a analise do tempo de subida (upertime)
{
step_flag = Controle_bili.improve_p_upertime(Camera.players[0], 'X', wanted_upertime, step_flag);
}
if (step_flag == 4)
{
param = 1;
}
else
{
point = 'i';
}
param0[0] = Camera.players[0].gain[0];
break;
case '1':
// Ganho integral
if (analise == 0) //se igual a zero fazemos a analise de ts
{
step_flag = Controle_bili.improve_i_ts(Camera.players[0], 'X', wanted_ts, step_flag);
}
else if (analise == 1) //se igual a 1 fazemos a anlise de overshoot
{
step_flag = Controle_bili.improve_i_overshoot(Camera.players[0], 'X', wanted_overshoot, step_flag);
}
else if (analise == 2) // se igual a 2 fazemos a analise do tempo de subida (upertime)
{
step_flag = Controle_bili.improve_i_upertime(Camera.players[0], 'X', wanted_upertime, step_flag);
}
if (step_flag == 4)
{
param = 2;
}
else
{
point = 'i';
}
param0[1] = Camera.players[0].gain[1];
break;
case '2':
// Ganho derivativo
step_flag = Controle_bili.improve_d_ts(Camera.players[0], 'x', wanted_ts, step_flag);
if ((step_flag == 4) && (analise == 0))
{
param = 1;
analise = 1;
}
else if ((step_flag == 4) && (analise == 1))
{
param = 1;
analise = 2;
}
else if ((step_flag == 4) && (analise == 2))
{
change_loop = 7;
}
else
{
point = 'i';
}
break;
}
point = 'i';
}
}
else if (point == 'i')
{
xd = 120; //valor de x de um extremo do campo
yd = -9; //valor de y de um extremo do campo
if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
{
thetad = 0.0;
if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
{
point = 'f';
Camera.players[0].group_x.clear();
//Camera.players[0].group_y.clear();
// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
Camera.players[0].old_gain[2] = Camera.players[0].gain[2];
Camera.players[0].gain[0] = param0[0];
Camera.players[0].gain[1] = param0[1];
Camera.players[0].gain[2] = param0[2];
}
}
}
}
else if (change_loop == 7)
{
cout << "Os ganhos são :" << endl;
cout << "GPL " << param0[0] << endl;
cout << "GIL " << param0[1] << endl;
cout << "GDL " << param0[2] << endl;
cout << "GPR " << param0[3] << endl;
cout << "GIR " << param0[4] << endl;
cout << "GDR " << param0[5] << endl;
}
///// -------------------------------------- TÉRMINO DA MODIFICAÇÃO BIEL -----------------------------------------------------/////
Controle_bili.rotina_controle(0, 0, &Camera, xd, yd);
newV2 = (0.000001947*pow(recievedValue, 2) - 0.004673*recievedValue + 3.495)*abs(Controle_bili.v2 * 255);
newV1 = (0.000001947*pow(recievedValue, 2) - 0.004673*recievedValue + 3.495)*abs(Controle_bili.v1 * 255);
//SP->sendApi(0, Controle_bili.o, abs(Controle_bili.v2 * 255), Controle_bili.i, abs(Controle_bili.v1 * 255));
SP->sendApi(0, Controle_bili.o, newV2, Controle_bili.i, newV1);
if (Camera.players[0].x != -1)
fprintf(gFile, "%f,%f,%f, %i, %f, %f,%f,%f,%f,%f,%f, %f\n", Controle_bili.x*1.3, Controle_bili.y*1.3, Controle_bili.theta, int(visionF - start), Controle_bili.thetad, Controle_bili.v1, Controle_bili.v2, w, xd*1.3, yd*1.3, u, Controle_bili.refaux);
//SP->sendApi(0, 1, 84, 0, 0);
//=================== DISPLAY DOS DADOS DO ROBO ============//
cout << "thetad = " << Controle_bili.thetad*180/M_PI << endl;
cout << "theta = " << Controle_bili.theta * 180 / M_PI << endl;
cout << "ang_error = " << Controle_bili.ang_error * 180 / M_PI << endl;
//cout << "ypc = " << dy0 << endl;
cout << "v1 = " << Controle_bili.v1 << endl;
cout << "v2 = " << Controle_bili.v2 << endl;
//cout << "u_p = " << Controle_bili.u_p << endl;
//======================== ENVIO DAS VELOCIDADES ====================//
if (!paused)
{
//SP->send(0, v2, v1);
//SP->send(v2, v1);
//SP->send(0, Controle00.v.x, Controle00.v.y);
// whishPath.push_back(robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y));
//sfollowedPath.push_back(Camera.players[0]);
//SP->send(1, Controle01.v.x, Controle01.v.y);
//SP->send(2, Controle02.v.x, Controle02.v.y);
}
else
{
/*
SP->send(0, 0, 0);
SP->send(1, 0, 0);
SP->send(2, 0, 0);
*/
}
commF = clock();
// Debug em prompt de comando
debugI = clock();
//system("cls");
//cout << "Roxo: " << Estrategia.players[0].x << ", " << Estrategia.players[0].y << " --> " << Estrategia.players[0].goal_x << ", " << Estrategia.players[0].goal_y << endl;
//cout << "Verde: " << Estrategia.players[1].x << ", " << Estrategia.players[1].y << " --> " << Estrategia.players[1].goal_x << ", " << Estrategia.players[1].goal_y << endl;
//cout << "Marrom: " << Estrategia.players[2].x << ", " << Estrategia.players[2].y << " --> " << Estrategia.players[2].goal_x << ", " << Estrategia.players[2].goal_y << endl;
//cout << "Carrinho: (" << Camera.players[0].x << ", " << Camera.players[0].y << ", " << Camera.players[0].theta *(180/M_PI) << ")" << endl;
//cout << "Objetivo: (" << (int) Estrategia.ponto_f.x << ", " << (int) Estrategia.ponto_f.y << ")" << endl;
//cout << "Velocidade (d, e): " << Controle00.v.x << ", "<< Controle00.v.y << ")" <<endl;
// Debug em janela
cvSetMouseCallback("Live", mouseEvent, 0);
// Desenhando nas janelas
// cv::resize(Estrategia.fieldI[1], pesos, cv::Size(Camera.frame->width, Camera.frame->height), 0, 0);
// cvReleaseImage(&pesos3);
pesos3 = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, Camera.frame->nChannels);
cvMerge(&(IplImage)pesos, &(IplImage)pesos, &(IplImage)pesos, NULL, pesos3);
//cvAddWeighted(pesos3, 0.9, Camera.frame, 0.8, 80, Camera.frame);
for (int i = 0; i < 7; i++)
{
if (Camera.players[i].x != -1)
{
cvCircle(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), radius, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
cvLine(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), cvPoint((int)Camera.players[i].x + radius*cos(Camera.players[i].theta),
(int)Camera.players[i].y + radius*sin(Camera.players[i].theta)), CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
}
}
cout << "nxd: " << xd << " nyd: " << yd << endl;
cvCircle(Camera.frame, cvPoint(xd + 155, -yd + 116), 5, CV_RGB(0, 255, 0), -1);
cvShowImage("Live", Camera.frame);
//Escape Sequence
char_pressed = cvWaitKey(1);
if (char_pressed == 27) //esc
{
temp[0] = 0x80;
temp[1] = 0x80;
SP->WriteData(temp, 2);
SP->sendApi(0, 0, 0, 0, 0);
SP->sendApi(0, 0, 0, 0, 0);
SP->sendApi(0, 0, 0, 0, 0);
break;
}
if (char_pressed == 112) //p
{
if (paused)
paused = false;
else{
paused = true;
flag = 1.0;
//thetad = alpha;
}
}
if (char_pressed == 99) //c
{
whishPath.clear();
followedPath.clear();
}
if (char_pressed == 105) //i
{
//e += 1;
}
if (char_pressed == 111) //o
{
//e -= 1;
}
if (char_pressed == 117) //u
{
wc = 0.0;
temp[0] = 0x80;
temp[1] = 0x80;
SP->WriteData(temp, 2);
}
if (char_pressed == 107) //k
{
R += 1;
}
if (char_pressed == 108) //l
{
R -= 1;
}
if (char_pressed == 113) //q
{
f += 5;
}
if (char_pressed == 119) //w
{
f -= 5;
}
if (char_pressed == 109) // m
{
xd += 10;
}
if (char_pressed == 98) // b
{
xd -= 10;
}
if (char_pressed == 104) // h
{
yd += 10;
}
if (char_pressed == 110) // n
{
yd -= 10;
}
debugF = clock();
totalF = clock();
//system("cls");
//if (paused)
// cout << "Paused" << endl;
//else
system("cls");
//cout << "Running" << endl;
//cout << "Tempo de analise de imagem: " << (visionF - visionI) << endl;
//cout << "Tempo da Estrategia: " << estraF - estraI << endl;
//cout << "Tempo do Controle: " << controlF - controlI << endl;
//cout << "posição: " << camera.players[0].x << " , " << camera.players[0].y << endl;
//cout << "Tempo da Comunicacao: " << commF - commI << endl;
//cout << "Tempo do Debug:" << debugF - debugI<< endl;
//cout << "Tempo Total: " << (totalF - totalI) << endl;
//cout << "Frame Rate: " << 1000 * counter/ (totalF - start) << endl;
//cout << "Frame Rate Instantanea: " << 1000 / (totalF - visionI) << endl;
//cout << "F1: " << f1 << endl;
//cout << "F2: " << f2 << endl;
//cout << counter << endl;
//cout << "tempread1 = " << (int)tempread[1] << endl;
//cout << "tempread2 = " << (int)tempread[0] << endl;
//cout << "temp = " << (int)temp[1] << endl;
//cout << "temp = " << (int)temp[0] << endl;
//cout << "PERÍODO = " << T << endl;
//cout << "Soma: " << (int)sum << endl;
//Sleep(100);
}
}
int main()
{
FILE * xFile;
xFile = fopen("motor1x.csv", "w");
FILE * yFile;
yFile = fopen("motor1y.csv", "w");
FILE * thetaFile;
thetaFile = fopen("motor1theta.csv", "w");
FILE * gFile;
xFile = fopen("geral.csv", "w");
cv::Mat pesos;
IplImage* pesos3;
pesos3 = cvCreateImage(cvSize(1, 1), 8, 3);
// Config Debug
clock_t visionI, visionF, estraI, estraF, controlI, controlF, commI, commF, totalI, totalF, debugI, debugF, start;
vector<robot> whishPath, followedPath;
int p = 0;
float counter = 0;
char char_pressed;
int time = 1;
int side = 1;
bool paused = true;
// Config Vision
CvRect ROI = cvRect(25, 49, 295, 184);
robot canto_area1 = robot(49, 35);
robot canto_area2 = robot(244, 146);
robot canto_gol1 = robot(23, 59);
robot canto_gol2 = robot(276, 126);
int amin = 23;
int amax = 173;
int radius = 20;
int min_distance = 12;
int parametro[36] = { 88, 139, 80, 121, 253, 255,
21, 62, 114, 60, 243, 255,
146, 98, 164, 180, 162, 255,
55, 189, 91, 94, 229, 255,
3, 102, 83, 17, 199, 229,
0, 85, 135, 21, 255, 255 };
//CvRect ROI = cvRect(36, 46, 266, 162);
//robot canto_area1 = robot(65, 67);
//robot canto_area2 = robot(244, 167);
//robot canto_gol1 = robot(43, 66);
//robot canto_gol2 = robot(282, 173);
//int amin = 20;
//int amax = 250;
//int radius = 20;
//int min_distance = 50;
//int parametro[36] = { 88, 139, 80, 121, 253, 255,
// 22, 59, 141, 55, 204, 255,
// 117, 58, 113, 141, 113, 255,
// 52, 82, 89, 97, 168, 169,
// 0, 79, 123, 19, 149, 191,
// 0, 129, 182, 18, 251, 255 };
int debug_colors[7][3] = { { 128, 0, 128 }, // Roxo
{ 0, 64, 0 }, // Verde
{ 117, 58, 0 }, // Marrom
{ 255, 255, 0 }, // Amarelo
{ 255, 255, 0 }, // Amarelo
{ 255, 255, 0 }, // Amarelo
{ 255, 128, 0 } }; // Laranja
if (time == 1)
{
for (int k = 0; k < 3; k++)
{
debug_colors[3 + k][0] = 0; // Azul
debug_colors[3 + k][1] = 0; // Azul
debug_colors[3 + k][2] = 255; // Azul
}
}
// Config Controle
// Parametros do PID na seguinte ordem: PF, IF, DF, PT, IT, DT, PFR, IFR, DFR, PTR, ITR, DTR
float param0[12] = { 11.45, 1.49, 26.9, 13.59, 1.63, 25.86, 1.03, 0.14, 9.36, 1.4, 0.23, 9.44 };
float param1[12] = { 25.72, 1.01, 20.07, 22.34, 0.94, 21.1, 10, 0.64, 4.92, 10, 0.61, 6.92 };
float param2[12] = { 21.86, 1.06, 18, 22, 1.03, 25.03, 3.59, 0.32, 4.94, 2.9, 0.89, 3.79 };
CvFont fonte;
cvInitFont(&fonte, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, 8);
cvNamedWindow("Live", CV_WINDOW_AUTOSIZE);
//cvNamedWindow("Filtro Atual");
//cvNamedWindow("Estrategia");
// Inicialização da Visão
vision Camera;
Camera.createCamera(1);
Camera.getImage(ROI);
Camera.configParam(parametro, amin, amax, time, min_distance, ROI);
// Inicialização da Estratégia
estrategia Estrategia(Camera.h, Camera.w, side, canto_gol1, canto_gol2, canto_area1, canto_area2);
Estrategia.Config(Camera.h, Camera.w);
pesos = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, 1);
Estrategia.ConfigAreas(canto_gol1, canto_gol2, canto_area1, canto_area2);
// Inicialização do Controle
//controle Controle00((float)0.2, (float)0.007, (float)6, (float)0.3, (float)0.000001, (float)4);
controle Controle00(param0);
controle Controle01(param1);
controle Controle02(param2);
Controle00.distance_lim = 50;
Controle01.distance_lim = 50;
Controle02.distance_lim = 50;
// Inicialização do Serial
Serial* SP = new Serial("COM3");
if (SP->IsConnected())
printf("We're connected\n");
start = clock();
while (1)
{
counter++;
totalI = clock();
// =========================== VISÃO =========================== //
visionI = clock();
Camera.getImage();
Camera.dataAcquire();
Camera.players[1] = robot(-1, -1);
Camera.players[2] = robot(-1, -1);
visionF = clock();
//if (Camera.players[0].x != -1)
// fprintf(xFile, "%f, %i\n", Camera.players[0].x * (184 / ROI.width), int(visionF - start));
//if (Camera.players[0].y != -1)
// fprintf(yFile, "%f, %i\n", Camera.players[0].y * (95 / ROI.height), int(visionF - start));
//if (Camera.players[0].x != -1)
// fprintf(thetaFile, "%f, %i\n", Camera.players[0].theta, int(visionF - start));
//if (Camera.players[0].x != -1)
// fprintf(gFile, "%f,%f,%f %i\n", Camera.players[0].x * (184 / ROI.width), Camera.players[0].y * (95 / ROI.height), Camera.players[0].theta, int(visionF - start));
// =========================== ESTRATEGIA =========================== //
estraI = clock();
//Atribuição de comportamentos
Estrategia.update(Camera.players);
Estrategia.comportamento3();
Estrategia.Posse();
//Estrategia.players[0].comportamento = 3;
//Estrategia.players[1].comportamento = 3;
//Estrategia.players[2].comportamento = 2;
// Atribuição de objetivos
Estrategia.goleiro4();
Estrategia.lateral();
Estrategia.atacante2();
Estrategia.zagueiro();
Estrategia.generalRouting();
//Estrategia.players[0].setGoal(23, 98);
//Estrategia.players[1].setGoal(23, 98);
estraF = clock();
// =========================== CONTROLE =========================== //
controlI = clock();
Controle00.controla(Camera.players[0], robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y), robot(Estrategia.players[0].objective_x, Estrategia.players[0].objective_y));
Controle01.controla(Camera.players[1], robot(Estrategia.players[1].goal_x, Estrategia.players[1].goal_y), robot(Estrategia.players[1].objective_x, Estrategia.players[1].objective_y));
Controle02.controla(Camera.players[2], robot(Estrategia.players[2].goal_x, Estrategia.players[2].goal_y), robot(Estrategia.players[2].objective_x, Estrategia.players[2].objective_y));
controlF = clock();
// =========================== COMUNICAÇÃO =========================== //
commI = clock();
if (!paused)
{
//SP->send(0, 0, 15);
SP->send(0, Controle00.v.x, Controle00.v.y);
whishPath.push_back(robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y));
followedPath.push_back(Camera.players[0]);
//SP->send(1, Controle01.v.x, Controle01.v.y);
//SP->send(2, Controle02.v.x, Controle02.v.y);
}
else
{
SP->send(0, 0, 0);
SP->send(1, 0, 0);
SP->send(2, 0, 0);
}
commF = clock();
// Debug em prompt de comando
debugI = clock();
//system("cls");
//cout << "Roxo: " << Estrategia.players[0].x << ", " << Estrategia.players[0].y << " --> " << Estrategia.players[0].goal_x << ", " << Estrategia.players[0].goal_y << endl;
//cout << "Verde: " << Estrategia.players[1].x << ", " << Estrategia.players[1].y << " --> " << Estrategia.players[1].goal_x << ", " << Estrategia.players[1].goal_y << endl;
//cout << "Marrom: " << Estrategia.players[2].x << ", " << Estrategia.players[2].y << " --> " << Estrategia.players[2].goal_x << ", " << Estrategia.players[2].goal_y << endl;
//cout << "Carrinho: (" << Camera.players[0].x << ", " << Camera.players[0].y << ", " << Camera.players[0].theta *(180/M_PI) << ")" << endl;
//cout << "Objetivo: (" << (int) Estrategia.ponto_f.x << ", " << (int) Estrategia.ponto_f.y << ")" << endl;
//cout << "Velocidade (d, e): " << Controle00.v.x << ", "<< Controle00.v.y << ")" <<endl;
// Debug em janela
cvSetMouseCallback("Live", mouseEvent, 0);
// Desenhando nas janelas
cv::resize(Estrategia.fieldI[1], pesos, cv::Size(Camera.frame->width, Camera.frame->height), 0, 0);
cvReleaseImage(&pesos3);
pesos3 = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, Camera.frame->nChannels);
cvMerge(&(IplImage)pesos, &(IplImage)pesos, &(IplImage)pesos, NULL, pesos3);
cvAddWeighted(pesos3, 0.9, Camera.frame, 0.8, 80, Camera.frame);
for (int i = 0; i < 7; i++)
{
if (Camera.players[i].x != -1)
{
cvCircle(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), radius, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
cvLine(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), cvPoint((int)Camera.players[i].x + radius*cos(Camera.players[i].theta),
(int)Camera.players[i].y + radius*sin(Camera.players[i].theta)), CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
}
}
for (int i = 0; i < 3; i++)
{
if (Estrategia.players[i].x != -1)
cvLine(Camera.frame, cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), cvPoint((int)Estrategia.players[i].goal_x, (int)Estrategia.players[i].goal_y), CV_RGB(255, 255, 255), 3);
}
for (int n = 0; n < 3; n++)
{
if (Estrategia.path[n].size() > 1)
{
for (int i = 0; i < (Estrategia.path[n].size() - 1); i++)
{
//cout << "( " << Estrategia.path[n][i].x << ", " << Estrategia.path[n][i].y << " ) ; " << endl;
cvLine(Camera.frame, cvPoint((int)Estrategia.path[n][i].x, (int)Estrategia.path[n][i].y), cvPoint((int)Estrategia.path[n][i + 1].x, (int)Estrategia.path[n][i + 1].y), CV_RGB(debug_colors[n][0], debug_colors[n][1], debug_colors[n][2]), 2);
}
}
}
for (int i = 0; i < 3; i++)
{
if (Estrategia.players[i].x != -1)
{
int comportamento = Estrategia.players[i].comportamento;
switch (comportamento)
{
case(1) :
cvPutText(Camera.frame, "G", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
//case(2) :
// cvPutText(Camera.frame, "A", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
//case(3) :
// cvPutText(Camera.frame, "L", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
//case(4) :
// cvPutText(Camera.frame, "Z", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
}
}
}
if (whishPath.size() > 1)
{
for (int i = 0; i < (whishPath.size() - 1); i++)
{
//cout << "( " << Estrategia.path[n][i].x << ", " << Estrategia.path[n][i].y << " ) ; " << endl;
cvLine(Camera.frame, cvPoint((int)whishPath[i].x, (int)whishPath[i].y), cvPoint((int)whishPath[i + 1].x, (int)whishPath[i + 1].y), CV_RGB(221, 0, 0, 2)); //Vermelho
cvLine(Camera.frame, cvPoint((int)followedPath[i].x, (int)followedPath[i].y), cvPoint((int)followedPath[i + 1].x, (int)followedPath[i + 1].y), CV_RGB(94, 83, 138, 2)); //Roxo
}
}
cvShowImage("Live", Camera.frame);
//Escape Sequence
char_pressed = cvWaitKey(1);
if (char_pressed == 27)
{
SP->send(0, 0, 0);
SP->send(1, 0, 0);
SP->send(2, 0, 0);
break;
}
if (char_pressed == 112)
{
if (paused)
paused = false;
else
paused = true;
}
if (char_pressed == 99)
{
whishPath.clear();
followedPath.clear();
}
debugF = clock();
totalF = clock();
system("cls");
//if (paused)
// cout << "Paused" << endl;
//else
cout << "Running" << endl;
cout << "Tempo de analise de imagem: " << (visionF - visionI) << endl;
cout << "Tempo da Estrategia: " << estraF - estraI << endl;
cout << "Tempo do Controle: " << controlF - controlI << endl;
cout << "Tempo da Comunicacao: " << commF - commI << endl;
cout << "Tempo do Debug:" << debugF - debugI<< endl;
cout << "Tempo Total: " << (totalF - totalI) << endl;
cout << "Frame Rate: " << 1000 * counter/ (totalF - start) << endl;
//cout << "posicao: "<< camera.players[0].x << "," << camera.players[0].y <<endl;
//cout << counter << endl;
//system("cls");
}
}
// application reads from the specified serial port and reports the collected data
int _tmain(int argc, _TCHAR* argv[])
{
// connect the COM
printf("Welcome to the serial test app!\n\n");
#if defined (_MSC_VER)
Serial* SP = new Serial("\\\\.\\COM6"); // adjust as needed
#elif defined (__linux__)
string PORTNAMEIN;
cout << "Input Serial Port (e.g. /dev/ttyACM0): " << endl;
cin >> PORTNAMEIN;
char *PORTNAME = new char [PORTNAMEIN.length() + 1];
std::strcpy(PORTNAME, PORTNAMEIN.c_str());
Serial* SP = new Serial(PORTNAME);
unsigned int JOYSTICKID1;
#endif
if (SP->IsConnected())
printf("We're connected!\n");
//data communication
char incomingData[256] = ""; // don't forget to pre-
int dataLengthin = 127;
int readResult = 0;
int steer=2048;
int breaking = 255;
const int encoder_resolution=4096;
char outSteer[6]="2048s";
int driveDutycycle=0;
const int fullDutycycle=255;
char outDrive[5]="001d";
char outBreak[5]="100b";
//joystick initialize***********************
joyinfoex_tag joyinfo;
#ifdef __linux__
if (SDL_Init(SDL_INIT_JOYSTICK) < 0){
cout << "Error initializing SDL!" << endl;
return 1;
}
SDL_Joystick *joy;
int CtrlNum = SDL_NumJoysticks();
if (CtrlNum == 1)
JOYSTICKID1 = 0;
else{
cout << "There are " << CtrlNum << " controllers found..." << endl;
for(int i=0;i<CtrlNum;i++)
{
joy = SDL_JoystickOpen(i);
printf("%s\n", SDL_JoystickName(joy));
}
cout << "Choose the one you wish to use: " << endl;
cin >> JOYSTICKID1;
}
joy=SDL_JoystickOpen(JOYSTICKID1);
if (joy) {
printf("Opened Joystick %d\n",JOYSTICKID1);
printf("Name: %s\n", SDL_JoystickNameForIndex(0));
printf("Number of Axes: %d\n", SDL_JoystickNumAxes(joy));
printf("Number of Buttons: %d\n", SDL_JoystickNumButtons(joy));
printf("Number of Balls: %d\n", SDL_JoystickNumBalls(joy));
} else {
printf("Couldn't open Joystick %d\n", JOYSTICKID1);
return -1*JOYSTICKID1;
}
#endif
//button status
bool is_in_situ=false;
bool is_any_direction=false;
bool is_tradition=true;
bool now_in_situ=false;
bool now_any_direction=false;
bool now_tradition=false;
bool last_in_situ=false;
bool last_any_direction=false;
bool last_tradition=false;
//int count = 0;
//start control
bool action = 1;
#if defined(_MSC_VER)
while(SP->IsConnected()) // CYCLES HERE...
{
//acquire joystick info **********************
joyGetPosEx(JOYSTICKID1, &joyinfo);
#elif defined(__linux__)
SDL_Event SysEvent;
bool NoQuit = true;
while(SP->IsConnected() && NoQuit)
{
if (SDL_PollEvent(&SysEvent))
{
switch (SysEvent.type)
{
case SDL_JOYAXISMOTION:
{
joyinfo.dwXpos = SDL_JoystickGetAxis(joy,0) + 32767 + 1 ;
joyinfo.dwYpos = SDL_JoystickGetAxis(joy,1) + 32767 + 1 ;
joyinfo.dwZpos = SDL_JoystickGetAxis(joy,2) + 32767 + 1 ;
joyinfo.dwRpos = SDL_JoystickGetAxis(joy,3) + 32767 + 1 ;
//Only 4 axis.
//joyinfo.dwUpos = SDL_JoystickGetAxis(joy,4);
//joyinfo.dwVpos = SDL_JoystickGetAxis(joy,5);
break;
}
case SDL_QUIT:
{
NoQuit = false;
break;
}
}
SDL_FlushEvents(SDL_APP_TERMINATING, SDL_LASTEVENT); //Flush Old Events
#endif
cout << "X Steering Wheel:" << joyinfo.dwXpos << endl;
if(joyinfo.dwZpos != 32767)
action = 0;
if(action)
{
cout << "Z Throttle:" << 65536 << endl; //The original value of the inactivated throttle is 32767, should be modified to 65535 so that throttle is 0
cout << "R Break:" << 0.9*65536 << endl;
}
else
{
cout << "Z Throttle:" << joyinfo.dwZpos << endl; //If activated (moved), output real value.
cout << "R Break:" << joyinfo.dwRpos << endl; //The same for breaks.
}
cout << "buttonNumber" << joyinfo.dwButtonNumber << endl;
cout << "buttonStatus" << bitset<64>(joyinfo.dwButtons) << endl; //Output button status
//Buttons:
now_in_situ=bitset<64>(joyinfo.dwButtons)[0];
now_any_direction=bitset<64>(joyinfo.dwButtons)[3];
now_tradition=bitset<64>(joyinfo.dwButtons)[1];
cout << "is_in_situ:" << is_in_situ << endl;
cout << "is_any_direction:" << is_any_direction << endl;
cout << "is_tradition:" << is_tradition << endl;
//PostProcessing********************************************************
if(now_in_situ== true && last_in_situ ==false)
{
is_in_situ =true;
is_any_direction =false;
is_tradition =false;
SP->WriteData("b",1);
}
if(now_any_direction== true && last_any_direction ==false)
{
is_in_situ =false;
is_any_direction =true;
is_tradition =false;
SP->WriteData("f",1);
}
if(now_tradition== true && last_tradition ==false)
{
is_in_situ =false;
is_any_direction =false;
is_tradition =true;
SP->WriteData("f",1);
}
last_in_situ =now_in_situ;
last_any_direction =now_any_direction;
last_tradition =now_tradition;
if(action)//Double-check that the inactivated throttle value is set to 0.
{
driveDutycycle = 0;
breaking = 225;
}
else
{
breaking = (int)((1-joyinfo.dwRpos/65535.0)*fullDutycycle); //Modify as necessary.
if (breaking=0)
driveDutycycle = (int)((1-joyinfo.dwZpos/65535.0)*fullDutycycle);
else
driveDutycycle = 0;
}
if(driveDutycycle>=0 && driveDutycycle<=255)
{
for (int k=0;k<3;k++)
{
outDrive[2-k]=char(driveDutycycle%10 + '0');
driveDutycycle=driveDutycycle/10;
}
}
if(breaking>=0 && breaking<=255)
{
for (int k=0;k<3;k++)
{
outBreak[2-k]=char(breaking%10 + '0');
breaking=breaking/10;
}
}
steer=(int)(joyinfo.dwXpos/65535.0*encoder_resolution);
if(steer>=0 && steer<=encoder_resolution)
{
for (int k=0;k<4;k++)
{
outSteer[3-k]=char(steer%10 + '0');
steer=steer/10;
}
}
//Writing to Arduino Seriall**************************************************
printf("outDrive:%s\n",outDrive);
bool isWriteDrive = SP->WriteData((char*)&outDrive, strlen(outDrive));
cout << "WriteSucceed?" << isWriteDrive << endl;
printf("outSteer:%s\n",outSteer);
bool isWriteSteer = SP->WriteData((char*)&outSteer, strlen(outSteer));
cout << "WriteSucceed?" << isWriteSteer << endl;
printf("outBreak:%s\n",outBreak);
bool isWriteBreak = SP->WriteData((char*)&outBreak, strlen(outBreak));
cout << "WriteSucceed?" << isWriteBreak << endl;
#if defined(_MSC_VER)
Sleep(120); //time in ms
system("cls");
#elif defined (__linux__)
} //If have sysevent then update joystick values
usleep(120000); //time in us
system("clear");
#endif
//Read back from Arduino
readResult = SP->ReadData(incomingData,dataLengthin);
///*if(readResult<2*sizeof(serial_format)){
// printf("No enough data received. Let's wait.\n");
// Sleep(1500);continue;
//}*/
int ptr=0;
while(!(incomingData[ptr]=='?' && incomingData[ptr+1]=='?') && ptr<readResult-sizeof(serial_format)) {
ptr++;
}
serial_format* ptr_to_first_valid = (serial_format*) &incomingData[ptr];
printf("Actual Steer: %d\n",ptr_to_first_valid->Steer);
printf("Actual Drive: %d\n",ptr_to_first_valid->Drive);
printf("Bus Voltage: %f\n", ptr_to_first_valid->Voltage);
printf("Current on Driving: %f\n", ptr_to_first_valid->CurrentD);
printf("Current on Steering: %f\n", ptr_to_first_valid->CurrentS);
printf("Power Consumed on This Unit: %f\n", (ptr_to_first_valid->Voltage)*((ptr_to_first_valid->CurrentD)+(ptr_to_first_valid->CurrentS)));
//However always read serial data.
#ifdef __linux__
int errNum = FFupdate(joy,ptr_to_first_valid->Steer);
#endif
}
SP->~Serial();
#ifdef __linux__
SDL_JoystickClose(joy);
#endif
system("pause");
return 0;
}
int main( int argc, char** argv )
{
VideoCapture cap(0);
if ( !cap.isOpened() )
{
cout << "Cannot open the web cam" << endl;
return -1;
}
Serial* SP = new Serial("COM7");
if (SP->IsConnected())
printf("We're connected");
char outgoingData[1] = "";
int dataLength = 1;
int readResult = 0;
while(SP->IsConnected())
{
namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"
int iLowH = 0;
int iHighH = 30;
int iLowS = 150;
int iHighS = 255;
int iLowV = 142;
int iHighV = 255;
createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
createTrackbar("HighH", "Control", &iHighH, 179);
createTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
createTrackbar("HighS", "Control", &iHighS, 255);
createTrackbar("LowV", "Control", &iLowV, 255);//Value (0 - 255)
createTrackbar("HighV", "Control", &iHighV, 255);
int iLastX = 320;
int iLastY = 240;
Mat imgTmp;
cap.read(imgTmp);
Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );;
while (true)
{
Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal);
if (!bSuccess)
{
cout << "Cannot read a frame from video stream" << endl;
break;
}
Mat imgHSV;
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV
Mat imgThresholded;
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image
//morphological opening (removes small objects from the foreground)
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//morphological closing (removes small holes from the foreground)
dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
//Calculate the moments of the thresholded image
Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01;
double dM10 = oMoments.m10;
double dArea = oMoments.m00;
// if the area <= 10000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
if (dArea > 10000)
{
//calculate the position of the ball
int posX = dM10 / dArea;
int posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
{
//Draw a red line from the previous point to the current point
// line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2);
Point pt1(posX-10, posY-10);
Point pt2(posX+10, posY+10);
rectangle(imgLines, pt1, pt2, cvScalar(0, 255, 0, 0), 1, 8, 0);
}
iLastX = posX;
iLastY = posY;
}
Point pt3(100,100);
Point pt4(520,380);
rectangle(imgLines, pt3, pt4, cvScalar(0, 0, 255, 0), 1, 8, 0);
if(iLastX-10 < 100)
{
//putText(image, "Your face is on the right side of me!", Point(faces[i].x, faces[i].y - 5), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
putText(imgLines, "The object is on the right side of me!", Point(70,90), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
outgoingData[0] = 'r';
if(SP->WriteData(outgoingData, dataLength))
Sleep(100);
}
if(iLastX+10 > 520)
{
putText(imgLines, "The object is on the left side of me!", Point(70,90), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
outgoingData[0] = 'l';
if(SP->WriteData(outgoingData, dataLength))
Sleep(100);
}
if(iLastY-10 > 380)
{
putText(imgLines, "The object is at the bottom!", Point(70,70), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
outgoingData[0] = 'b';
if(SP->WriteData(outgoingData, dataLength))
Sleep(100);
}
if(iLastY+10 < 100)
{
putText(imgLines, "The object is at the top!", Point(70,70), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
outgoingData[0] = 't';
if(SP->WriteData(outgoingData, dataLength))
Sleep(100);
}
imshow("Thresholded Image", imgThresholded); //show the thresholded image
imgOriginal = imgOriginal + imgLines;
imshow("Original", imgOriginal); //show the original image
imgLines=cv::Scalar(0, 0, 0);
if (cvWaitKey(33) == 27) //wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
{
cout << "esc key is pressed by user" << endl;
return -1;
}
}
}
return 0;
}