I currently have a program running on an ESP32(1) that receives 'realtime' data every 10ms from another ESP32(2) via BT SPP.
Also there runs some code to read an MPU9250's gyro by the use of the https://github.com/natanaeljr/esp32-MPU-driver library.
All works fine seperately (BT / reading the gyro). But when both code segments are enabled this happens:
gyro is first up and running -> every ms the angular speed is read out of the MPU by use of I2C.
I plug-in the second ESP(2) running adapted spp_vfs_acceptor code (the one that transmits the data), as soon as they get a link and the BT data is received on ESP(1), I get a whole lot of the following errors on the monitor:
Code: Select all
W (9276) BT_APPL: new conn_srvc id:26, app_id:255
...
I (9276) SPP_RING: ESP_SPP_SRV_OPEN_EVT //so there is a connection made with the ESP(2),
//right after that this begins to flood the monitor (every ms the gyro is meant to be read out:]
...
E (75026) i2c: /home/labo/esp/esp-idf/components/driver/i2c.c:1243 (i2c_master_cmd_begin):i2c number error
E (75026) I2Cbus: [port:-1073684474, slave:0x32] Failed to read 6 bytes from register 0x43, error: 0x102
E (75026) MPU9250: func:esp_err_t mpud::MPU::rotation(mpud::types::raw_axes_t*) @ line:946, expr:"readBytes(regs::GYRO_XOabsRingPos UT_H, 6, buffer)", error:0x102
.. (repeat)
Thanks in advance!
** to be complete, the current main code:
Code: Select all
/*
* 1. Open up the project properties
* 2. Visit C/C++ General > Preprocessor Include Paths, Macros, etc
* 3. Select the Providers tab
* 4. Check the box for "CDT GCC Built-in Compiler Settings"
* 5. Set the compiler spec command to "xtensa-esp32-elf-gcc ${FLAGS} -E -P -v -dD "${INPUTS}""
* 6. Rebuild the index
*/
//#define CONSOLE_INPUT
//#define ONLY_PRINT_FAST_WITH_BUTTON1_ENABLED
//#define PID_DEBUG_VALUES
//define PRINT_CSV_PROFILE_AT_STARTUP
//#define PRINT_ELAPSED_BT_TIME
//#define PRINT_FOILTENSION
//#define PRINT_ELAPSED_TIME_GYRO
#define PRINT_FOIL_TENSION_MOTORSPEED_FAST
//#define PRINT_FOIL_TENSION_AND_CORRECTED_FAST
//#define PRINT_FOIL_TENSION_MOTORSPEED_SLOW
//#define PRINT_GYRO_SPEED
#define PRINT_BT_DATA_FAST
//#define PRINT_BT_DATA_SLOW
//#define PRINT_DEBUG_BUTTONS
#define PROFILE_PATH "/spiffs/profile.csv"
//#define USE_MPU
#define Z_AXIS_GYRO
#define foilTensionDefaultPID_P 1
#define foilTensionDefaultPID_I 0
#define foilTensionDefaultPID_D 0
#define DEFAULT_FOILTENSION_SETPOINT 500
#define FOIL_TENSION_DUE_TO_PRELOAD 2540 //will be determined when performing a calibration of the loadcell
#define MS portTICK_PERIOD_MS
extern "C"
{
#ifdef CONSOLE_INPUT
#include "argtable3/argtable3.h"
#include "cmd_console.h"
#endif
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "driver/ledc.h"
#include "driver/uart.h"
#include "esp_bt.h"
#include "esp_bt_device.h"
#include "esp_bt_main.h"
#include "esp_console.h"
#include "esp_err.h"
#include "esp_event_loop.h"
#include "esp_gap_bt_api.h"
#include "esp_log.h"
#include "esp_spp_api.h"
#include "esp_spiffs.h"
#include "esp_system.h"
#include "esp_vfs.h"
#include "esp_vfs_dev.h"
#include "esp_wifi.h"
//#include "esp_vfs_fat.h"
#include "freertos/FreeRTOS.h"
#include "freertos/portmacro.h"
#include "freertos/task.h"
#include "linenoise/linenoise.h"
#include "nvs.h"
#include "nvs_flash.h"
#include "sdkconfig.h"
#include "soc/timer_group_reg.h"
#include "soc/timer_group_struct.h"
#include "spp_task.h"
#include "sys/time.h"
#include "sys/unistd.h"
#include "time.h"
#include <driver/adc.h>
#include <esp_int_wdt.h>
#include <esp_task_wdt.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <driver/dac.h>
#include <cstring>
}
#include "I2Cbus.hpp"
#include "InternalADC.h" //class (library) to read an internal ADC and filter the value with runningAverage & exponentialSmoothing
#include "MPU.hpp" // main file, provides the class itself
#include "mpu/math.hpp" // math helper for dealing with MPU data
#include "mpu/types.hpp" // MPU data types and definitions
#include "PID_v1.h"
#include <iostream>
#include <string>
#include <cstring>
#include <vector>
#include "ServoControl.h"
//**********************************************************************************
//**********************************************************************************
// TYPE_DEFS
typedef union
{
int32_t _int32;
uint8_t _byte[4];
} INT32UNION_t;
typedef union
{
int16_t _int16;
uint8_t _byte[2];
} INT16UNION_t;
typedef union
{
float _float;
uint8_t _byte[4];
} FLOATUNION_t;
typedef struct{
float Kp;
float Ki;
float Kd;
} PID_PARAMS_t;
//**********************************************************************************
//**********************************************************************************
// GPIO Parameters
#define ONBOARD_LED_GPIO GPIO_NUM_5
#define SDA_GPIO GPIO_NUM_21
#define SCL_GPIO GPIO_NUM_22
#define AdcChannel_FoilTension ADC1_CHANNEL_0//pin 36
//#define AdcChannel_PidTuning_P ADC1_CHANNEL_1//pin 37
//#define AdcChannel_PidTuning_I ADC1_CHANNEL_2//pin 38
#define MOTOR_B_PWM_GPIO -1 //GPIO_NUM_14 was voorzien
#define MOTOR_B_RESET_GPIO GPIO_NUM_25 //currently together with motor C - pin 25
#define MOTOR_B_ENABLE_GPIO GPIO_NUM_14 //GPIO_NUM_26 was voorzien
#define MOTOR_B_DAC_CHANNEL DAC_CHANNEL_1 //GPIO_PIN_25
#define MOTOR_C_PWM_GPIO -1//GPIO_NUM_19
//#define MOTOR_C_ENABLE_GPIO GPIO_NUM_27
#define MOTOR_C_RESET_GPIO GPIO_NUM_25
#define MOTOR_C_ENABLE_GPIO GPIO_NUM_19
#define MOTOR_C_DAC_CHANNEL DAC_CHANNEL_2 //GPIO_PIN_26
#define DEBUG_BUTTON1_GPIO GPIO_NUM_17
#define DEBUG_BUTTON2_GPIO GPIO_NUM_18
#define DEBUG_BUTTON3_GPIO GPIO_NUM_23 //rode drukknop
bool button1_toggled = false;
// ADC Parameters
InternalADC foilTensionADC;
InternalADC pidTuning_P_ADC;
InternalADC pidTuning_I_ADC;
// DAC Parameters
#define FULLSCALE_DAC_VALUE_WIDTH 255
// PWM Parameters
#define LEDC_CH_NUM (2)
ledc_timer_config_t ledc_timer;
ledc_channel_config_t ledc_channel[LEDC_CH_NUM];
#define LEDC_HS_TIMER LEDC_TIMER_0
#define LEDC_HS_MODE LEDC_HIGH_SPEED_MODE
#define LEDC_HS_CH0_CHANNEL LEDC_CHANNEL_0
#define LEDC_HS_CH1_CHANNEL LEDC_CHANNEL_1
#define LEDC_DUTY_RESOLUTION LEDC_TIMER_12_BIT // resolution of PWM duty 12bit -> 0..4096
#define LEDC_DUTY_WIDTH 4096
#define LEDC_FREQ 5000 // frequency of PWM signal
//**********************************************************************************
//**********************************************************************************
// Foil Control Parameters
bool calibrateLoadcellRequested = false;
float foilTension_raw=0;
float foilTension_corrected=0;
float foilTensionSetpoint=0;
float staticFoilTensionOffset = FOIL_TENSION_DUE_TO_PRELOAD;
float foilStretchFactor = 1.5;
float speed_gyro_dps;
float speed_gyro_radps;
int32_t absRingPosition = 0;
int32_t relativeRingPosition = 0;
int32_t ringTurns = 0;
int ringSpeed = 0;
bool resetPositionRequested = false;
PID_PARAMS_t foilTensionPidTunings;
float profileStrainTension[360][2];
//**********************************************************************************
//**********************************************************************************
// ServoMotors Control Parameters
#define MAX_CONTROL_SPEED_RPM_FWD_SERVO_B 2500
#define MAX_CONTROL_SPEED_RPM_FWD_SERVO_C 2500
#define MAX_CONTROL_SPEED_RPM_BWD_SERVO_B 0
#define MAX_CONTROL_SPEED_RPM_BWD_SERVO_C 0
#define countsPerRev_MotorSide 4096
#define countsPerRev_StretchRol 6007.451 //geared 4096*44T/30T
#define MAX_SPEED_RPM_FWD 3000
#define MAX_SPEED_RPM_BWD 3000
#define LOWEST_DUTY_PERCENTAGE_FOR_ROTATION_C 0
#define HIGHEST_DUTY_PERCENTAGE_FOR_ROTATION_C 100 //3.3V = 100%
#define SERVO_MOTION_RANGE 200000 //200.000 @ 3.3V = 303.030 @ 5V -> 303030 in ingenia motionlab (reads 0..5V)
#define SERVO_MOTION_OFFSET 0
float motorBSpeedSetpointRPM=0;
float motorC_PID_SpeedTarget_RPM=0;
float motorCSpeedSetpointRPM=0;
const char* servoB_NAME = "servoB";
const char* servoC_NAME = "servoC";
bool resetDrivesRequested=false;
bool servosEnabled = true;
bool estopActive = true;
ServoControl servoB(servoB_NAME, (gpio_num_t)MOTOR_B_ENABLE_GPIO, (gpio_num_t) MOTOR_B_RESET_GPIO, (gpio_num_t) MOTOR_B_PWM_GPIO,&ledc_channel[0],MOTOR_B_DAC_CHANNEL);
ServoControl servoC(servoC_NAME,(gpio_num_t)MOTOR_C_ENABLE_GPIO,(gpio_num_t) MOTOR_C_RESET_GPIO, (gpio_num_t) MOTOR_C_PWM_GPIO,&ledc_channel[1],MOTOR_C_DAC_CHANNEL);
int32_t motorB_Duty=0, motorC_Duty=0;
//**********************************************************************************
//**********************************************************************************
// PID Parameters
PID foilTensionPID(&foilTension_corrected,
&motorC_PID_SpeedTarget_RPM,
&foilTensionSetpoint,
foilTensionDefaultPID_P,
foilTensionDefaultPID_I,
foilTensionDefaultPID_D,
P_ON_E,
REVERSE);
//**********************************************************************************
//**********************************************************************************
// I2C Parameters
static constexpr uint32_t CLOCK_SPEED = 400000; // range from 100 KHz ~ 400Hz
#define I2C_TAG "i2c:"
#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master do not need buffer */
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master do not need buffer */
#define I2C_MASTER_TX_BUF_ENABLE 1 /*!< I2C master do not need buffer */
#define I2C_MASTER_RX_BUF_ENABLE 1 /*!< I2C master do not need buffer */
#define I2C_MASTER_NUM I2C_NUMBER(CONFIG_I2C_MASTER_PORT_NUM) /*!< I2C port number for master dev */
//**********************************************************************************
//**********************************************************************************
// MPU Parameters
MPU_t MPU; // create a default MPU object
//**********************************************************************************
//**********************************************************************************
// SPIFFS/ NVS flash Parameters
#define STORAGE_NAMESPACE "storage"
//**********************************************************************************
//**********************************************************************************
// BlueTooth Parameters
enum BT_SERVER_STATE {
SERVER_TIMEOUT, //server should reboot, complete reinitialisation (also state at startup)
INITIALIZING_SERVER, //server is initialising, not yet ready to receive request
WAITING_FOR_PARTNER,
PARTNER_CONNECTED,
CYCLIC_DATA_RECEIVE,
CYCLIC_DATA_TIMEOUT,
} ;
BT_SERVER_STATE Bt_ServerState=SERVER_TIMEOUT;
#define SPP_TAG "SPP_RING"
#define SPP_SERVER_NAME "SPP_SERVER"
#define EXCAMPLE_DEVICE_NAME "ESP_SPP_ACCEPTOR"
#define WN "_WN_"
#define DATA_TAG "data"
static const esp_spp_mode_t esp_spp_mode = ESP_SPP_MODE_VFS;
static const esp_spp_sec_t sec_mask = ESP_SPP_SEC_AUTHENTICATE;
static const esp_spp_role_t role_slave = ESP_SPP_ROLE_SLAVE;
bool btConnectionOpen = false;
#define SPP_DATA_LENGTH 1 //was 1 voor eerste code //normaal 18
#define VALUE_LENGTH 16
static uint8_t spp_data[SPP_DATA_LENGTH];
std::string receivedSerialBTString = "";
int btFd=-1;
int receivedValue[VALUE_LENGTH];
//**********************************************************************************
//***********************************SPP_DATA_LENGTH***********************************************
// WiFi Parameters
static const char *WIFI_TAG="example";
wifi_init_config_t cfg;
bool wifiEnabled=false;
//**********************************************************************************
extern "C"
{
void app_main (void);
void blink_task (void *pvParameter);
void digitalInputs(void *pvParameter);
void internalADC (void *pvParameters);
void ads111x_test (void *pvParameters);
void i2c_master_init (void *pvParameters);
void pwm_init(void *pvParameters);
static void spp_write(const char* command);
static void spp_read_handle (void * param);
static void esp_spp_cb (uint16_t e, void *p);
static void esp_spp_stack_cb (esp_spp_cb_event_t event, esp_spp_cb_param_t *param);
void esp_bt_gap_cb (esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param);
void mpu_task (void * params);
static void periodic_timer_callback(void* arg);
#ifdef CONSOLE_INPUT
void consoleInput_loop(void *pvParameter);
#endif
// WiFi functions
static esp_err_t event_handler(void *ctx, system_event_t *event);
static void initialise_wifi(void);
static esp_err_t init_spiffs(void);
esp_err_t start_file_server(const char *base_path);
}
// Helper Functions
static int64_t map_int(int64_t x, int64_t in_min, int64_t in_max, int64_t out_min, int64_t out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
#define millis() esp_timer_get_time()*0.001
#define micros() esp_timer_get_time()
#define printSec(s, v) for (static uint32_t _lasttime; (uint32_t)((uint32_t)millis() - _lasttime) >= (1000); _lasttime += (1000)) { std::cout<<s<<"\t"<<v<<"\n";}
#define runEvery(t) for (static uint16_t _lasttime;(uint16_t)((uint16_t)millis() - _lasttime) >= (t); _lasttime += (t))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
//**********************************************************************************
//**********************************************************************************
// Blink Functions
//
//void blink_task (void *pvParameter)
//{
// while (1)
// {
// /* Blink off (output low) */
// gpio_set_level (ONBOARD_LED_GPIO, 0);
// vTaskDelay (100 / portTICK_PERIOD_MS);
// /* Blink on (output high) */
// gpio_set_level (ONBOARD_LED_GPIO, 1);
// vTaskDelay (10 / portTICK_PERIOD_MS);
// }
//}
//**********************************************************************************
//**********************************************************************************
// GPIO Functions
void _gpio_init(){
//***************************** IO ************************************************
//LED
gpio_pad_select_gpio (ONBOARD_LED_GPIO);
gpio_set_direction (ONBOARD_LED_GPIO, GPIO_MODE_OUTPUT);
//INPUT BUTTON
gpio_pad_select_gpio (DEBUG_BUTTON1_GPIO);
gpio_set_direction ((gpio_num_t) DEBUG_BUTTON1_GPIO, GPIO_MODE_INPUT);
gpio_set_pull_mode ((gpio_num_t) DEBUG_BUTTON1_GPIO, GPIO_PULLUP_ONLY);
gpio_pad_select_gpio (DEBUG_BUTTON2_GPIO);
gpio_set_direction ((gpio_num_t) DEBUG_BUTTON2_GPIO, GPIO_MODE_INPUT);
gpio_set_pull_mode ((gpio_num_t) DEBUG_BUTTON2_GPIO, GPIO_PULLUP_ONLY);
gpio_pad_select_gpio (DEBUG_BUTTON3_GPIO);
gpio_set_direction ((gpio_num_t) DEBUG_BUTTON3_GPIO, GPIO_MODE_INPUT);
gpio_set_pull_mode ((gpio_num_t) DEBUG_BUTTON3_GPIO, GPIO_PULLUP_ONLY);
// //***************************** ADC ************************************************
////default: ADC_ATTEN_DB_6,ADC_WIDTH_BIT_12
foilTensionADC.initADC1 (AdcChannel_FoilTension,ADC_ATTEN_DB_0,ADC_WIDTH_BIT_12);
foilTensionADC.setSmoothingFactor (0.1); //0 = infinite filtered, 1 = no filtering
}
//**********************************************************************************
//**********************************************************************************
// DAC Init Functions
void dac_init(void){
dac_output_enable(MOTOR_B_DAC_CHANNEL);
dac_output_enable(MOTOR_C_DAC_CHANNEL);
dac_output_voltage(MOTOR_B_DAC_CHANNEL, 0);
dac_output_voltage(MOTOR_C_DAC_CHANNEL, 0);
}
//**********************************************************************************
//**********************************************************************************
// PWM Init Functions
void pwm_init(void){
// ************** PWM won't be used, switched over to analog speed setpoint
//
// int ch;
//
// /*
// * Prepare and set configuration of timers
// * that will be used by LED Controller
// */
//
//
// ledc_timer.duty_resolution = LEDC_DUTY_RESOLUTION; // resolution of PWM duty
// ledc_timer.freq_hz = LEDC_FREQ; // frequency of PWM signal
// ledc_timer.speed_mode = LEDC_HS_MODE; // timer mode
// ledc_timer.timer_num = LEDC_HS_TIMER; // timer index
//
// // Set configuration of timer0 for high speed channels
// ledc_timer_config(&ledc_timer);
//
//
//
// ledc_channel[0].channel = LEDC_HS_CH0_CHANNEL;
// ledc_channel[0].duty = 0;
// ledc_channel[0].gpio_num = MOTOR_B_PWM_GPIO;
// ledc_channel[0].speed_mode = LEDC_HS_MODE;
// ledc_channel[0].hpoint = 0;
// ledc_channel[0].timer_sel = LEDC_HS_TIMER;
//
// ledc_channel[1].channel = LEDC_HS_CH1_CHANNEL;
// ledc_channel[1].duty = 0;
// ledc_channel[1].gpio_num = MOTOR_C_PWM_GPIO;
// ledc_channel[1].speed_mode = LEDC_HS_MODE;
// ledc_channel[1].hpoint = 0;
// ledc_channel[1].timer_sel = LEDC_HS_TIMER;
//
//
// // Set LED Controller with previously prepared configuration
// for (ch = 0; ch < LEDC_CH_NUM; ch++) {
// ledc_channel_config(&ledc_channel[ch]);
// }
}
//**********************************************************************************
//**********************************************************************************
// Internal ADC Functions
void updateADC ()
{
foilTension_raw = foilTensionADC.updateADC1 ();
#ifdef PRINT_FOILTENSION
std::cout<<foilTension_raw<<"\t"<<"\n";
#endif
}
//**********************************************************************************
//**********************************************************************************
// Digital Input Functions
void digitalInputs (void *pvParameter){
while (1)
{
// printf("looping digitalInputs\n");
TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE;
TIMERG0.wdt_feed=1;
TIMERG0.wdt_wprotect=0;
if (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON1_GPIO))
{ //BUTTON1 pressed -> gpio_level = low -> read ADC & write to parameter
// static int j = 0;
//still needs a MAP(x,...)
while (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON1_GPIO))
{
gpio_set_level (ONBOARD_LED_GPIO, 1);
vTaskDelay (25 / portTICK_PERIOD_MS);
gpio_set_level (ONBOARD_LED_GPIO, 0);
vTaskDelay (25 / portTICK_PERIOD_MS);
}
#ifdef PRINT_DEBUG_BUTTONS
std::cout << "button1 pressed\n";
vTaskDelay(100/MS);
#endif
servosEnabled = !servosEnabled;
servoB.disable();
servoC.disable();
}
if (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON2_GPIO))
{ //BUTTON2 pressed -> gpio_level = low -> read ADC & write to parameter
while (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON2_GPIO)) // No bluetooth, time for WiFi!
{
gpio_set_level (ONBOARD_LED_GPIO, 1);
vTaskDelay (25 / portTICK_PERIOD_MS);
gpio_set_level (ONBOARD_LED_GPIO, 0);
vTaskDelay (25 / portTICK_PERIOD_MS);
}
// int mode = foilTensionPID.GetMode();
// if(mode==MANUAL) foilTensionPID.SetMode(AUTOMATIC);
// else if (mode==AUTOMATIC) foilTensionPID.SetMode(MANUAL);
// std::cout << "FoilTensionPID: mode =" << foilTensionPID.GetMode()<<"\n";
//
#ifdef PRINT_DEBUG_BUTTONS
std::cout << "button2 pressed\n";
vTaskDelay(100/MS);
#endif
button1_toggled = !button1_toggled;
// wifiEnabled=!wifiEnabled;
//
// if(wifiEnabled){
// printf("starting WiFi\n");
// ESP_ERROR_CHECK(esp_wifi_start());
// }
// else{
// printf("stopping WiFi\n");
// ESP_ERROR_CHECK(esp_wifi_stop());
// for(int i = 0; i< SPP_DATA_LENGTH; i++) {
// spp_data[i]=0;
// }
//
// }
}
//************************************************
// DEBUG_BUTTON3 IS NOT CONNECTED TO Input Switch Anymore! (code still present for future reference)
//************************************************
// if (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON3_GPIO))
// { //BUTTON2 pressed -> gpio_level = low -> read ADC & write to parameter
// while (!gpio_get_level ((gpio_num_t) DEBUG_BUTTON3_GPIO))
// {
// gpio_set_level (ONBOARD_LED_GPIO, 1);
// vTaskDelay (150 / portTICK_PERIOD_MS);
// gpio_set_level (ONBOARD_LED_GPIO, 0);
// vTaskDelay (150 / portTICK_PERIOD_MS);
//#ifdef PRINT_DEBUG_BUTTONS
// std::cout << "button3 hold\n";
//#endif
// }
// servoB.resetFault ();
// servoC.resetFault ();
//#ifdef PRINT_DEBUG_BUTTONS
// std::cout << "button3 pressed - Reset Drives\n";
// vTaskDelay(100/MS);
//#endif
// vTaskDelay (1000 / MS);
// }
//
vTaskDelay (50 / MS);
//
}
}
//**********************************************************************************
//**********************************************************************************
// Other Low Priority Task Functions
void otherLowPriorityTasks (void *pvParameter){
while (1)
{
// printf("looping otherLowPriorityTasks\n");
TIMERG0.wdt_wprotect = TIMG_WDT_WKEY_VALUE;
TIMERG0.wdt_feed = 1;
TIMERG0.wdt_wprotect = 0;
if (resetDrivesRequested)
{
servoB.resetFault ();
servoC.resetFault ();
std::cout << "Reset Drives\n";
resetDrivesRequested = false;
}
static bool prevServosEnabled = !servosEnabled;
if (servosEnabled != prevServosEnabled)
{
if (servosEnabled)
{ //servos must be on while they are not
servoB.enable ();
servoC.enable ();
servosEnabled = true;
prevServosEnabled = servosEnabled;
}
else
{
servoB.disable ();
servoC.disable ();
servosEnabled = false;
prevServosEnabled = servosEnabled;
}
}
if (resetPositionRequested)
{
if (btConnectionOpen)
{
spp_write ("resetpos");
printf ("sent resetpos over bt\n");
resetPositionRequested = false;
}
else
{
printf ("Position not resetted, BT connection is not open...\n");
printf ("Position will be reset when BT connection is made!\n");
}
}
vTaskDelay (200 / MS);
}
}
//**********************************************************************************
//**********************************************************************************
// Motor Control Functions
void servomotors_init(){
servoB.setSpeedParameters(MAX_SPEED_RPM_FWD, MAX_SPEED_RPM_BWD);
servoC.setSpeedParameters(MAX_SPEED_RPM_FWD, MAX_SPEED_RPM_BWD);
servoB.setCountsPerRev(countsPerRev_StretchRol);
servoC.setCountsPerRev(countsPerRev_StretchRol);
servoB.setAnalogValueScalings(FULLSCALE_DAC_VALUE_WIDTH, SERVO_MOTION_RANGE, SERVO_MOTION_OFFSET);
servoC.setAnalogValueScalings(FULLSCALE_DAC_VALUE_WIDTH, SERVO_MOTION_RANGE, SERVO_MOTION_OFFSET);
servoB.setRPM(0,true,true);
servoB.enable ();
servoC.setRPM (0,true,true); //speed 0 has to be written to PWM pin; speed 0 != duty 0!!!!
servoC.enable ();
//servoC.disable();
//servoB.disable();
}
void motorTask(){
if(estopActive) {
servoB.disable();
servoC.disable();
}
else{
servoB.setRPM(motorBSpeedSetpointRPM,true,true); // should then be converted to dac_outp_voltage..
servoC.setRPM(motorCSpeedSetpointRPM,true,true);
}
// std::cout<<motorCSpeedSetpointRPM<<"\n";
// dac_output_voltage(MOTOR_B_DAC_CHANNEL, (uint8_t) motorBSpeedSetpointRPM/2);
// dac_output_voltage(MOTOR_C_DAC_CHANNEL, (uint8_t) motorCSpeedSetpointRPM); // /5 om rpm ongeveer naar 0-255 om te zetten (8bit dac)
}
//**********************************************************************************
//**********************************************************************************
// I2C Functions
void i2c_master_init ()
{
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = SDA_GPIO;
conf.scl_io_num = SCL_GPIO;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = CLOCK_SPEED;
ESP_ERROR_CHECK(i2c_param_config (I2C_NUM_0, &conf));
ESP_ERROR_CHECK(i2c_driver_install (I2C_NUM_0, conf.mode, 0, 0, 0));
}
//**********************************************************************************
//**********************************************************************************
// MPU Functions
void mpu_init(){
// Initialize I2C on port 0 using I2Cbus interface
// i2c0.begin(SDA, SCL, CLOCK_SPEED);
// Or directly with esp-idf API
MPU.setBus (i2c0); // set bus port, not really needed since default is i2c0
MPU.setAddr (mpud::MPU_I2CADDRESS_AD0_LOW); // set address, default is AD0_LOW
// Great! Let's verify the communication
// (this also check if the connected MPU supports the implementation of chip selected in the component menu)
while (esp_err_t err = MPU.testConnection ())
{
ESP_LOGE(I2C_TAG, "Failed to connect to the MPU, error=%#X", err);
vTaskDelay (1000 / portTICK_PERIOD_MS);
}
ESP_LOGI(I2C_TAG, "MPU connection successful!");
// Initialize
ESP_ERROR_CHECK(MPU.initialize ()); // initialize the chip and set initial configurations
// Setup with your configurations
ESP_ERROR_CHECK(MPU.setSampleRate(1000)); // set sample rate to 50 Hz
ESP_ERROR_CHECK(MPU.setGyroFullScale (mpud::GYRO_FS_2000DPS));
// ESP_ERROR_CHECK(MPU.setAccelFullScale(mpud::ACCEL_FS_4G));
}
void readMPU(){
mpud::float_axes_t gyroDPS,gyroRPS; // gyro axes in (DPS) º/s format
mpud::raw_axes_t gyroRaw; // x, y, z axes as int16
MPU.rotation (&gyroRaw); // fetch raw data from the registers
gyroDPS = mpud::gyroDegPerSec (gyroRaw, mpud::GYRO_FS_2000DPS);
gyroRPS = mpud::gyroRadPerSec(gyroRaw, mpud::GYRO_FS_2000DPS);
MPU.rotation (&gyroRaw); // fetch raw data from the registers
gyroDPS = mpud::gyroDegPerSec (gyroRaw, mpud::GYRO_FS_2000DPS);
#ifdef PRINT_ELAPSED_TIME_GYRO
static int64_t prevCurrentMicros = esp_timer_get_time();
int64_t currentMicros = esp_timer_get_time();
int elapsedTime =currentMicros-prevCurrentMicros;
printf("elapsedTime:%d\n",elapsedTime);
prevCurrentMicros = currentMicros;
#endif
#ifdef X_AXIS_GYRO
speed_gyro_dps = gyroDPS[0];
speed_gyro_radps = gyroRPS[0];
#endif
#ifdef Y_AXIS_GYRO
speed_gyro_dps = gyroDPS[1];
speed_gyro_radps = gyroRPS[1];
#endif
#ifdef Z_AXIS_GYRO
speed_gyro_dps = gyroDPS[2]; //get degrees/sec speed from gyro's Z axis
speed_gyro_radps = gyroRPS[2]; //get radians/sec speed from gyro's Z axis
#endif
#ifdef PRINT_GYRO_SPEED
printf("gyro speed:%f °/s %f rad/s\n",speed_gyro_dps,speed_gyro_radps);
#endif
}
//**********************************************************************************
//**********************************************************************************
// PID/Control Functions
void setupPID(){
// PID foilTensionPID(&foilTension_corrected,
// &motorC_PID_SpeedTarget_RPM,
// &foilTensionSetpoint,
// foilTensionDefaultPID_P,
// foilTensionDefaultPID_I,
// foilTensionDefaultPID_D,
// P_ON_E,
// REVERSE);
foilTensionPID.SetMode(false);
foilTensionPID.SetSampleTime(1000); //1ms sample time
foilTensionPID.SetOutputLimits(MAX_CONTROL_SPEED_RPM_BWD_SERVO_C,MAX_CONTROL_SPEED_RPM_FWD_SERVO_C);
#ifdef PID_DEBUG_VALUES
foilTensionPID.SetDebugMessaging(true,100000);
#else
foilTensionPID.SetDebugMessaging(false,100000);
#endif
foilTensionSetpoint = DEFAULT_FOILTENSION_SETPOINT;
foilTensionPID.SetMode(AUTOMATIC);
}
float floatRunningAverage (float M)
{
#define LM_SIZE 20
static float LM[LM_SIZE]; // LastMeasurements
static float _sum=0;
static int _count=0;
static int index = 0;
// keep sum updated to improve speed.
_sum -= LM[index];
LM[index] = M;
_sum += LM[index];
index++;
index = index % LM_SIZE;
if (_count < LM_SIZE)
_count++;
return _sum / _count;
}
void foilTensionTask(){
if(calibrateLoadcellRequested){ //ONLY ENABLE WHEN NO FOIL IS STRETCHED AROUND THE LOADCELL!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//disabling servo's to prevent EMC error:
bool servosWereEnabled = (servoB.getEnabledState()||servoC.getEnabledState());
if(servosWereEnabled){
servoB.disable();
servoC.disable ();
}
// measuring the average value -> looping
double smoothedAvg = foilTensionADC.getRawADC1 ();
for (int i = 0; i < 10000; i++)
{
float newMeasurement = foilTensionADC.getRawADC1 ();
smoothedAvg = smoothedAvg + (0.0001 * (newMeasurement - smoothedAvg));
vTaskDelay (2 / MS);
}
staticFoilTensionOffset = (float) smoothedAvg + 50;
printf ("foilTension_raw: %f, staticFoilTensionOffset set to: %f\n",foilTension_raw, staticFoilTensionOffset);
calibrateLoadcellRequested = false;
printf ("calibrated the loadcell:\n");
if (servosWereEnabled)
{
servoB.enable ();
servoC.enable ();
}
}
static int i = 0;
if(i>100){
i=0;
static float Kp=0, Ki=0, Kd=0;
if((Kp != foilTensionPidTunings.Kp)|| (Ki != foilTensionPidTunings.Ki) || (Kd != foilTensionPidTunings.Kd)){
Kp = foilTensionPidTunings.Kp;
Ki = foilTensionPidTunings.Ki;
Kd = foilTensionPidTunings.Kd;
foilTensionPID.SetTunings(Kp, Ki,Kd);
printf("Changed the PI Tunings\n");
std::cout<<"Kp:" << Kp << "\n";
}
}
i++;
foilTension_corrected = foilTension_raw - staticFoilTensionOffset;
foilTensionPID.Compute();
// motorCSpeedSetpointRPM = motorC_PID_SpeedTarget_RPM;
// motorBSpeedSetpointRPM = motorC_PID_SpeedTarget_RPM/foilStretchFactor;
float runningAvgPIDSetpoint=floatRunningAverage(motorC_PID_SpeedTarget_RPM);
static float smoothedMotorSpeed = 0;
smoothedMotorSpeed = smoothedMotorSpeed + (0.2 * (runningAvgPIDSetpoint-smoothedMotorSpeed));
if(smoothedMotorSpeed<4) smoothedMotorSpeed = 0;
motorCSpeedSetpointRPM = smoothedMotorSpeed;
motorBSpeedSetpointRPM = motorCSpeedSetpointRPM/foilStretchFactor; //motorC_PID_SpeedTarget_RPM
#ifdef PRINT_FOIL_TENSION_MOTORSPEED_SLOW
printSec("foilTension_raw",foilTension_raw);
printSec("foilTension_corrected",foilTension_corrected);
printSec("motorC_PID_SpeedTarget_RPM",motorC_PID_SpeedTarget_RPM);
#endif
#ifdef ONLY_PRINT_FAST_WITH_BUTTON1_ENABLED
if(button1_toggled == true){
#ifdef PRINT_FOIL_TENSION_AND_CORRECTED_FAST
std::cout<<foilTension_raw<<"\t"<<foilTension_corrected<<"\n";
#endif
#ifdef PRINT_FOIL_TENSION_MOTORSPEED_FAST
//std::cout<<foilTension_corrected<< "\t" << motorCSpeedSetpointRPM << "\t" << motorBSpeedSetpointRPM << "\n";
std::cout<<foilTension_corrected<< "\t" << motorCSpeedSetpointRPM << "\n";
#endif
}
#endif
/*
*/
}
//**********************************************************************************
//**********************************************************************************
// Timer Functions
static void setupTimers(void) {
esp_timer_create_args_t periodic_timer_args;
periodic_timer_args.callback = &periodic_timer_callback;
periodic_timer_args.name = "periodic";
esp_timer_handle_t periodic_timer;
ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &periodic_timer));
/* The timer has been created but is not running yet */
/* Start the timers */
ESP_ERROR_CHECK(esp_timer_start_periodic(periodic_timer, 1000));
ESP_LOGI("TIMER:", "Started timers, time since boot: %lld us", esp_timer_get_time());
}
static void periodic_timer_callback(void* arg) { //updates every 1 ms
#ifdef USE_MPU
readMPU();
#endif
motorTask();
updateADC();
foilTensionTask();
}
//**********************************************************************************
//**********************************************************************************
// BlueTooth Functions
//
// This is the bt_spp_vfs_acceptor. It can create servers, wait for connected and receive data.
// run this one, the bt_spp_vfs_initiator will automatically connect the bt_spp_vfs_acceptor.
//
static void spp_read_handle (void * param)
{
std::string receivedMessage = "";
int size = 0;
int fd = (int) param;
do
{
if (!wifiEnabled) //if NO_SIGNAL on GPIOxx -> read bluetooth data
{
size = read (fd, spp_data, 64);
bool dataLineComplete =false;
if (size >0) {
receivedMessage = (char*)spp_data;
// std::cout<<receivedMessage<<"\n";
if(!(receivedMessage.find('\n')>0)){
receivedSerialBTString +=receivedMessage ;
// printf("found no 'n'\n");
}
else{
receivedSerialBTString = receivedMessage;
dataLineComplete = true;
}
if(receivedSerialBTString.length()>150) {
receivedSerialBTString= "";
// std::cout<<"restarting string\n";
}
//std::cout<<"size="<<size<<"\n";
}
//receivedSerialBTString +=spp_data[0];
if(dataLineComplete){
int startpos, endpos;
std::string valStr = "";
int estopReleased=false;
int button2=false;
long relPos=0;
int turns=0;
int speed=0;
int32_t redundancyCheckReceived = 0;
int32_t redundancyCheckSum = -1;
// std::cout<<"working with:" <<receivedSerialBTString<<"\n";
startpos = receivedSerialBTString.find("es:")+3;
endpos = receivedSerialBTString.find("bn:",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos-1);
if(startpos>0) estopReleased = (bool)atoi(valStr.c_str());
startpos = endpos+3; //this startpoint is 3 indexes further than the previous endpoint
endpos = receivedSerialBTString.find("rP:",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos-1);
button2 = (bool)atoi(valStr.c_str());
if(startpos>0) button2 = (bool)atoi(valStr.c_str());
startpos = endpos+3; //this startpoint is 3 indexes further than the previous endpoint
endpos = receivedSerialBTString.find("tu:",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos-1);
if(startpos>0) relPos = atol(valStr.c_str());
startpos = endpos+3; //this startpoint is 3 indexes further than the previous endpoint
endpos = receivedSerialBTString.find("sp:",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos-1);
if(startpos>0) turns = atoi(valStr.c_str());
startpos = endpos+3; //this startpoint is 3 indexes further than the previous endpoint
endpos = receivedSerialBTString.find("rc:",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos-1);
if(startpos>0) speed =atoi(valStr.c_str());
startpos = endpos+3; //this startpoint is 3 indexes further than the previous endpoint
endpos = receivedSerialBTString.find("*",startpos);
valStr = receivedSerialBTString.substr(startpos,endpos-startpos);
if(startpos>0) redundancyCheckReceived =atol(valStr.c_str());
#ifdef PRINT_BT_DATA_FAST
std::cout<< "es_"<<estopReleased<<" bn_" << button2 << " rP_" <<relPos<< " tu_" << turns << " sp_" << speed << " rc_"<< redundancyCheckReceived <<"\n";
#endif
//"micros_" << esp_timer_get_time() <<
redundancyCheckSum = (estopReleased + button2 + relPos + turns + speed+7);
if((redundancyCheckReceived == redundancyCheckSum)){
//redundancyCheck OK -> write temp values to global variables!
absRingPosition = 360000*turns+ relPos;
relativeRingPosition = relPos;
ringTurns = turns;
ringSpeed = speed;
estopActive = !estopReleased;
}
else{
// ESP_LOGE("BT_COMM","redundancyCheck Failed!");
}
// std::cout<<"relPos_" <<relPos<< "\n";
// std::cout<<receivedSerialBTString<<"\n";
// // receivedSerialBTString = receivedSerialBTString + receivedBTChar;
// if (receivedSerialBTString.rfind("estop", 0) == 0) {
// receivedSerialBTString = "";
// continue;
// }
// std::cout<< "Unknown: " << receivedSerialBTString << "\n";
receivedSerialBTString = "";
continue;
}
}
else{
printf("wifi enabled from BT task \n");
vTaskDelay(5000/MS);
size = read (fd, spp_data, SPP_DATA_LENGTH);
}
}
while (1);
spp_wr_task_shut_down ();
}
// if (spp_data[0] == '^')
// { //start of line '^', 0x5e 94(dec)
// receivedIndex = 0;
// for (int i = 0; i < VALUE_LENGTH; i++)
// {
// receivedValue[i] = 0;
// }
// }
// else if (spp_data[0] == '!')
// { //BT Estop character
// servosEnabled =false;
// printf("BT Estop Triggered\n");
// printf("wtfuk2\n");
// }
// else if (spp_data[0] == '~')
// { //BT servosEnabled character
// servosEnabled = true;
// printf ("BT StartServo\n");
// printf("wtfuk\n");
// }
// else if (spp_data[0] == 0x2a)
// { //'*'
// int startingIndex = 0;
// for (int i = 0; i < 4; i++)
// {
// abs_pos._byte[i] = receivedValue[i + startingIndex];
// // printf("abs_pos byte %d, value %d=%d\n",i,abs_pos._byte[i],receivedValue[i+startingIndex]);
// }
// startingIndex = 4;
// for (int i = 0; i < 4; i++)
// {
// rel_pos._byte[i] = receivedValue[i + startingIndex];
// }
// startingIndex = 8;
// for (int i = 0; i < 4; i++)
// {
// turns._byte[i] = receivedValue[i + startingIndex];
// }
// startingIndex = 12;
// for (int i = 0; i < 4; i++)
// {
// speed._byte[i] = receivedValue[i + startingIndex];
// }
//
//#ifdef PRINT_ELAPSED_BT_TIME
// static int64_t prevCurrentMicros = esp_timer_get_time();
// int64_t currentMicros = esp_timer_get_time();
// int elapsedTime =currentMicros-prevCurrentMicros;
// printf("%d\n",elapsedTime);
// prevCurrentMicros = currentMicros;
//#endif
//#ifdef PRINT_BT_DATA_FAST
// printf ("%d %d %d %f\n", abs_pos._int32, rel_pos._int32,
// turns._int32, speed._float);
//#endif
//#ifdef PRINT_BT_DATA_SLOW
// // printf("Abs_pos = %d\n", abs_pos._int32);
// // printf("Rel_pos = %d\n", rel_pos._int32);
// // printf("Turns = %d\n", turns._int32);
// //printf("Speed = %f\n", speed._float);
// static int32_t prevMillis = millis();
// int milli = millis();
// if((milli-1000)>prevMillis){
// prevMillis = milli;
// printf("%d %d %d %f\n", abs_pos._int32, rel_pos._int32,turns._int32,speed._float);
// }
//#endif
// // for(int i = 0;i<16;i++) printf("i:%d,byte:%d\n",i,receivedValue[i]);
// }
// else if (receivedIndex > (VALUE_LENGTH + 2))
// {
// receivedIndex = 0;
// }
// else
// {
// receivedValue[receivedIndex] = spp_data[0];
// receivedIndex++;
// }
//
// // esp_log_buffer_hex(SPP_TAG, spp_data, size);
//
// }
//
// // if (size == 0) {
// // //read fail due to there is no data, retry after 1s
// // }
void spp_write(const char* command) {
int size = 0;
uint8_t data[VALUE_LENGTH];
//const char* source1 = "resetPos";
esp_log_buffer_hex(SPP_TAG, command, 16);
size = write (btFd, command, VALUE_LENGTH);
if(size>=1){
printf("Successfully written data to btFd: %d\n",btFd);
}
if (size == -1)
{
}
else if (size == 0)
{
/*write fail due to ringbuf is full, retry after 1ms //NORMALLY 10000/portTICK_PERIOD_MS*/
//vTaskDelay((uint) (1 / portTICK_PERIOD_MS));
}
}
static void esp_spp_cb (uint16_t e, void *p)
{
esp_spp_cb_event_t event = (esp_spp_cb_event_t) e;
esp_spp_cb_param_t *param = (esp_spp_cb_param_t*) p;
switch (event)
{
case ESP_SPP_INIT_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_INIT_EVT");
esp_bt_dev_set_device_name (EXCAMPLE_DEVICE_NAME);
esp_bt_gap_set_scan_mode (ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
esp_spp_start_srv (sec_mask, role_slave, 0, SPP_SERVER_NAME);
break;
case ESP_SPP_DISCOVERY_COMP_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_DISCOVERY_COMP_EVT");
break;
case ESP_SPP_OPEN_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_OPEN_EVT");
break;
case ESP_SPP_CLOSE_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_CLOSE_EVT");
btConnectionOpen = false;
break;
case ESP_SPP_START_EVT: // When the server is started successfully, the callback is called with ESP_SPP_START_EVT.
ESP_LOGI(SPP_TAG, "ESP_SPP_START_EVT");
ESP_LOGD(SPP_TAG, "Server started, still waiting for connection");
//waiting for statical ESP to make a pair.
break;
case ESP_SPP_CL_INIT_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_CL_INIT_EVT");
break;
case ESP_SPP_SRV_OPEN_EVT:
ESP_LOGI(SPP_TAG, "ESP_SPP_SRV_OPEN_EVT"); //When the connection is established, the callback is called with ESP_SPP_SRV_OPEN_EVT.
ESP_LOGD(SPP_TAG, "connection is made with my buddy");
btConnectionOpen = true;
spp_wr_task_start_up (spp_read_handle, param->srv_open.fd);
btFd = param->srv_open.fd;
break;
default:
break;
}
}
static void esp_spp_stack_cb (esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
{
spp_task_work_dispatch (esp_spp_cb, event, param, sizeof(esp_spp_cb_param_t),
NULL);
}
void esp_bt_gap_cb (esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
{
switch (event)
{
case ESP_BT_GAP_AUTH_CMPL_EVT:
{
if (param->auth_cmpl.stat == ESP_BT_STATUS_SUCCESS)
{
ESP_LOGI(SPP_TAG, "authentication success: %s",
param->auth_cmpl.device_name);
esp_log_buffer_hex(SPP_TAG, param->auth_cmpl.bda, ESP_BD_ADDR_LEN);
}
else
{
ESP_LOGE(SPP_TAG, "authentication failed, status:%d",
param->auth_cmpl.stat);
}
break;
}
case ESP_BT_GAP_PIN_REQ_EVT:
{
ESP_LOGI(SPP_TAG, "ESP_BT_GAP_PIN_REQ_EVT min_16_digit:%d",
param->pin_req.min_16_digit);
if (param->pin_req.min_16_digit)
{
ESP_LOGI(SPP_TAG, "Input pin code: 0000 0000 0000 0000");
esp_bt_pin_code_t pin_code =
{ 0 };
esp_bt_gap_pin_reply (param->pin_req.bda, true, 16, pin_code);
}
else
{
ESP_LOGI(SPP_TAG, "Input pin code: 1234");
esp_bt_pin_code_t pin_code;
pin_code[0] = '1';
pin_code[1] = '2';
pin_code[2] = '3';
pin_code[3] = '4';
esp_bt_gap_pin_reply (param->pin_req.bda, true, 4, pin_code);
}
break;
}
case ESP_BT_GAP_CFM_REQ_EVT:
ESP_LOGI(SPP_TAG,
"ESP_BT_GAP_CFM_REQ_EVT Please compare the numeric value: %d",
param->cfm_req.num_val);
esp_bt_gap_ssp_confirm_reply (param->cfm_req.bda, true);
break;
case ESP_BT_GAP_KEY_NOTIF_EVT:
ESP_LOGI(SPP_TAG, "ESP_BT_GAP_KEY_NOTIF_EVT passkey:%d",
param->key_notif.passkey);
break;
case ESP_BT_GAP_KEY_REQ_EVT:
ESP_LOGI(SPP_TAG, "ESP_BT_GAP_KEY_REQ_EVT Please enter passkey!");
break;
default:
{
ESP_LOGI(SPP_TAG, "event: %d", event);
break;
}
}
return;
}
static void setupBluetooth(){
esp_err_t ret = nvs_flash_init ();
Bt_ServerState = INITIALIZING_SERVER;
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_ERROR_CHECK(nvs_flash_erase ());
ret = nvs_flash_init ();
}
ESP_ERROR_CHECK(ret);
ESP_ERROR_CHECK(esp_bt_controller_mem_release (ESP_BT_MODE_BLE));
esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT()
;
if (esp_bt_controller_init (&bt_cfg) != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s initialize controller failed", __func__);
return;
}
if (esp_bt_controller_enable (ESP_BT_MODE_CLASSIC_BT) != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s enable controller failed", __func__);
return;
}
if (esp_bluedroid_init () != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s initialize bluedroid failed", __func__);
return;
}
if (esp_bluedroid_enable () != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s enable bluedroid failed", __func__);
return;
}
if (esp_bt_gap_register_callback (esp_bt_gap_cb) != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s gap register failed: %s\n", __func__,
esp_err_to_name (ret));
return;
}
if (esp_spp_register_callback (esp_spp_stack_cb) != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s spp register failed", __func__);
return;
}
esp_spp_vfs_register ();
spp_task_task_start_up ();
Bt_ServerState = WAITING_FOR_PARTNER;
if (esp_spp_init (esp_spp_mode) != ESP_OK)
{
ESP_LOGE(SPP_TAG, "%s spp init failed", __func__);
return;
}
/* Set default parameters for Secure Simple Pairing */
esp_bt_sp_param_t param_type = ESP_BT_SP_IOCAP_MODE;
esp_bt_io_cap_t iocap = ESP_BT_IO_CAP_IO;
esp_bt_gap_set_security_param (param_type, &iocap, sizeof(uint8_t));
/*
* Set default parameters for Legacy Pairing
* Use variable pin, input pin code when pairing
*/
esp_bt_pin_type_t pin_type = ESP_BT_PIN_TYPE_VARIABLE;
esp_bt_pin_code_t pin_code;
esp_bt_gap_set_pin (pin_type, 0, pin_code);
/*
i2c_master_init();
xTaskCreatePinnedToCore(ads111x_test, "ads111x_test", configMINIMAL_STACK_SIZE * 8, NULL, 5, NULL, APP_CPU_NUM);
*/
}
//**********************************************************************************
//**********************************************************************************
// Console Functions
//
#ifdef CONSOLE_INPUT
static void initialize_console()
{
/* Disable buffering on stdin and stdout */
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
// Minicom, screen, idf_monitor send CR when ENTER key is pressed //
esp_vfs_dev_uart_set_rx_line_endings(ESP_LINE_ENDINGS_CR);
// Move the caret to the beginning of the next line on '\n' //
esp_vfs_dev_uart_set_tx_line_endings(ESP_LINE_ENDINGS_CRLF);
// Install UART driver for interrupt-driven reads and writes /
ESP_ERROR_CHECK( uart_driver_install((uart_port_t)CONFIG_CONSOLE_UART_NUM,
256, 0, 0, NULL, 0) );
/* Tell VFS to use UART driver */
esp_vfs_dev_uart_use_driver(CONFIG_CONSOLE_UART_NUM);
/* Initialize the console */
esp_console_config_t console_config;
console_config.max_cmdline_args = 8;
console_config.max_cmdline_length = 256;
ESP_ERROR_CHECK(esp_console_init(&console_config));
/* Configure linenoise line completion library */
/* Enable multiline editing. If not set, long commands will scroll within
* single line.
*/
linenoiseSetMultiLine(1);
/* Tell linenoise where to get command completions and hints */
linenoiseSetCompletionCallback(&esp_console_get_completion);
linenoiseSetHintsCallback((linenoiseHintsCallback*) &esp_console_get_hint);
/* Set command history size */
linenoiseHistorySetMaxLen(100);
}
void consoleInput_loop(void *pvParameter) {
//initialize_console();
/* Register commands */
esp_console_register_help_command();
register_cmd();
register_inputs (&foilTensionPidTunings.Kp, &foilTensionPidTunings.Ki,
&foilTensionPidTunings.Kd, &foilStretchFactor,
&foilTensionSetpoint, &calibrateLoadcellRequested,
&resetDrivesRequested, &servosEnabled,
&resetPositionRequested);
/* Prompt to be printed before each line.
* This can be customized, made dynamic, etc.
*/
const char* prompt = LOG_COLOR_I "esp32> " LOG_RESET_COLOR;
printf("\n"
"Type 'help' to get the list of commands.\n"
"Use UP/DOWN arrows to navigate through command history.\n"
"Press TAB when typing command name to auto-complete.\n");
/* Figure out if the terminal supports escape sequences */
int probe_status = linenoiseProbe();
if (probe_status) { /* zero indicates success */
printf("\n"
"Your terminal application does not support escape sequences.\n"
"Line editing and history features are disabled.\n"
"On Windows, try using Putty instead.\n");
linenoiseSetDumbMode(1);
}
while (1) {
/* Get a line using linenoise.
* The line is returned when ENTER is pressed.
*/
char* line = linenoise(prompt);
if (line == NULL) { /* Ignore empty lines */
continue;
}
/* Add the command to the history */
linenoiseHistoryAdd(line);
/* Try to run the command */
int ret;
esp_err_t err = esp_console_run(line, &ret);
if (err == ESP_ERR_NOT_FOUND) {
printf("Unrecognized command\n");
} else if (err == ESP_ERR_INVALID_ARG) {
// command was empty
} else if (err == ESP_OK && ret != ESP_OK) {
printf("Command returned non-zero error code: 0x%x\n", ret);
} else if (err != ESP_OK) {
printf("Internal error: 0x%x\n", err);
}
/* linenoise allocates line buffer on the heap, so need to free it */
linenoiseFree(line);
}
}
#endif
//**********************************************************************************
//**********************************************************************************
// WiFi Functions
/* Wi-Fi event handler */
static esp_err_t event_handler(void *ctx, system_event_t *event)
{
switch(event->event_id) {
case SYSTEM_EVENT_STA_START:
ESP_LOGI(WIFI_TAG, "SYSTEM_EVENT_STA_START");
ESP_ERROR_CHECK(esp_wifi_connect());
break;
case SYSTEM_EVENT_STA_GOT_IP:
ESP_LOGI(WIFI_TAG, "SYSTEM_EVENT_STA_GOT_IP");
ESP_LOGI(WIFI_TAG, "Got IP: '%s'",
ip4addr_ntoa(&event->event_info.got_ip.ip_info.ip));
break;
case SYSTEM_EVENT_STA_DISCONNECTED:
ESP_LOGI(WIFI_TAG, "SYSTEM_EVENT_STA_DISCONNECTED");
ESP_ERROR_CHECK(esp_wifi_connect());
break;
default:
break;
}
return ESP_OK;
}
/* Function to initialize Wi-Fi at station */
static void initialise_wifi(void)
{
ESP_ERROR_CHECK(nvs_flash_init());
tcpip_adapter_init();
ESP_ERROR_CHECK(esp_event_loop_init(event_handler, NULL));
cfg = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&cfg));
ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
//Allocate storage for the struct
wifi_config_t wifi_config = {};
//Assign ssid & password strings
// strcpy((char*)wifi_config.ap.ssid, "ESP32");
// strcpy((char*)wifi_config.ap.password, "ESP32");
// wifi_config.ap.beacon_interval = 6000;
// wifi_config.ap.channel = 7;
// wifi_config.ap.max_connection = 2;
// ESP_LOGI(WIFI_TAG, "Setting WiFi configuration SSID %s...", wifi_config.ap.ssid);
// ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
// ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &wifi_config));
strcpy((char*)wifi_config.sta.ssid, "PSK_GENT_LiTL");
strcpy((char*)wifi_config.sta.password, "0UtQ}fM0T]RjRC{}");
ESP_LOGI(WIFI_TAG, "Connecting to WiFi configuration SSID %s...", wifi_config.sta.ssid);
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config));
// ESP_ERROR_CHECK(esp_wifi_start());
}
/* Function to initialize SPIFFS */
static esp_err_t init_spiffs(void)
{
ESP_LOGI(WIFI_TAG, "Initializing SPIFFS");
esp_vfs_spiffs_conf_t conf = {
.base_path = "/spiffs",
.partition_label = NULL,
.max_files = 5, // This decides the maximum number of files that can be created on the storage
.format_if_mount_failed = true
};
esp_err_t ret = esp_vfs_spiffs_register(&conf);
if (ret != ESP_OK) {
if (ret == ESP_FAIL) {
ESP_LOGE(WIFI_TAG, "Failed to mount or format filesystem");
} else if (ret == ESP_ERR_NOT_FOUND) {
ESP_LOGE(WIFI_TAG, "Failed to find SPIFFS partition");
} else {
ESP_LOGE(WIFI_TAG, "Failed to initialize SPIFFS (%s)", esp_err_to_name(ret));
}
return ESP_FAIL;
}
size_t total = 0, used = 0;
ret = esp_spiffs_info(NULL, &total, &used);
if (ret != ESP_OK) {
ESP_LOGE(WIFI_TAG, "Failed to get SPIFFS partition information (%s)", esp_err_to_name(ret));
return ESP_FAIL;
}
ESP_LOGI(WIFI_TAG, "Partition size: total: %d, used: %d", total, used);
return ESP_OK;
}
/* Declare the function which starts the file server.
* Implementation of this function is to be found in
* file_server.c */
esp_err_t start_file_server(const char *base_path);
#define FILEREAD_TAG "TAG"
void read_profile_into_profileStrainTension(){ //
// Open file for reading
//ESP_LOGI(FILEREAD_TAG, "Reading file");
FILE* f = fopen (PROFILE_PATH, "r");
if (f == NULL)
{
// ESP_LOGE(FILEREAD_TAG, "Failed to open file for reading");
printf ("Failed to open the file for reading\n");
return;
}
//read file into array
int i;
if (f == NULL)
{
printf ("Error Reading File\n");
exit (0);
}
int filledLength = -1;
for (i = 0; i <= 360; i++)
{
if ((fscanf (f, "%f,%f,", &profileStrainTension[i][0], &profileStrainTension[i][1])) != 2)
{ //!=2 indicates that there are not two variables read into the array.
printf ("end of file reached at index %d\n", i);
break;
}
filledLength=i;
if(i==360) printf("read successful\n");
}
#ifdef PRINT_CSV_PROFILE_AT_STARTUP
for (i = 0; i <= filledLength; i++)
{
printf ("%f \t %f\n", profileStrainTension[i][0],
profileStrainTension[i][1]);
}
#endif
}
//
//**********************************************************************************************************************************//
//**********************************************************************************************************************************//
// MAIN
//**********************************************************************************************************************************//
//**********************************************************************************************************************************//
#define MWIDTH 100
#define MHEIGHT 100
void
app_main (void)
{
initialise_wifi();
// Initialize file storage
ESP_ERROR_CHECK(init_spiffs());
// Start the file server
ESP_ERROR_CHECK(start_file_server("/spiffs"));
#ifdef CONSOLE_INPUT
initialize_console ();
#endif
// pwm_init(); //No PWM is being used, only here for future reference
dac_init();
_gpio_init();
servomotors_init();
i2c_master_init(); //initialized in mpu_init
setupBluetooth();
setupPID();
#ifdef USE_MPU
mpu_init();
#endif
xTaskCreatePinnedToCore (&digitalInputs, "digitalInputs", configMINIMAL_STACK_SIZE*4, NULL, 6, NULL, APP_CPU_NUM);
xTaskCreatePinnedToCore (&otherLowPriorityTasks, "otherLowPriorityTasks", configMINIMAL_STACK_SIZE*4, NULL, 6, NULL, APP_CPU_NUM);
setupTimers(); //running MPU task at 1kHz
read_profile_into_profileStrainTension(); //read the profile in the CSV-file profile.csv of SPIFFS into the profileStrainTension array
#ifdef CONSOLE_INPUT
xTaskCreatePinnedToCore (&consoleInput_loop, "consoleInput_loop",
configMINIMAL_STACK_SIZE * 8, NULL, 6, NULL,
APP_CPU_NUM);
#endif
}
//**********************************************************************************************************************************//
//************************************* END OF MAIN ****************************************************//
//**********************************************************************************************************************************//