// 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; }
void ctl_Close() { mot_Close(); att_Close(); }