static int get_property(struct power_supply *b,
enum power_supply_property psp,
union power_supply_propval *val)
{
int rc = 0;
switch (psp) {
case POWER_SUPPLY_PROP_STATUS:
val->intval = get_battery_status(b);
break;
case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
val->intval = 0xfd0;
break;
case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
val->intval = 0xc00;
break;
case POWER_SUPPLY_PROP_VOLTAGE_NOW:
val->intval = get_battery_voltage();
break;
case POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN:
val->intval = 100;
break;
default:
val->intval = -1;
rc = -1;
}
return rc;
};
// makes a battery voltage string in the 2.34V format
// the buff buffer has to be at least 6 bytes long
void get_battery_voltage_str(char* buff)
{
uint16_t voltage = get_battery_voltage();
buff[0] = '0' + voltage / 100;
buff[1] = '.';
buff[2] = '0' + (voltage / 10) % 10;
buff[3] = '0' + voltage % 10;
buff[4] = 'V';
buff[5] = '\0';
}
/******************************************************************
* battery_checker()
* super slow loop checking battery voltage
*******************************************************************/
void* battery_checker(void* ptr){
float new_v;
while(get_state()!=EXITING){
new_v = get_battery_voltage();
// check if there is a bad reading
if (new_v>9.0 || new_v<5.0){
// printf("problem reading battery\n");
// use nominal for now
new_v = config.v_nominal;
}
cstate.vBatt = new_v;
usleep(1000000);
usleep(1000000);
usleep(1000000);
}
return NULL;
}
int main(){
float pack_voltage; // 2S pack voltage on JST XH 2S balance connector
float cell_voltage; // cell voltage
float jack_voltage; // could be dc power supply or another battery
if(initialize_cape()){
printf("ERROR: failed to initialize_cape()\n");
return -1;
}
while(get_state()!=EXITING){
// read in the voltage of the 2S pack and DC jack
pack_voltage = get_battery_voltage();
jack_voltage = get_dc_jack_voltage();
// sanity check the SDC didn't return an error
if(pack_voltage==-1 || jack_voltage==-1){
printf("can't read battery voltages\n");
return -1;
}
// check if a pack is on the 2S balance connector
if(pack_voltage<VOLTAGE_DISCONNECT){
pack_voltage = 0;
}
if(jack_voltage<VOLTAGE_DISCONNECT){
jack_voltage = 0;
}
// 2S pack, so divide by two for cell voltage
cell_voltage = pack_voltage/2;
// print results
printf("\rPack: %0.2fV Cell: %0.2fV DC Jack: %0.2fV ", \
pack_voltage, cell_voltage, jack_voltage);
fflush(stdout);
//check periodically
usleep(1000000);
}
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 Function
//-----------------------------------------------------------------------------
void main(void)
{
desired_range = 150;
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
PCA_Init();
ADC_Init();
Interrupt_Init();
Port_Init();
XBR0_Init();
SMB_Init();
putchar('\r');
//print beginning message
printf("En taro Adun, Executor!\r\n");
// set the PCA output to a neutral setting
steering_neutral();
ranger_neutral();
wait(); /* Let stuff warm up */
// Get the PD constants and target heading from the keypad
compass_kp = read_gain()/10.;
desired_heading = read_heading();
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
//Write the settings to the record
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
PCA0CPL1 = 0xFFFF - 3200;
PCA0CPH1 = ( 0xFFFF - 3200 ) >> 8 ;
// Start at time 0.
time = 0;
while(1) /* Infinite loop is go */
{
if ( nCounts == 50 )
{
// Update the LCD loop
lcd_clear();
lcd_print( "Batt level: %d\n", get_battery_voltage() );
lcd_print("Heading :%d\n", heading/10);
lcd_print("Alt: %d", range );
if (SS)
lcd_print("PAUSED\n");
else
lcd_print("\n");
lcd_print("# forHead, * forGain");
}
if ( read_keypad() != -1 )
{
// Check the keypad
pause();
if ( getKey() == '#' )
{
// If # was pressed, get a new heading
steering_neutral();
ranger_neutral();
desired_heading = read_heading();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
else if( getKey() == '*' )
{
// If * was pressed, get new gain contants
steering_neutral();
ranger_neutral();
compass_kp = read_gain()/10.;
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
}
else
pause();
if(SS) //while the SS is off
{
//printf("The slide switch is OFF \r\n");
steering_neutral();
ranger_neutral();
}
if(!SS) //SS is On
{
//printf("The Slide switch is On \r\n");
if ( new_heading )
printf("%lu,%d,%d\r\n",
time,
compass_old_error,
ranger_old_error );
if (new_heading) //wait for 40ms
{
heading=read_compass();
//printf("The heading result is %d\r\n", heading);
new_heading=0;
Steering_Servo( );
}
if (new_range)
{
ReadRanger( );
Speed_Cont( ); //adjust motor speed
new_range=0; //reset ranger flag
}
}
}
}
int main(void)
{
uint8_t more, ack;
uint8_t rf_pckt_ok = 0, rf_pckt_lost = 0;
uint8_t voltage_counter = 0, temperature_counter = 0;
bool read_result;
mpu_packet_t pckt;
hw_init();
for (;;)
{
pckt.flags = 0; // reset the flags
// get the battery voltage every VOLTAGE_READ_EVERY iterations
if (++voltage_counter == VOLTAGE_READ_EVERY)
{
pckt.flags |= FLAG_VOLTAGE_VALID;
pckt.voltage = get_battery_voltage();
voltage_counter = 0;
}
// same as with the voltage, send the temperature
if (++temperature_counter == TEMPERATURE_READ_EVERY)
{
pckt.flags |= FLAG_TEMPERATURE_VALID;
mpu_get_temperature(&pckt.temperature);
temperature_counter = 0;
}
// Waits for the interrupt from the MPU-6050.
// Instead of polling, I should put the MCU to sleep and then have it awaken by the MPU-6050.
// However, I have not succeeded in the making the wakeup work reliably.
while (MPU_IRQ)
dbgPoll();
while (!MPU_IRQ)
;
do {
// read all the packets in the MPU fifo
do {
read_result = dmp_read_fifo(&pckt, &more);
} while (more);
if (read_result)
{
pckt.flags |= (RECENTER_BTN == 0 ? FLAG_RECENTER : 0);
// send the message
if (rf_head_send_message(&pckt, sizeof(pckt)))
++rf_pckt_ok;
else
++rf_pckt_lost;
// update the LEDs
if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL)
{
if (rf_pckt_ok > rf_pckt_lost)
LED_GREEN = 1;
else
LED_RED = 1;
} else if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL + LED_PCKT_LED_ON) {
LED_RED = 0;
LED_GREEN = 0;
rf_pckt_ok = rf_pckt_lost = 0;
}
// check for an ACK payload
if (rf_head_read_ack_payload(&ack, 1))
{
if (ack == CMD_CALIBRATE)
{
mpu_calibrate_bias();
} else if (ack == CMD_READ_TRACKER_SETTINGS) {
rf_head_send_message(get_tracker_settings(), sizeof(tracker_settings_t));
} else if (ack >= CMD_RF_PWR_LOWEST && ack <= CMD_RF_PWR_HIGHEST) {
tracker_settings_t new_settings;
memcpy(&new_settings, get_tracker_settings(), sizeof(tracker_settings_t));
new_settings.rf_power = ack;
save_tracker_settings(&new_settings);
}
}
}
} while (more);
}
}
/*****************************************************************
* int initialize_cape()
* sets up necessary hardware and software
* should be the first thing your program calls
*****************************************************************/
int initialize_cape(){
FILE *fd;
printf("\n");
// check if another project was using resources
// kill that process cleanly with sigint if so
#ifdef DEBUG
printf("checking for existing PID_FILE\n");
#endif
fd = fopen(PID_FILE, "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(PID_FILE);
}
// create new pid file with process id
#ifdef DEBUG
printf("opening PID_FILE\n");
#endif
fd = fopen(PID_FILE, "ab+");
if (fd < 0) {
printf("\n error opening PID_FILE 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);
// check the device tree overlay is actually loaded
if (is_cape_loaded() != 1){
printf("ERROR: Device tree overlay not loaded by cape manager\n");
return -1;
}
// initialize mmap io libs
printf("Initializing GPIO\n");
if(initialize_gpio()){
printf("mmap_gpio_adc.c failed to initialize gpio\n");
return -1;
}
//export all GPIO output pins
gpio_export(RED_LED);
gpio_set_dir(RED_LED, OUTPUT_PIN);
gpio_export(GRN_LED);
gpio_set_dir(GRN_LED, OUTPUT_PIN);
gpio_export(MDIR1A);
gpio_set_dir(MDIR1A, OUTPUT_PIN);
gpio_export(MDIR1B);
gpio_set_dir(MDIR1B, OUTPUT_PIN);
gpio_export(MDIR2A);
gpio_set_dir(MDIR2A, OUTPUT_PIN);
gpio_export(MDIR2B);
gpio_set_dir(MDIR2B, OUTPUT_PIN);
gpio_export(MDIR3A);
gpio_set_dir(MDIR3A, OUTPUT_PIN);
gpio_export(MDIR3B);
gpio_set_dir(MDIR3B, OUTPUT_PIN);
gpio_export(MDIR4A);
gpio_set_dir(MDIR4A, OUTPUT_PIN);
gpio_export(MDIR4B);
gpio_set_dir(MDIR4B, OUTPUT_PIN);
gpio_export(MOT_STBY);
gpio_set_dir(MOT_STBY, OUTPUT_PIN);
gpio_export(PAIRING_PIN);
gpio_set_dir(PAIRING_PIN, OUTPUT_PIN);
gpio_export(SPI1_SS1_GPIO_PIN);
gpio_set_dir(SPI1_SS1_GPIO_PIN, OUTPUT_PIN);
gpio_export(SPI1_SS2_GPIO_PIN);
gpio_set_dir(SPI1_SS2_GPIO_PIN, OUTPUT_PIN);
gpio_export(INTERRUPT_PIN);
gpio_set_dir(INTERRUPT_PIN, INPUT_PIN);
gpio_export(SERVO_PWR);
gpio_set_dir(SERVO_PWR, OUTPUT_PIN);
printf("Initializing ADC\n");
if(initialize_adc()){
printf("mmap_gpio_adc.c failed to initialize adc\n");
return -1;
}
printf("Initializing eQEP\n");
if(init_eqep(0)){
printf("mmap_pwmss.c failed to initialize eQEP\n");
return -1;
}
if(init_eqep(1)){
printf("mmap_pwmss.c failed to initialize eQEP\n");
return -1;
}
if(init_eqep(2)){
printf("mmap_pwmss.c failed to initialize eQEP\n");
return -1;
}
// setup pwm driver
printf("Initializing PWM\n");
if(simple_init_pwm(1,PWM_FREQ)){
printf("simple_pwm.c failed to initialize PWMSS 0\n");
return -1;
}
if(simple_init_pwm(2,PWM_FREQ)){
printf("simple_pwm.c failed to initialize PWMSS 1\n");
return -1;
}
// start some gpio pins at defaults
deselect_spi1_slave(1);
deselect_spi1_slave(2);
disable_motors();
//set up function pointers for button press events
printf("Initializing button interrupts\n");
initialize_button_handlers();
// Load binary into PRU
printf("Initializing PRU\n");
if(initialize_pru()){
printf("ERROR: PRU init FAILED\n");
return -1;
}
// Start Signal Handler
printf("Initializing exit signal handler\n");
signal(SIGINT, ctrl_c);
signal(SIGTERM, ctrl_c);
// Print current battery voltage
printf("Battery Voltage: %2.2fV\n", get_battery_voltage());
// all done
set_state(PAUSED);
printf("\nRobotics Cape Initialized\n");
return 0;
}
