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;
}
