diff --git a/ESP-IDF_Robot_RC/main/rc.h b/ESP-IDF_Robot_RC/main/rc.h new file mode 100644 index 000000000..c9ac67f41 --- /dev/null +++ b/ESP-IDF_Robot_RC/main/rc.h @@ -0,0 +1,216 @@ +#ifndef RC_H +#define RC_H + +#include "driver/adc.h" +#include "esp_adc_cal.h" +#include "esp_adc/adc_oneshot.h" +#include "esp_adc/adc_continuous.h" +#include "esp_adc/adc_cali.h" +#include "esp_adc/adc_cali_scheme.h" + +#include "controls.h" + +#define ADC_CHNL ADC_CHANNEL_1 +#define ADC_ATTEN ADC_ATTEN_DB_11 +#define ADC1_CHAN0 ADC1_CHANNEL_0 +#define ADC1_CHAN1 ADC1_CHANNEL_1 + +//static const char *TAG = "ESP IDF Robot" +struct motors_rpm m; + + +static int adc_raw[2][10]; +static int voltage[2][10]; +static int s = 0, sample = 5, x = 0, y = 0, x_sum = 0, y_sum = 0; +static bool adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle); +static void adc_calibration_deinit(adc_cali_handle_t handle); +adc_cali_handle_t adc1_cali_chan0_handle, adc1_cali_chan1_handle; + +adc_oneshot_unit_handle_t adc1_handle; +bool do_calibration1_chan0, do_calibration1_chan1; + +static int interpolate_raw_val (int raw); +static int rescale_raw_val (int raw); + +static esp_err_t rc_adc_init (void) { + + adc_oneshot_unit_init_cfg_t init_config1 = { + .unit_id = ADC_UNIT_1, + }; + + ESP_ERROR_CHECK( adc_oneshot_new_unit(&init_config1, &adc1_handle)); + + adc_oneshot_chan_cfg_t config = { + .bitwidth = SOC_ADC_DIGI_MAX_BITWIDTH,//ADC_BITWIDTH_DEFAULT, + .atten = ADC_ATTEN, + }; + ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC1_CHAN0, &config)); + ESP_ERROR_CHECK(adc_oneshot_config_channel(adc1_handle, ADC1_CHAN1, &config)); + + //-------------ADC1 Calibration Init---------------// + + adc1_cali_chan0_handle = NULL; + adc1_cali_chan1_handle = NULL; + do_calibration1_chan0 = adc_calibration_init(ADC_UNIT_1, ADC1_CHAN0, ADC_ATTEN, &adc1_cali_chan0_handle); + do_calibration1_chan1 = adc_calibration_init(ADC_UNIT_1, ADC1_CHAN1, ADC_ATTEN, &adc1_cali_chan1_handle); + + return ESP_OK; +} + +static int check_motor_pcm(int x) { + int lim = 7440; + if (x > lim) + return lim; + else if (x < -lim) + return -lim; + else + return x; +} + + +static void rc_get_raw_data() { + + ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC1_CHAN0, &adc_raw[0][0])); + ESP_ERROR_CHECK(adc_oneshot_read(adc1_handle, ADC1_CHAN1, &adc_raw[0][1])); + ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, ADC1_CHAN0, adc_raw[0][0]); + ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Raw Data: %d", ADC_UNIT_1 + 1, ADC1_CHAN1, adc_raw[0][1]); + ESP_LOGI("Joystick L/R", "Position: %d (%d)", rescale_raw_val(adc_raw[0][0]), check_motor_pcm(rescale_raw_val(adc_raw[0][0]))); + ESP_LOGI("Joystick F", "Position: %d (%d)", rescale_raw_val(adc_raw[0][1]), check_motor_pcm(rescale_raw_val(adc_raw[0][1]))); + ESP_LOGW("Joystick", " sample %d, (x,y): (%d, %d)", sample, x, y); + + if (s < sample) { + x_sum += check_motor_pcm(rescale_raw_val(adc_raw[0][0])); + y_sum += check_motor_pcm(rescale_raw_val(adc_raw[0][1])); + s ++; + } + else if (s == sample) { + //x = check_motor_pcm(rescale_raw_val(adc_raw[0][0])); + //y = check_motor_pcm(rescale_raw_val(adc_raw[0][1])); + x = x_sum / sample; + y = y_sum / sample; + + + if ((x > 0 && x < 500) && (y > 500)) { + ESP_LOGW("RC", "FORWARD"); + // Both sides rotate in forward direction. + m.motor1_rpm_pcm = y; // left, forward + m.motor2_rpm_pcm = y; // right, forward + m.motor3_rpm_pcm = 0; + m.motor4_rpm_pcm = 0; + } + else if ((x > 0 && x < 500) && (y < -200)) { + ESP_LOGW("RC", "REVERSE"); + // Both sides rotate in reverse direction. + m.motor1_rpm_pcm = 0; + m.motor2_rpm_pcm = 0; + m.motor3_rpm_pcm = -y; + m.motor4_rpm_pcm = -y; + } + else if ((y < 0 && y > -200) && (x < -1000)) { + ESP_LOGW("RC", "LEFT"); + // Left side rotates in forward direction, right side rotates in reverse direction. + m.motor1_rpm_pcm = -x; + m.motor2_rpm_pcm = 0; + m.motor3_rpm_pcm = -x; + m.motor4_rpm_pcm = 0; + } + else if ((y < 0 && y > -200) && (x > 1000)) { + ESP_LOGW("RC", "RIGHT"); + // Right side rotates in forward direction, left side rotates in reverse direction. + m.motor1_rpm_pcm = 0; + m.motor2_rpm_pcm = x; + m.motor3_rpm_pcm = 0; + m.motor4_rpm_pcm = x; + } + else { + ESP_LOGW("RC", "STAND STILL"); + m.motor1_rpm_pcm = 0; + m.motor2_rpm_pcm = 0; + m.motor3_rpm_pcm = 0; + m.motor4_rpm_pcm = 0; + } + s++; + } + else { + x_sum = 0; + y_sum = 0; + s = 0; + } + + ESP_LOGI("PWM", "Motor 1 PWM: %d", m.motor1_rpm_pcm); + ESP_LOGI("PWM", "Motor 2 PWM: %d", m.motor2_rpm_pcm); + ESP_LOGI("PWM", "Motor 3 PWM: %d", m.motor3_rpm_pcm); + ESP_LOGI("PWM", "Motor 4 PWM: %d", m.motor4_rpm_pcm); + + if (do_calibration1_chan0) { + ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan0_handle, adc_raw[0][0], &voltage[0][0])); + ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, ADC1_CHAN0, voltage[0][0]); + } + if (do_calibration1_chan1) { + ESP_ERROR_CHECK(adc_cali_raw_to_voltage(adc1_cali_chan1_handle, adc_raw[0][1], &voltage[0][1])); + ESP_LOGI("ESP IDF Robot", "ADC%d Channel[%d] Cali Voltage: %d mV", ADC_UNIT_1 + 1, ADC1_CHAN1, voltage[0][1]); + } +} + +static bool adc_calibration_init(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten, adc_cali_handle_t *out_handle) +{ + adc_cali_handle_t handle = NULL; + esp_err_t ret = ESP_FAIL; + bool calibrated = false; + +#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED + if (!calibrated) { + ESP_LOGI("ESP IDF Robot", "calibration scheme version is %s", "Curve Fitting"); + adc_cali_curve_fitting_config_t cali_config = { + .unit_id = unit, + .chan = channel, + .atten = atten, + .bitwidth = ADC_BITWIDTH_DEFAULT, + }; + ret = adc_cali_create_scheme_curve_fitting(&cali_config, &handle); + if (ret == ESP_OK) { + calibrated = true; + } + } +#endif + +#if ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED + if (!calibrated) { + ESP_LOGI("ESP IDF Robot", "calibration scheme version is %s", "Line Fitting"); + adc_cali_line_fitting_config_t cali_config = { + .unit_id = unit, + .atten = atten, + .bitwidth = ADC_BITWIDTH_DEFAULT, + }; + ret = adc_cali_create_scheme_line_fitting(&cali_config, &handle); + if (ret == ESP_OK) { + calibrated = true; + } + } +#endif + + *out_handle = handle; + if (ret == ESP_OK) { + ESP_LOGI("ESP IDF Robot", "Calibration Success"); + } else if (ret == ESP_ERR_NOT_SUPPORTED || !calibrated) { + ESP_LOGW("ESP IDF Robot", "eFuse not burnt, skip software calibration"); + } else { + ESP_LOGE("ESP IDF Robot", "Invalid arg or no memory"); + } + + return calibrated; +} + +static void adc_calibration_deinit(adc_cali_handle_t handle) +{ +#if ADC_CALI_SCHEME_CURVE_FITTING_SUPPORTED + ESP_LOGI("ESP IDF Robot", "deregister %s calibration scheme", "Curve Fitting"); + ESP_ERROR_CHECK(adc_cali_delete_scheme_curve_fitting(handle)); + +#elif ADC_CALI_SCHEME_LINE_FITTING_SUPPORTED + ESP_LOGI("ESP IDF Robot", "deregister %s calibration scheme", "Line Fitting"); + ESP_ERROR_CHECK(adc_cali_delete_scheme_line_fitting(handle)); +#endif +} + +#endif \ No newline at end of file