bool initialise(FlightBoard *fb, GPS *gps, IMU *imu, CAMERA_STREAM *cam) {
cout << "\033[36m[COPTER]\033[0m Initialising." << endl;
/* Initialise WiringPi */
gpio::startWiringPi();
/* Initialise Flight Board */
if(fb->setup() != FB_OK) {
cout << "\033[1;31m[COPTER]\033[0m Error setting up flight board. Terminating program" << endl;
return false;
}
fb->start();
/* Initialise GPS */
if(gps->setup() != GPS_OK) {
cout << "\033[1;31m[COPTER]\033[0m Error setting up GPS. Will retry continuously." << endl;
while (gps->setup() != GPS_OK) usleep(1000000);
}
gps->start();
cout << "\033[36m[COPTER]\033[0m GPS detected." << endl;
/* Initialise IMU
if(imu->setup() != IMU_OK) {
cout << "\033[1;31m[COPTER]\033[0m Error setting up IMU. Will retry continuously." << endl;
while (imu->setup() != IMU_OK) usleep(1000);
}
imu->start();
cout << "\033[36m[COPTER]\033[0m IMU detected." << endl;*/
/* Initialise Camera */
if (cam->setup() != CAMERA_OK) {
cout << "\033[1;31m[COPTER]\033[0m Error setting up camera. Will retry continuously." << endl;
while (cam->setup() != CAMERA_OK) usleep(1000000);
}
cam->setMode(3);
cam->start();
return true;
}
int main(int argc, char* argv[]) {
cout << "Starting program" << endl;
//Signal to exit program.
struct sigaction signalHandler;
signalHandler.sa_handler = terminate;
sigemptyset(&signalHandler.sa_mask);
signalHandler.sa_flags = 0;
sigaction(SIGTERM, &signalHandler, NULL);
sigaction(SIGINT, &signalHandler, NULL);
//Setup parameters
ConfigParser::ParamMap parameters;
parameters.insert("TOL_rotate", &TOL_rotate);
parameters.insert("KP_rotate", &KP_rotate);
parameters.insert("SPIN_SPEED", &SPIN_SPEED);
parameters.insert("MAIN_LOOP_SLEEP", &MAIN_LOOP_SLEEP);
ConfigParser::loadParameters(TABLE_NAME, ¶meters, PARAMETER_FILE);
//Startup sensors
gpio::startWiringPi();
FlightBoard fb = FlightBoard();
if(fb.setup() != FB_OK) {
cout << "Error setting up flight board." << endl;
return -1;
}
fb.start();
FB_Data course = {0, 0, 0, 0};
FB_Data stop = {0, 0, 0, 0};
CAMERA_VAR3 cam = CAMERA_VAR3();
if(cam.setup(PARAMETER_FILE) != CAMERA_OK) {
cout << "Error setting up camera." << endl;
fb.stop();
return -1;
}
cam.start();
ObjectLocation red_object;
int state = 0; //State machine implemetation
//State 0; Manual mode
//State 1; Object detected, face object + gimbal
//State 2; No object detected -> wait there
usleep(700); //wait for laggy camera stream
//Setup curses
initscr();
start_color();
init_pair(1, COLOR_GREEN, COLOR_BLACK);
init_pair(2, COLOR_CYAN, COLOR_BLACK);
refresh();
int LINES, COLUMNS;
getmaxyx(stdscr, LINES, COLUMNS);
//Set up title window
int TITLE_HEIGHT = 4;
WINDOW *title_window = newwin(TITLE_HEIGHT, COLUMNS -1, 0, 0);
wattron(title_window, COLOR_PAIR(1));
wborder(title_window, ' ' , ' ' , '-', '-' , '-', '-', '-', '-');
wmove(title_window, 1, 0);
wprintw(title_window, "\t%s\t\n", "FACE_RED_OBJECT");
wprintw(title_window, "\t%s\t\n", "Hexacopter will rotate on spot to look at red object.");
wrefresh(title_window);
//Set up messages window
WINDOW *msg_window = newwin(LINES - TITLE_HEIGHT -1, COLUMNS -1, TITLE_HEIGHT, 0);
wattron(msg_window, COLOR_PAIR(2));
usleep(1000);
while(!exitProgram) {
if(!gpio::isAutoMode()) { //If not in autonomous mode
state = 0;
} else if(cam.objectDetected()) { //If object detected
state = 1;
} else {
state = 2; //No object found, wait for it.
}
/*
cout << "State:\t" << state << endl;
cout << "Framerate:\t" << cam.getFramerate() << endl;
*/
wclear(msg_window);
wprintw(msg_window, "\n");
wprintw(msg_window, "State:\t%d\n", state);
wprintw(msg_window, "Framerate:\t%1.4f\n", cam.getFramerate());
wprintw(msg_window, "\n");
switch(state) {
case 0: //Case 0: Not in auto mode, standby
fb.setFB_Data(&stop); //Stop moving
/*
printFB_Data(&stop);
cout << "In manual mode, standby" << endl; //PRINT SOMETHING
*/
wprintw(msg_window, "In manual mode, standby\n");
wprintw(msg_window, "%s", (stringFB_Data(&stop)).c_str());
break;
case 1: //Case 1: Object detected, face and point gimbal at it.
cam.getObjectLocation(&red_object); //GET OBJECT LOCATION
setCourse_faceObject(&course, &red_object); //P ON OBJECT
setCourse_moveGimbal(&course, &red_object);
fb.setFB_Data(&course);
/*
printFB_Data(&course);
*/
wprintw(msg_window, "Red object detected\n");
wprintw(msg_window, "%s", (stringFB_Data(&course)).c_str());
break;
case 2: //Case 2 wait there.
default:
setCourse_stopTurning(&course);
fb.setFB_Data(&course);
/*
printFB_Data(&course); //STOP
cout << "No object." << endl; //PRINT: NO OBJECT FOUND.
*/
wprintw(msg_window, "No red objects detected\n");
wprintw(msg_window, "%s", (stringFB_Data(&course)).c_str());
break;
}
wrefresh(msg_window);
boost::this_thread::sleep(boost::posix_time::milliseconds(MAIN_LOOP_SLEEP));
}
endwin();
fb.stop();
cam.stop();
cam.close();
return 0;
}
