int main(){
initialize_cape();
printf("\nPress mode to change blink rate\n");
printf("hold pause to exit\n");
//Assign your own functions to be called when events occur
set_pause_pressed_func(&on_pause_press);
set_pause_unpressed_func(&on_pause_release);
set_mode_pressed_func(&on_mode_press);
set_mode_unpressed_func(&on_mode_release);
// start in slow mode
mode = 0;
//toggle leds till the program state changes
while(get_state() != EXITING){
usleep(500000 - (mode*200000));
if(!paused && toggle){
setGRN(LOW);
setRED(HIGH);
toggle = 0;
}
else if(!paused && !toggle){
setGRN(HIGH);
setRED(LOW);
toggle=1;
}
}
cleanup_cape();
return 0;
}
/*********************************************************************************
* int initialize_button_interrups()
*
* start 4 threads to handle 4 interrupt routines for pressing and
* releasing the two buttons.
**********************************************************************************/
int initialize_button_handlers(){
#ifdef DEBUG
printf("setting up mode & pause gpio pins\n");
#endif
//set up mode pin
if(gpio_export(MODE_BTN)){
printf("can't export gpio %d \n", MODE_BTN);
return (-1);
}
gpio_set_dir(MODE_BTN, INPUT_PIN);
gpio_set_edge(MODE_BTN, "both"); // Can be rising, falling or both
//set up pause pin
if(gpio_export(PAUSE_BTN)){
printf("can't export gpio %d \n", PAUSE_BTN);
return (-1);
}
gpio_set_dir(PAUSE_BTN, INPUT_PIN);
gpio_set_edge(PAUSE_BTN, "both"); // Can be rising, falling or both
#ifdef DEBUG
printf("starting button handling threads\n");
#endif
struct sched_param params;
pthread_attr_t attr;
params.sched_priority = sched_get_priority_max(SCHED_FIFO)/2;
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
set_pause_pressed_func(&null_func);
set_pause_unpressed_func(&null_func);
set_mode_pressed_func(&null_func);
set_mode_unpressed_func(&null_func);
pthread_create(&pause_pressed_thread, &attr, \
pause_pressed_handler, (void*) NULL);
pthread_create(&pause_unpressed_thread, &attr, \
pause_unpressed_handler, (void*) NULL);
pthread_create(&mode_pressed_thread, &attr, \
mode_pressed_handler, (void*) NULL);
pthread_create(&mode_unpressed_thread, &attr, \
mode_unpressed_handler, (void*) NULL);
// apply medium priority to all threads
pthread_setschedparam(pause_pressed_thread, SCHED_FIFO, ¶ms);
pthread_setschedparam(pause_unpressed_thread, SCHED_FIFO, ¶ms);
pthread_setschedparam(mode_pressed_thread, SCHED_FIFO, ¶ms);
pthread_setschedparam(mode_unpressed_thread, SCHED_FIFO, ¶ms);
return 0;
}
int initialize_cape(){
FILE *fd; // opened and closed for each file
char path[128]; // buffer to store file path string
int i = 0; // general use counter
printf("\n");
// check if another project was using resources
// kill that process cleanly with sigint if so
fd = fopen(LOCKFILE, "r");
if (fd != NULL) {
int old_pid;
fscanf(fd,"%d", &old_pid);
if(old_pid != 0){
printf("warning, shutting down existing robotics project\n");
kill((pid_t)old_pid, SIGINT);
sleep(1);
}
// close and delete the old file
fclose(fd);
remove(LOCKFILE);
}
// create new lock file with process id
fd = fopen(LOCKFILE, "ab+");
if (fd < 0) {
printf("\n error opening LOCKFILE for writing\n");
return -1;
}
pid_t current_pid = getpid();
printf("Current Process ID: %d\n", (int)current_pid);
fprintf(fd,"%d",(int)current_pid);
fflush(fd);
fclose(fd);
// ensure gpios are exported
printf("Initializing GPIO\n");
for(i=0; i<NUM_OUT_PINS; i++){
if(gpio_export(out_gpio_pins[i])){
printf("failed to export gpio %d", out_gpio_pins[i]);
return -1;
};
gpio_set_dir(out_gpio_pins[i], OUTPUT_PIN);
}
// set up default values for some gpio
disable_motors();
deselect_spi1_slave(1);
deselect_spi1_slave(2);
//Set up PWM
printf("Initializing PWM\n");
i=0;
for(i=0; i<4; i++){
strcpy(path, pwm_files[i]);
strcat(path, "polarity");
fd = fopen(path, "a");
if(fd<0){
printf("PWM polarity not available in /sys/class/devices/ocp.3\n");
return -1;
}
//set correct polarity such that 'duty' is time spent HIGH
fprintf(fd,"%c",'0');
fflush(fd);
fclose(fd);
}
//leave duty cycle file open for future writes
for(i=0; i<4; i++){
strcpy(path, pwm_files[i]);
strcat(path, "duty");
pwm_duty_pointers[i] = fopen(path, "a");
}
//read in the pwm period defined in device tree overlay .dts
strcpy(path, pwm_files[0]);
strcat(path, "period");
fd = fopen(path, "r");
if(fd<0){
printf("PWM period not available in /sys/class/devices/ocp.3\n");
return -1;
}
fscanf(fd,"%i", &pwm_period_ns);
fclose(fd);
// mmap pwm modules to get fast access to eQep encoder position
// see mmap_eqep example program for more mmap and encoder info
printf("Initializing eQep Encoders\n");
int dev_mem;
if ((dev_mem = open("/dev/mem", O_RDWR | O_SYNC))==-1){
printf("Could not open /dev/mem \n");
return -1;
}
pwm_map_base[0] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM0_BASE);
pwm_map_base[1] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM1_BASE);
pwm_map_base[2] = mmap(0,getpagesize(),PROT_READ|PROT_WRITE,MAP_SHARED,dev_mem,PWM2_BASE);
if(pwm_map_base[0] == (void *) -1) {
printf("Unable to mmap pwm \n");
return(-1);
}
close(dev_mem);
// Test eqep and reset position
for(i=1;i<3;i++){
if(set_encoder_pos(i,0)){
printf("failed to access eQep register\n");
printf("eQep driver not loaded\n");
return -1;
}
}
//set up function pointers for button press events
printf("starting button interrupts\n");
set_pause_pressed_func(&null_func);
set_pause_unpressed_func(&null_func);
set_mode_pressed_func(&null_func);
set_mode_unpressed_func(&null_func);
initialize_button_handlers();
// Load binary into PRU
printf("Starting PRU servo controller\n");
if(initialize_pru_servos()){
printf("WARNING: PRU init FAILED");
}
// Print current battery voltage
printf("Battery Voltage = %fV\n", getBattVoltage());
// Start Signal Handler
printf("Enabling exit signal handler\n");
signal(SIGINT, ctrl_c);
// all done
set_state(PAUSED);
printf("\nRobotics Cape Initialized\n");
return 0;
}
