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; }
/*********************************************************************** * main() * start all the threads, and wait still something * triggers a shut down ***********************************************************************/ int main(){ // initialize cape hardware if(initialize_cape()<0){ blink_red(); return -1; } setRED(HIGH); setGRN(LOW); set_state(UNINITIALIZED); // set up button handlers first // so user can exit by holding pause set_pause_pressed_func(&on_pause_press); set_mode_unpressed_func(&on_mode_release); // load data from disk. if(load_config(&config)==-1){ printf("aborting, config file error\n"); return -1; } // start a thread to slowly sample battery pthread_t battery_thread; pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL); // start printf_thread if running from a terminal // if it was started as a background process then don't bother if(isatty(fileno(stdout))){ pthread_t printf_thread; pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL); } // start listening for RC control from dsm2 radio if(config.enable_dsm2){ if(initialize_dsm2()<0){ printf("failed to start DSM2\n"); } else{ pthread_t dsm2_thread; pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL); } } // this thread is in charge of arming and managing the core pthread_t drive_stack_thread; pthread_create(&drive_stack_thread, NULL, drive_stack, (void*) NULL); // all threads have started, off we go set_state(RUNNING); setRED(LOW); setGRN(HIGH); // chill until something exits the program while(get_state()!=EXITING){ usleep(100000); } cleanup_cape(); // always end with cleanup to shut down cleanly 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; }
/*********************************************************************** * main() * initialize the IMU, start all the threads, and wait still something * triggers a shut down ***********************************************************************/ int main(int argc, char* argv[]){ // initialize cape hardware if(initialize_cape()<0){ blink_red(); return -1; } setRED(HIGH); setGRN(LOW); set_state(UNINITIALIZED); // set up button handlers first // so user can exit by holding pause set_pause_pressed_func(&on_pause_press); set_mode_unpressed_func(&on_mode_release); // load data from disk. if(load_config(&config)==-1){ printf("aborting, config file error\n"); return -1; } // start a thread to slowly sample battery pthread_t battery_thread; pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL); // start listening for RC control from dsm2 radio if(config.enable_dsm2){ if(initialize_dsm2()<0){ printf("failed to start DSM2\n"); } else{ pthread_t dsm2_thread; pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL); } } // // start logging thread if enabled // if(config.enable_logging){ // if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){ // printf("failed to start log\n"); // } // else{ // // start new thread to write the file occationally // pthread_t logging_thread; // pthread_create(&logging_thread, NULL, log_writer, (void*) NULL); // } // } // // first check for user options // if(parse_arguments(argc, argv)<0){ // return -1; // } // // start logging thread if enabled // if(config.enable_logging){ // if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){ // printf("failed to start log\n"); // } // else{ // // start new thread to write the file occationally // pthread_t logging_thread; // pthread_create(&logging_thread, NULL, log_writer, (void*) NULL); // } // } // // Start Safety checking thread // pthread_create(&safety_thread, NULL, safety_thread_func, (void*) NULL); // Finally start the real-time interrupt driven control thread // start IMU with equilibrium set with upright orientation // for MiP with Ethernet pointing relatively up signed char orientation[9] = ORIENTATION_FLAT; if(initialize_imu(SAMPLE_RATE_HZ, orientation)){ // can't talk to IMU, all hope is lost // blink red until the user exits printf("IMU initialization failed, please reboot\n"); blink_red(); cleanup_cape(); return -1; } // assigning the interrupt function and stack // should be the last step in initialization // to make sure other setup functions don't interfere printf("starting core IMU interrupt\n"); cstate.core_start_time_us = microsSinceEpoch(); set_imu_interrupt_func(&flight_core); // start flight stack to control setpoints // this thread is in charge of arming and managing the core pthread_t flight_stack_thread; pthread_create(&flight_stack_thread, NULL, flight_stack, (void*) NULL); printf("\nReady for arming sequence\n"); set_state(RUNNING); // start printf_thread if running from a terminal // if it was started as a background process then don't bother if(isatty(fileno(stdout))){ pthread_t printf_thread; pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL); } //chill until something exits the program while(get_state()!=EXITING){ usleep(100000); } // cleanup before closing //close(sock); // mavlink UDP socket cleanup_cape(); // de-initialize cape hardware return 0; }
// main() has a brief setup and starts one background thread. // then it enters one big while loop for testing multiple capes. int main(){ int ret; float volt; imu_data_t data; // not really used, just necessary to test imu // use defaults for now, except also enable magnetometer. imu_config_t conf = get_default_imu_config(); conf.enable_magnetometer=1; // counters for how many pass and fail num_passes = 0; num_fails = 0; // initialize_cape, this should never fail unless software is not set up // in which case a useful error message should be printed out. if(initialize_cape()<0){ printf("initialize_cape() failed, this is a software issue,\n"); printf("not a hardware issue. Try running install.sh and restart\n"); return -1; } // set up the button handlers once set_pause_pressed_func(&on_pause_pressed); set_pause_released_func(&on_pause_released); set_mode_pressed_func(&on_mode_pressed); set_mode_released_func(&on_mode_released); // start blinking thread for 6V test pthread_create(&blinking_thread, NULL, blinking_function, (void*) NULL); // print welcome clear_screen(); goto_line(0); printf("Welcome to the Robotics Cape tester!\n\n"); printf("this will walk you through testing multiple capes and keep\n"); printf("track of how many pass and fail.\n"); printf("Closing the program erases the pass/fail count.\n\n"); printf("Press enter to begin, anything else to quit.\n"); if(continue_or_quit()<1){ goto END; } /*************************************************************************** * Begin main while loop ***************************************************************************/ while(get_state()!=EXITING){ line = 0; // reset current printing line to top of terminal set_led(RED,OFF); set_led(GREEN,OFF); // clear screen and print pass/fail header clear_screen(); goto_line(line); printf("passes: %d fails: %d\n", num_passes, num_fails); line+=2; goto_line(INSTRUCTION_LINE-1); printf("*******************************************************************\n"); printf("Place a new cape in the test jig but don't connect anything else.\n"); printf("Press any key to start test.\n"); // wait to start test if(continue_or_quit()<0){ goto END; } /*********************************************************************** * begin list of tests ***********************************************************************/ // make sure 12V DC supply is disconnected CHECK_DC_DISCONNECT: volt = get_dc_jack_voltage(); if(volt>2.0){ clear_instruction_area(); printf("Voltage detected on the DC jack input. This is supposed to be\n"); printf("disconnected for this part of the test.\n"); printf("Disconnect and hit ENTER to continue\n"); printf("If the DC supply was disconnected, there may be a problem with resistors\n"); printf("R1 or R14, press any key other than ENTER to FAIL this test.\n"); ret = continue_or_quit(); if(ret==1) goto CHECK_DC_DISCONNECT; else if(ret<0) goto END; else{ goto_line(line); printf("FAILED DC JACK VOLTAGE TEST\n"); line++; fail_test(); continue; } } // test imu ret = initialize_imu(&data, conf); power_off_imu(); goto_line(line); line++; if(ret<0){ printf("FAILED MPU9250 IMU\n"); fail_test(); continue; // go to beginning to test next cape } printf("PASSED MPU9250 IMU\n"); // test barometer ret = initialize_barometer(BMP_OVERSAMPLE_16,BMP_FILTER_OFF); power_off_barometer(); goto_line(line); line++; if(ret<0){ printf("FAILED BMP280 BAROMETER\n"); fail_test(); continue; // go to beginning to test next cape } printf("PASSED BMP280 BAROMETER\n"); // test buttons/LEDS clear_instruction_area(); printf("Press the PAUSE button on cape, the RED led should light up.\n"); printf("Press the MODE button on cape, the GREEN led should light up.\n"); printf("Press ENTER to indicate the buttons/leds work\n"); printf("Press any other key to indicate a failure\n"); ret = continue_or_quit(); goto_line(line); line++; if(ret==0){ printf("FAILED BUTTON/LED\n"); fail_test(); continue; } else if(ret<0) goto END; printf("PASSED BUTTON/LED\n"); // DC power jack ADC check clear_instruction_area(); printf("Plug in the 12V power supply, GREEN CHG LED should turn on.\n"); printf("Press ENTER if the GREEN CHG LED turns on\n"); printf("Press any other key if not\n"); ret = continue_or_quit(); if(ret<0) goto END; goto_line(line); line++; if(ret==0){ printf("FAILED CHARGER\n"); printf("CHG_IC may be bad.\n"); fail_test(); continue; } printf("PASSED CHARGER\n"); volt = get_dc_jack_voltage(); if(volt<11.0 || volt>13.0){ printf("FAILED 12V DC VOLTAGE\n"); printf("measuring %0.2fV at DC jack, should be roughly 12V\n", volt); printf("Resistors R1 or R14 may be bad, shorted, or missing.\n"); fail_test(); continue; } line++; printf("PASSED 12V DC VOLTAGE\n"); // 5V regulator test clear_instruction_area(); printf("Plug in the 4-pin dongle to PWR socket\n"); printf("Press ENTER if the dongle LED lights up, any other key if not.\n"); ret = continue_or_quit(); goto_line(line); line++; if(ret==0){ printf("FAILED 5V REGULATOR\n"); printf("Diode D3 or IC 5VREG may be bad\n"); fail_test(); continue; } else if(ret<0) goto END; printf("PASSED 5V REGULATOR\n"); // battery ADC check clear_instruction_area(); printf("Plug in 2-cell battery and press any key to continue\n"); ret = continue_or_quit(); if(ret<0) goto END; volt = get_battery_voltage(); goto_line(line); line++; if(volt<5.0 || volt>9.0){ printf("FAILED BATTERY VOLTAGE\n"); printf("measuring %0.2fV at battery, should be between 6 and 8.4\n", volt); printf("Resistors R19 or R25 may be bad, shorted, or missing.\n"); fail_test(); continue; } printf("PASSED BATTERY VOLTAGE\n"); // battery discharge check clear_instruction_area(); printf("Disconnect the 12V DC power supply.\n"); printf("If 4-pin dongle LED is still lit, press ENTER\n"); printf("Otherwise press any other key.\n"); ret = continue_or_quit(); if(ret<0) goto END; goto_line(line); line++; if(ret==0){ printf("FAILED BATTERY DISCHARGE\n"); printf("Diode D2, or mosfet Q3 are bad.\n"); fail_test(); continue; } printf("PASSED BATTERY DISCHARGE\n"); // 6V regulator check clear_instruction_area(); printf("Plug in the 3-pin dongle into any of the 8 servo channels.\n"); printf("If the dongle LED is blinking press ENTER.\n"); printf("Otherwise press any other key.\n"); ret = continue_or_quit(); if(ret<0) goto END; goto_line(line); line++; if(ret==0){ printf("FAILED 6VREG\n"); printf("AOZ1284PI 6VREG or supporting components are bad.\n"); fail_test(); continue; } printf("PASSED 6VREG CHECK\n"); // END OF TESTING THIS CAPE, PASSED!!! num_passes++; printf("COMPLETE TEST PASSED\n"); goto_line(0); printf("passes: %d fails: %d\n", num_passes, num_fails); clear_instruction_area(); printf("Press any key to continue with next cape\n"); continue_or_quit(); if(ret<0) goto END; // now loop back to test next cape } // end while(get_state()!= EXITING) // if we got here there was a critical error or user hit ctrl+c END: pthread_join(blinking_thread, NULL); disable_servo_power_rail(); cleanup_cape(); clear_screen(); return 0; }
/****************************************************************** * main() * initialize the IMU, start all the threads, and wait still * something triggers a shut down *******************************************************************/ int main(){ initialize_cape(); set_led(RED,HIGH); set_led(GREEN,LOW); set_state(UNINITIALIZED); // set up button handlers first // so user can exit by holding pause set_pause_pressed_func(&on_pause_press); set_mode_unpressed_func(&on_mode_release); // load data from disk. if(load_config(&config)==-1){ printf("aborting, config file error\n"); return -1; } // start a thread to slowly sample battery pthread_t battery_thread; pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL); // start printf_thread if running from a terminal // if it was started as a background process then don't bother if(isatty(fileno(stdout))){ pthread_t printf_thread; pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL); } // start listening for RC control from dsm2 radio if(config.enable_dsm2){ initialize_dsm2(); pthread_t dsm2_thread; pthread_create(&dsm2_thread, NULL, dsm2_listener, (void*) NULL); } // start mavlink if enabled if(config.enable_mavlink_listening || config.enable_mavlink_listening){ char target_ip[16]; strcpy(target_ip, DEFAULT_MAV_ADDRESS); // open a udp port for mavlink // sock and gcAddr are global variables needed to send and receive gcAddr = initialize_mavlink_udp(target_ip, udp_sock); if(udp_sock != NULL){ printf("WARNING: continuing without mavlink enabled\n"); } else { if(config.enable_mavlink_listening){ // start a thread listening for incoming packets pthread_t mav_listen_thread; pthread_create(&mav_listen_thread, NULL, mavlink_listener, (void*) NULL); printf("Listening for Packets\n"); } if(config.enable_mavlink_transmitting){ // Start thread sending heartbeat and IMU attitude packets pthread_t mav_send_thread; pthread_create(&mav_send_thread, NULL, mavlink_sender, (void*) NULL); printf("Transmitting Heartbeat Packets\n"); } } } // start logging thread if enabled if(config.enable_logging){ if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){ printf("failed to start log\n"); } else{ // start new thread to write the file occationally pthread_t logging_thread; pthread_create(&logging_thread, NULL, log_writer, (void*) NULL); } } // Finally start the real-time interrupt driven control thread // start IMU with equilibrium set with upright orientation // for MiP with Ethernet pointing relatively up signed char orientation[9] = ORIENTATION_UPRIGHT; if(initialize_imu(SAMPLE_RATE_HZ, orientation)){ // can't talk to IMU, all hope is lost // blink red until the user exits blink_red(); return -1; } // this should be the last step in initialization // to make sure other setup functions don't interfere printf("starting core IMU interrupt\n"); core_start_time_us = microsSinceEpoch(); set_imu_interrupt_func(&balance_core); // start balance stack to control setpoints pthread_t balance_stack_thread; pthread_create(&balance_stack_thread, NULL, balance_stack, (void*) NULL); printf("\nHold your MIP upright to begin balancing\n"); set_state(RUNNING); // chill until something exits the program while(get_state()!=EXITING){ usleep(100000); } // close(*udp_sock); // close network socket cleanup_cape(); // always end with cleanup to shut down cleanly 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; }