From f46c682715dbcc3331e3c83f67e5c5319cc30188 Mon Sep 17 00:00:00 2001 From: Peter Cornwell Date: Mon, 29 Mar 2021 22:08:57 -0700 Subject: [PATCH 1/5] Added code to prevent SPI1 CS becoming an output. Added delay to doit so scp completes properly --- fw/src/app/spi.c | 6 +++++- fw/src/doit | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/fw/src/app/spi.c b/fw/src/app/spi.c index 9cda7f9..8d9ceec 100644 --- a/fw/src/app/spi.c +++ b/fw/src/app/spi.c @@ -69,7 +69,11 @@ void spi_task(void) while(true) { - memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) + // prevent CS from being treated as an output after SPI operation. + // TODO: find a proper solution to keep CS as an input pin + gpio_pin_configure(spi_cs.gpio_dev, DT_INST_0_ST_STM32_SPI_CS_GPIOS_PIN, GPIO_DIR_IN); + + memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) memset(&hat_to_pi, 0, sizeof(hat_to_pi)); // 8 bytes to Raspberry Pi (Rx) hat_to_pi.menu_button = atomic_get(&g_btn_menu); diff --git a/fw/src/doit b/fw/src/doit index f33d74f..dfb1319 100755 --- a/fw/src/doit +++ b/fw/src/doit @@ -78,6 +78,7 @@ install() { echo "Installing $DEST/$FN on moab" scp "$DEST/$FN" moab:$FN || echo "scp copy to moab failed" + sleep 5 ssh moab "sudo moab/bin/flash /home/pi/$FN" fi } From 0477d71667bb346b2f8f4a68c1d011b952ee12c5 Mon Sep 17 00:00:00 2001 From: Peter Cornwell Date: Fri, 2 Apr 2021 17:04:13 -0700 Subject: [PATCH 2/5] Adjusted to work with Zephyr SPI driver fix. Added additional error checking. --- fw/src/app/spi.c | 16 +++++++++++----- fw/src/prj.conf | 2 +- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/fw/src/app/spi.c b/fw/src/app/spi.c index 8d9ceec..fcc0d0d 100644 --- a/fw/src/app/spi.c +++ b/fw/src/app/spi.c @@ -37,6 +37,8 @@ void spi_task(void) struct device *spi; struct spi_config spi_cfg; struct spi_cs_control spi_cs; + int spi_trancieve_ret = 0; + //int cnt = 0; spi = device_get_binding("SPI_1"); if (!spi) @@ -69,10 +71,6 @@ void spi_task(void) while(true) { - // prevent CS from being treated as an output after SPI operation. - // TODO: find a proper solution to keep CS as an input pin - gpio_pin_configure(spi_cs.gpio_dev, DT_INST_0_ST_STM32_SPI_CS_GPIOS_PIN, GPIO_DIR_IN); - memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) memset(&hat_to_pi, 0, sizeof(hat_to_pi)); // 8 bytes to Raspberry Pi (Rx) @@ -84,7 +82,15 @@ void spi_task(void) // tx: outbound (button state and joystick positions) // rx: incoming (command verbs) - if (spi_transceive(spi, &spi_cfg, &tx, &rx) > 0) + spi_trancieve_ret = spi_transceive(spi, &spi_cfg, &tx, &rx); + if(spi_trancieve_ret != sizeof(pi_to_hat)) + { + if(spi_trancieve_ret >= 0) + LOG_ERR("Recieved %d bytes from pi.", spi_trancieve_ret); + else + LOG_ERR("SPI1 Error: %d", spi_trancieve_ret); + } + else { size_t size = sizeof(fifo_item_t); fifo_item_t *ptr = (fifo_item_t *) k_malloc(size); diff --git a/fw/src/prj.conf b/fw/src/prj.conf index cbb6402..8763fdf 100644 --- a/fw/src/prj.conf +++ b/fw/src/prj.conf @@ -63,7 +63,7 @@ CONFIG_SPI_INIT_PRIORITY=70 CONFIG_SPI_0=y CONFIG_SPI_0_OP_MODES=1 CONFIG_SPI_1=y -CONFIG_SPI_1_OP_MODES=1 +CONFIG_SPI_1_OP_MODES=2 CONFIG_SPI_2=y CONFIG_SPI_2_OP_MODES=1 CONFIG_SPI_STM32_INTERRUPT=y From 1779acb932a35026571743f725d268fa8fee2d91 Mon Sep 17 00:00:00 2001 From: Peter Cornwell Date: Tue, 6 Apr 2021 17:31:27 -0700 Subject: [PATCH 3/5] Added comments and cleaned-up log messages --- fw/src/app/spi.c | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/fw/src/app/spi.c b/fw/src/app/spi.c index fcc0d0d..6e0871b 100644 --- a/fw/src/app/spi.c +++ b/fw/src/app/spi.c @@ -20,6 +20,9 @@ fifo_item_t fifo_rx_queue; K_FIFO_DEFINE(my_fifo); +// Read message received from the pi out of the FIFO +// Call not thread safe. Run from one thread at a time. +// Returned message buffer must be freed after processing is done. int wait_for_pi_message(pi_to_hat_t* msg, int32_t timeout) { fifo_item_t *rx_data = k_fifo_get(&my_fifo, timeout); @@ -32,14 +35,16 @@ int wait_for_pi_message(pi_to_hat_t* msg, int32_t timeout) return 0; } +// Zephyr thread to read and write data over the +// SPI bus to/from the pi. void spi_task(void) { struct device *spi; struct spi_config spi_cfg; struct spi_cs_control spi_cs; int spi_trancieve_ret = 0; - //int cnt = 0; + // Get SPI device binding from Zephyr device tree spi = device_get_binding("SPI_1"); if (!spi) { @@ -47,6 +52,10 @@ void spi_task(void) return; } + // Set-up config struct for SPI + // Note: using custom patch for Zephyr 2.1 SPI driver to correct operation + // in slave mode. OS upgrades will need the patch implemented in spi_ll_stm32.c + // and spi_context.h. spi_cfg.cs must be set for patch to work correctly. spi_cfg.operation = SPI_WORD_SET(8) | SPI_OP_MODE_SLAVE; spi_cfg.frequency = 25000000; @@ -57,6 +66,7 @@ void spi_task(void) LOG_INF("SPI interface listening"); + // Init Rx and Tx buffers const struct spi_buf tx_buf = { .buf = &hat_to_pi, .len = sizeof(hat_to_pi) @@ -69,8 +79,10 @@ void spi_task(void) const struct spi_buf_set tx = { .buffers = &tx_buf, .count = 1 }; const struct spi_buf_set rx = { .buffers = &rx_buf, .count = 1 }; + // Infinite loop in dedicated thread to transfer data over SPI bus while(true) { + // Fill Tx buff with message for PI memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) memset(&hat_to_pi, 0, sizeof(hat_to_pi)); // 8 bytes to Raspberry Pi (Rx) @@ -82,16 +94,23 @@ void spi_task(void) // tx: outbound (button state and joystick positions) // rx: incoming (command verbs) + // Send and recieve data from the Pi. No limit on wait length. spi_trancieve_ret = spi_transceive(spi, &spi_cfg, &tx, &rx); + + // Process transfer results if(spi_trancieve_ret != sizeof(pi_to_hat)) { - if(spi_trancieve_ret >= 0) - LOG_ERR("Recieved %d bytes from pi.", spi_trancieve_ret); - else - LOG_ERR("SPI1 Error: %d", spi_trancieve_ret); + // did not recieve full length packet or an error was encountered + // in the SPI driver during the transfer + + if(spi_trancieve_ret >= 0) // was the data the wrong length? + LOG_INF("Recieved %d bytes from pi but expected %d.", spi_trancieve_ret, sizeof(pi_to_hat)); + else // Internal SPI driver error + LOG_ERR("Pi-Hat spi_transceive() returned with error: %d", spi_trancieve_ret); } else { + // Valid packet was recieved. Put on FIFO queue for later processing. size_t size = sizeof(fifo_item_t); fifo_item_t *ptr = (fifo_item_t *) k_malloc(size); __ASSERT_NO_MSG(ptr != 0); From ad11241db3820bd25ddd4e59a2c793ac5eb3aa6f Mon Sep 17 00:00:00 2001 From: Peter Cornwell Date: Fri, 9 Apr 2021 09:42:22 -0700 Subject: [PATCH 4/5] Adjusted indentation to consistently use spaces instead of tabs --- fw/src/app/spi.c | 144 +++++++++++++++++++++++------------------------ 1 file changed, 72 insertions(+), 72 deletions(-) diff --git a/fw/src/app/spi.c b/fw/src/app/spi.c index 6e0871b..6506ba5 100644 --- a/fw/src/app/spi.c +++ b/fw/src/app/spi.c @@ -12,8 +12,8 @@ static pi_to_hat_t pi_to_hat; static hat_to_pi_t hat_to_pi; typedef struct fifo_item_t { - void *fifo_reserved; /* 1st word reserved for use by fifo */ - pi_to_hat_t msg; + void *fifo_reserved; /* 1st word reserved for use by fifo */ + pi_to_hat_t msg; } fifo_item_t; fifo_item_t fifo_rx_queue; @@ -26,64 +26,64 @@ K_FIFO_DEFINE(my_fifo); int wait_for_pi_message(pi_to_hat_t* msg, int32_t timeout) { fifo_item_t *rx_data = k_fifo_get(&my_fifo, timeout); - if (rx_data == NULL) - return -1; + if (rx_data == NULL) + return -1; // shallow copy ok? - *msg = rx_data->msg; + *msg = rx_data->msg; k_free(rx_data); - return 0; + return 0; } // Zephyr thread to read and write data over the // SPI bus to/from the pi. -void spi_task(void) +void spi_task(void) { - struct device *spi; - struct spi_config spi_cfg; - struct spi_cs_control spi_cs; - int spi_trancieve_ret = 0; - - // Get SPI device binding from Zephyr device tree - spi = device_get_binding("SPI_1"); - if (!spi) - { - LOG_ERR("Could not find SPI driver\n"); - return; - } - - // Set-up config struct for SPI - // Note: using custom patch for Zephyr 2.1 SPI driver to correct operation - // in slave mode. OS upgrades will need the patch implemented in spi_ll_stm32.c - // and spi_context.h. spi_cfg.cs must be set for patch to work correctly. - spi_cfg.operation = SPI_WORD_SET(8) | SPI_OP_MODE_SLAVE; - spi_cfg.frequency = 25000000; - - spi_cs.gpio_dev = device_get_binding(DT_INST_0_ST_STM32_SPI_CS_GPIOS_CONTROLLER); - spi_cs.gpio_pin = DT_INST_0_ST_STM32_SPI_CS_GPIOS_PIN; - spi_cs.delay = 0; - spi_cfg.cs = &spi_cs; - - LOG_INF("SPI interface listening"); - - // Init Rx and Tx buffers - const struct spi_buf tx_buf = { - .buf = &hat_to_pi, - .len = sizeof(hat_to_pi) - }; - const struct spi_buf rx_buf = - { - .buf = &pi_to_hat, - .len = sizeof(pi_to_hat) - }; - const struct spi_buf_set tx = { .buffers = &tx_buf, .count = 1 }; - const struct spi_buf_set rx = { .buffers = &rx_buf, .count = 1 }; - - // Infinite loop in dedicated thread to transfer data over SPI bus - while(true) - { - // Fill Tx buff with message for PI - memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) + struct device *spi; + struct spi_config spi_cfg; + struct spi_cs_control spi_cs; + int spi_trancieve_ret = 0; + + // Get SPI device binding from Zephyr device tree + spi = device_get_binding("SPI_1"); + if (!spi) + { + LOG_ERR("Could not find SPI driver\n"); + return; + } + + // Set-up config struct for SPI + // Note: using custom patch for Zephyr 2.1 SPI driver to correct operation + // in slave mode. OS upgrades will need the patch implemented in spi_ll_stm32.c + // and spi_context.h. spi_cfg.cs must be set for patch to work correctly. + spi_cfg.operation = SPI_WORD_SET(8) | SPI_OP_MODE_SLAVE; + spi_cfg.frequency = 25000000; + + spi_cs.gpio_dev = device_get_binding(DT_INST_0_ST_STM32_SPI_CS_GPIOS_CONTROLLER); + spi_cs.gpio_pin = DT_INST_0_ST_STM32_SPI_CS_GPIOS_PIN; + spi_cs.delay = 0; + spi_cfg.cs = &spi_cs; + + LOG_INF("SPI interface listening"); + + // Init Rx and Tx buffers + const struct spi_buf tx_buf = { + .buf = &hat_to_pi, + .len = sizeof(hat_to_pi) + }; + const struct spi_buf rx_buf = + { + .buf = &pi_to_hat, + .len = sizeof(pi_to_hat) + }; + const struct spi_buf_set tx = { .buffers = &tx_buf, .count = 1 }; + const struct spi_buf_set rx = { .buffers = &rx_buf, .count = 1 }; + + // Infinite loop in dedicated thread to transfer data over SPI bus + while(true) + { + // Fill Tx buff with message for PI + memset(&pi_to_hat, 0, sizeof(pi_to_hat)); // 8 bytes from Raspberry Pi (Tx) memset(&hat_to_pi, 0, sizeof(hat_to_pi)); // 8 bytes to Raspberry Pi (Rx) hat_to_pi.menu_button = atomic_get(&g_btn_menu); @@ -94,23 +94,23 @@ void spi_task(void) // tx: outbound (button state and joystick positions) // rx: incoming (command verbs) - // Send and recieve data from the Pi. No limit on wait length. - spi_trancieve_ret = spi_transceive(spi, &spi_cfg, &tx, &rx); - - // Process transfer results - if(spi_trancieve_ret != sizeof(pi_to_hat)) - { - // did not recieve full length packet or an error was encountered - // in the SPI driver during the transfer - - if(spi_trancieve_ret >= 0) // was the data the wrong length? - LOG_INF("Recieved %d bytes from pi but expected %d.", spi_trancieve_ret, sizeof(pi_to_hat)); - else // Internal SPI driver error - LOG_ERR("Pi-Hat spi_transceive() returned with error: %d", spi_trancieve_ret); - } - else - { - // Valid packet was recieved. Put on FIFO queue for later processing. + // Send and recieve data from the Pi. No limit on wait length. + spi_trancieve_ret = spi_transceive(spi, &spi_cfg, &tx, &rx); + + // Process transfer results + if(spi_trancieve_ret != sizeof(pi_to_hat)) + { + // did not recieve full length packet or an error was encountered + // in the SPI driver during the transfer + + if(spi_trancieve_ret >= 0) // was the data the wrong length? + LOG_INF("Recieved %d bytes from pi but expected %d.", spi_trancieve_ret, sizeof(pi_to_hat)); + else // Internal SPI driver error + LOG_ERR("Pi-Hat spi_transceive() returned with error: %d", spi_trancieve_ret); + } + else + { + // Valid packet was recieved. Put on FIFO queue for later processing. size_t size = sizeof(fifo_item_t); fifo_item_t *ptr = (fifo_item_t *) k_malloc(size); __ASSERT_NO_MSG(ptr != 0); @@ -118,9 +118,9 @@ void spi_task(void) ptr->fifo_reserved = 0x0; // don't care memcpy(&ptr->msg, &pi_to_hat, sizeof(pi_to_hat)); - k_fifo_put(&my_fifo, ptr); - } - } + k_fifo_put(&my_fifo, ptr); + } + } } K_THREAD_DEFINE(spi_task_id, STACKSIZE, spi_task, NULL, NULL, NULL, PRIORITY, 0, K_NO_WAIT); From 4bfa195f348a95159d45f6e6c6cf30b3d2ed2a39 Mon Sep 17 00:00:00 2001 From: Peter Cornwell Date: Fri, 9 Apr 2021 15:21:23 -0700 Subject: [PATCH 5/5] added comments for code review. See RVR prefix for recomendations --- fw/src/app/display.c | 2 +- fw/src/app/joystick.c | 22 ++++++++++++++++++++- fw/src/app/plate.c | 45 ++++++++++++++++++++++++++++++++----------- 3 files changed, 56 insertions(+), 13 deletions(-) diff --git a/fw/src/app/display.c b/fw/src/app/display.c index a59c7da..8c01186 100644 --- a/fw/src/app/display.c +++ b/fw/src/app/display.c @@ -45,7 +45,7 @@ static lv_style_t t4_style; // big, offset 32 power_symbol int display_init() { // Although the lvgl docs say to call lv_init() - // It's already by called by this point + // It's already called by this point display_dev = device_get_binding(CONFIG_LVGL_DISPLAY_DEV_NAME); if (display_dev == NULL) { diff --git a/fw/src/app/joystick.c b/fw/src/app/joystick.c index 34e13ae..c9589b1 100644 --- a/fw/src/app/joystick.c +++ b/fw/src/app/joystick.c @@ -82,6 +82,9 @@ static int joy_button_init(struct device *gpio_dev) // gpio_pin_interrupt_configure is called here in the sample code // https://github.com/zephyrproject-rtos/zephyr/blob/master/samples/basic/button/src/main.c // why is it missing here? + + // RVR: gpio_pin_interrupt_configureis not yet present in v2.1 of Zephyr and was added in a later + // release. The current GPIO configuration is correct. gpio_init_callback(&gpio_cb_joystick, joy_button_pressed, BIT(JOY_BTN_PIN)); gpio_add_callback(gpio_dev, &gpio_cb_joystick); @@ -119,6 +122,7 @@ int joystick_position(int8_t* x_percent, int8_t* y_percent, joystick_cal_t *cal) adc_table.buffer = &adc_buffer; adc_table.buffer_size = sizeof(adc_buffer); + // read X and Y channels from the ADC ------------------------------------- if ((err = adc_read(adc, &adc_table)) != 0) return err; @@ -132,6 +136,9 @@ int joystick_position(int8_t* x_percent, int8_t* y_percent, joystick_cal_t *cal) analog_val_y = adc_buffer; + // apply calibration ------------------------------------------------------ + + // remove offset error from raw ADC counts if (cal) { analog_val_x -= (cal->x_error); @@ -139,9 +146,11 @@ int joystick_position(int8_t* x_percent, int8_t* y_percent, joystick_cal_t *cal) } + // convert counts to volts float vx = analog_val_x * ADC_VOLT_BIT; float vy = analog_val_y * ADC_VOLT_BIT; + // load in appropriate cal factor for applying gain correction xscale = 1.0; yscale = 1.0; @@ -154,6 +163,10 @@ int joystick_position(int8_t* x_percent, int8_t* y_percent, joystick_cal_t *cal) } // Remove the joystick's calibrated offset error + // RVR: this is actually removing the gain error only. Offest error correction is done above. + // RVR: why not just assign to vx/vy again or another float? That way there is no need to + // perform the over flow check. Just assign to y/x_percent after clamping to +/-100 + // Underflow can be eliminated by making analog_val_x/y a signed int16 instead of unsigned. *x_percent = -(int8_t)(((vx - JOY_CENTER_VOLTS) * (100 / JOY_CENTER_VOLTS))/xscale); *y_percent = -(int8_t)(((vy - JOY_CENTER_VOLTS) * (100 / JOY_CENTER_VOLTS))/yscale); @@ -239,6 +252,7 @@ static void joy_task(void) char c; uint32_t address = (OTP_BASE_ADDR + (OTP_BANK_SIZE*bank)); + // Read calibration from NVM ---------------------------------------------- /* Start searching for calibration data at bank 14 and work downwards. This makes sure we're always looking at the most recent valid calibration. */ bank = 14; @@ -247,6 +261,7 @@ static void joy_task(void) c = *(unsigned char *)address; // 0xCB is a magic value that indicates the beginning of calibration data + // RVR: should add magic number to defines if (c == 0xCB) { break; @@ -275,6 +290,9 @@ static void joy_task(void) } memcpy(&calibration, &cal, sizeof(joystick_cal_t)); } + + + // init ADC and IO lines for reading joystick ----------------------------- joy_gpio_dev = device_get_binding(JOY_BTN_PORT); if (!joy_gpio_dev) { @@ -296,7 +314,7 @@ static void joy_task(void) if (adc_init() != 0) LOG_ERR("ADC failed Init."); - // Read joystick at 60 Hz + // Read joystick at 62.5 Hz ----------------------------------------------- while(true) { if ((err = joystick_position(&joyx, &joyy, &calibration)) == 0) @@ -313,4 +331,6 @@ static void joy_task(void) } } +// RVR: should use K_FP_REGS flag for thread definition as floating point math is done on this thread. +// see https://docs.zephyrproject.org/2.1.0/reference/kernel/threads/index.html?highlight=k_thread_define#thread-options K_THREAD_DEFINE(joy_task_id, STACKSIZE, joy_task, NULL, NULL, NULL, PRIORITY, 0, K_NO_WAIT); diff --git a/fw/src/app/plate.c b/fw/src/app/plate.c index 3637f43..fc59c29 100644 --- a/fw/src/app/plate.c +++ b/fw/src/app/plate.c @@ -121,7 +121,8 @@ static void plate_coerce_to_range(float* value, float low, float high) *value = high; } - +// RVR: what's the difference between plate_servo_set_position and plate_servo_update_position? +// The two functions have a lot of duplicated code that could be merged. static int plate_servo_set_position(u8_t id, float angle) { int err = 0; @@ -180,6 +181,7 @@ int plate_init(void) unsigned char c; int cal_invalid = 0; + // get device bindings and set-up IO pins --------------------------------- plate_servo_en_dev = device_get_binding(SERVO_EN_PORT); if (!plate_servo_en_dev) { @@ -188,6 +190,7 @@ int plate_init(void) } gpio_pin_configure(plate_servo_en_dev, SERVO_EN_PIN, GPIO_DIR_OUT); + // RVR: Explixitly set inital state of SERVO_EN_PIN here? pwm_dev = device_get_binding(DT_ALIAS_PWM_3_LABEL); if (!pwm_dev) { @@ -195,6 +198,8 @@ int plate_init(void) return -1; } + // Load in calibration values for the three servos ------------------------ + /* If there's no calibration data, we will leave these "default" values as the calibration */ for (i=0; i<3; i++) @@ -204,29 +209,36 @@ int plate_init(void) servocal.servo_min[i] = TYPICAL_MIN; } - bank = 15; + + bank = 15; // RVR: magic number for bank. Add to defines. address = (OTP_BASE_ADDR + (OTP_BANK_SIZE * bank)); for (i=0; i<8; i++) { c = *(unsigned char *)address; //LOG_INF ("0x%08X = %02X", address, c); - if (c == 0xFF) bank-=2; - else break; + if (c == 0xFF) + bank-=2; + else + break; address = (OTP_BASE_ADDR + (OTP_BANK_SIZE * bank)); } /* Actual servo calibration values are in increments of 5 so we will never see a value of 0xFF if the calibration data is present in OTP */ if (*(unsigned char *)address != 0xFF) { - for (i=0; i MAXPULSEWIDTH) || (servocal.servo_max[i] < MINPULSEWIDTH)) cal_invalid = 1; - if ((servocal.servo_min[i] > MAXPULSEWIDTH) || (servocal.servo_min[i] < MINPULSEWIDTH)) cal_invalid = 1; - if ((servocal.servo_133[i] > MAXPULSEWIDTH) || (servocal.servo_133[i] < MINPULSEWIDTH)) cal_invalid = 1; + if ((servocal.servo_max[i] > MAXPULSEWIDTH) || (servocal.servo_max[i] < MINPULSEWIDTH)) + cal_invalid = 1; + if ((servocal.servo_min[i] > MAXPULSEWIDTH) || (servocal.servo_min[i] < MINPULSEWIDTH)) + cal_invalid = 1; + if ((servocal.servo_133[i] > MAXPULSEWIDTH) || (servocal.servo_133[i] < MINPULSEWIDTH)) + cal_invalid = 1; } } @@ -242,14 +254,23 @@ int plate_init(void) } for (i=0; i<3; i++) - LOG_INF ("Bank: %i Servo %i: Max %i Mid %i Min %i", bank, i, servocal.servo_max[i], servocal.servo_133[i], servocal.servo_min[i]); + { + LOG_INF ("Bank: %i Servo %i: Max %i Mid %i Min %i", + bank, + i, + servocal.servo_max[i], + servocal.servo_133[i], + servocal.servo_min[i]); + } + + // Apply calibration values found ----------------------------------------- servo_usec_degree[0] = (servocal.servo_max[0] - servocal.servo_min[0])/ARM_TRAVEL; servo_usec_degree[1] = (servocal.servo_max[1] - servocal.servo_min[1])/ARM_TRAVEL; servo_usec_degree[2] = (servocal.servo_max[2] - servocal.servo_min[2])/ARM_TRAVEL; plate_servos[0].dev = pwm_dev; - plate_servos[0].pwm_channel = 1; + plate_servos[0].pwm_channel = 1; // RVR: magic numbers for channel. Recomend adding to define or enum. plate_servos[1].dev = pwm_dev; plate_servos[1].pwm_channel = 2; @@ -257,9 +278,11 @@ int plate_init(void) plate_servos[2].dev = pwm_dev; plate_servos[2].pwm_channel = 3; + // set servos to inital default position ---------------------------------- + // Need to start with some defaults. We tried removing // but it's a load-bearing poster. - plate_servo_set_position(0, 150); + plate_servo_set_position(0, 150); // RVR: magic number for inital position. Recomend adding to define. plate_servo_set_position(1, 150); plate_servo_set_position(2, 150);