int main()
{
int rc;
struct nav_struct nav;
printf("Nav board test program\r\n");
//nav board
printf("Init Navboard ...\r\n");
rc = nav_Init(&nav);
if(rc) return rc;
printf("Init Navboard OK\r\n");
//calibrate
printf("Calibration ...\r\n");
rc=nav_FlatTrim();
if(rc) {printf("FlatTrim Failed: retcode=%d\r\n",rc); return rc;}
printf("Calibration OK\r\n");
//main loop
while(1) {
//get sample
rc = nav_GetSample(&nav);
if(rc) {
printf("ERROR: nav_GetSample return code=%d\n",rc);
}
nav_Print(&nav);
}
nav_Close();
printf("\nDone...\n");
return 0;
}
// Begin control algorithm main method
int main()
{
printf("Start of Control\r\n");
mot_Init();
// Declare variables for navdata
int rc;
nav_struct nav;
// Initialize navdata
rc = nav_Init(&nav);
// Check if navdata initializes
int nav_fail = 0;
if (rc==0) {
printf("navdata failed to initialize\n");
nav_fail = 1;
}
// Calibrate navdata
rc=nav_FlatTrim();
if(rc) {
printf("Failed navdata: retcode=%d\r\n",rc);
nav_fail = 1;
}
// Kick off value getting thing in a separate thread!
pthread_create(&image_processing_thread, NULL, process_images, NULL);
int angle = getAngle();
printf("Angle: %i\r\n",angle);
int dir = angle != 0 ? angle/abs(angle) : 0;
/*
//first pulse
pulse(dir,pulseDuration);
printf("Wait: %f\r\n",wait(angle)*1000000);
for(int i=0; i<100; i++){
usleep(wait(angle)/100*1000000);
checkKeypress();
}
pulse(-dir,pulseDuration -.04);
*/
prevAngle = angle;
// start timer
gettimeofday(&t1, NULL);
// PID Loop
float s = .01;
dir= 1;
while(1) {
checkKeypress();
if(waitToStart) continue;
if(stopLoop) break;
// Check navdata if navdata initiates
if (nav_fail == 0) {
checkNavdata(&nav);
}
pid_controller();
/* smallPulse(dir,.9);
usleep(s * 1000000);
smallPulse(dir,.9);
usleep(1.5 * 1000000);
printf("%f\n",s);
if(dir > 0) dir = -1; else dir = 1;
s+=.01;
smallPulse(dir,.9);
usleep(s * 1000000);
smallPulse(dir,.9);
usleep(1.5 * 1000000);
printf("%f\n",s);
s+=.01;
*/
//yield to other threads
pthread_yield();
}
// Cleanup
// Delete the mutex
pthread_mutex_destroy(&video_results_mutex);
//close(sockfd);
//close(newsockfd); // Close TCP socket
//video_Close(&vid); // Close video thread
mot_Close(); // Close motor thread
printf("\nDone!\n");
return 0;
}
