From 8b9b5ff736aad4bf19dfceb6f6ed0b6d299a51d1 Mon Sep 17 00:00:00 2001 From: jonny_ji7 Date: Thu, 18 Aug 2022 14:23:32 +0200 Subject: [PATCH] Add control statemachine, optimize vfd logging Add all logic for the machine to control.cpp optimize logging (on change only) for vfd functions --- main/control.cpp | 127 +++++++++++++++++++++++++++++++++++++++-------- main/control.hpp | 2 + main/vfd.cpp | 38 +++++++++++--- main/vfd.hpp | 4 +- 4 files changed, 141 insertions(+), 30 deletions(-) diff --git a/main/control.cpp b/main/control.cpp index 3402f77..0b21791 100644 --- a/main/control.cpp +++ b/main/control.cpp @@ -72,30 +72,51 @@ int readAdc(adc1_channel_t adc_channel, bool inverted = false) { + + //==================== //==== variables ===== //==================== static const char *TAG = "control"; + +const char* systemStateStr[4] = {"COUNTING", "WINDING_START", "WINDING", "TARGET_REACHED"}; +systemState_t controlState = COUNTING; + +max7219_t display; //display device char buf_disp[20]; //both displays char buf_disp1[10];// 8 digits + decimal point + \0 char buf_disp2[10];// 8 digits + decimal point + \0 char buf_tmp[15]; -max7219_t display; //display device -QueueHandle_t encoder_queue = NULL; //encoder event queue + rotary_encoder_info_t encoder; //encoder device/info +QueueHandle_t encoder_queue = NULL; //encoder event queue rotary_encoder_state_t encoderState; uint8_t count = 0; //count for testing uint32_t timestamp_pageSwitched = 0; bool page = false; //store page number currently displayed -bool state = false; //store state of motor int lengthNow = 0; //length measured in mm int lengthTarget = 3000; //target length in mm +int lengthDiff = 0; //length difference int potiRead = 0; //voltage read from adc +uint32_t timestamp_motorStarted = 0; //timestamp winding started -systemState_t controlState = COUNTING; +//===== change State ===== +//function for changing the controlState +void changeState (systemState_t stateNew) { + //only proceed when state actually changed + if (controlState == stateNew){ + //already at target state -> nothing to do + return; + } + //log change + ESP_LOGW(TAG, "changed state from %s to %s", systemStateStr[(int)controlState], systemStateStr[(int)stateNew]); + //change state + controlState = stateNew; +} + //======================== //===== control task ===== @@ -149,8 +170,7 @@ void task_control(void *pvParameter) // Poll current position and direction rotary_encoder_get_state(&encoder, &encoderState); //--- calculate distance --- - //lengthNow = encoderState.position * (MEASURING_ROLL_DIAMETER * PI); //TODO dont calculate constant factor every time FIXME: ROUNDING ISSUE float-int? - lengthNow += 155; + lengthNow = (float)encoderState.position * (MEASURING_ROLL_DIAMETER * PI); //TODO dont calculate constant factor every time FIXME: ROUNDING ISSUE float-int? //--------------------------- @@ -165,6 +185,7 @@ void task_control(void *pvParameter) //round to whole meters lengthTarget = round(lengthTarget / 1000) * 1000; //TODO update lengthTarget only at button release? + //TODO beep for each m step? } //beep if (SW_SET.risingEdge) { @@ -175,27 +196,95 @@ void task_control(void *pvParameter) } + + //--------------------------- + //--------- buttons --------- + //--------------------------- //TODO: ADD PRESET SWITCHES HERE + //--- RESET switch --- + if (SW_RESET.risingEdge) { + rotary_encoder_reset(&encoder); + lengthNow = 0; + buzzer.beep(1, 700, 100); + } - //-------------------------------- - //--------- statemachine --------- - //-------------------------------- + + //--------------------------- + //--------- control --------- + //--------------------------- + //calculate length difference + lengthDiff = lengthNow - lengthTarget; + + //--- stop conditions --- + //stop conditions that are checked in any mode + //disable motor and switch to COUNTING when start button released + if (SW_START.state == false) { //TODO use fallingEdge here more clean? + changeState(COUNTING); + vfd_setState(false); + } + //disable motor and switch to TARGET_REACHED + else if (lengthDiff >= 0 ) { + //TODO: display "REACHED" on 7segment here or reached state (start pressed but reached) + changeState(TARGET_REACHED); + vfd_setState(false); + } + + //--- statemachine --- switch (controlState) { - case COUNTING: - + case COUNTING: //no motor action + //TODO stop motor here every run instead of at button event? + if (SW_START.risingEdge) { + changeState(WINDING_START); + vfd_setSpeedLevel(2); //start at low speed + vfd_setState(true); //start motor + timestamp_motorStarted = esp_log_timestamp(); //save time started + } break; - case WINDING_START: - + case WINDING_START: //wind slow for certain time + //TODO: cancel / stay in this state when no change to lengthNow + if (esp_log_timestamp() - timestamp_motorStarted > 4000) { + changeState(WINDING); + } + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 2; break; - case WINDING: - + case WINDING: //wind at dynamic speed + lengthDiff = abs(lengthDiff); + //adjust speed according to difference + if (lengthDiff < 10) { + vfd_setSpeedLevel(1); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 1; + } else if (lengthDiff < 50) { + vfd_setSpeedLevel(2); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 4; + } else if (lengthDiff < 200) { + vfd_setSpeedLevel(3); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 6; + } else if (lengthDiff < 500) { + vfd_setSpeedLevel(4); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 50; + } else if (lengthDiff < 1000) { + vfd_setSpeedLevel(6); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 100; + } else { //more than last step + vfd_setSpeedLevel(7); + //TESTING: SIMULATE LENGTH INCREASE + //lengthNow += 200; + } + //see "stop conditions" above that switches to TARGET_REACHED when targed reached break; case TARGET_REACHED: - + //nothing to do here yet + //see "stop conditions" above that switches to COUNTING when start button released break; } @@ -259,12 +348,6 @@ void task_control(void *pvParameter) // buzzer.beep(1, 100, 100); //} - ////testing: toggle motor state - //if(SW_START.risingEdge){ - // state = !state; - // vfd_setState(state); - // buzzer.beep(1, 500, 100); - //} } } diff --git a/main/control.hpp b/main/control.hpp index 67dead7..632c8c6 100644 --- a/main/control.hpp +++ b/main/control.hpp @@ -23,5 +23,7 @@ extern "C" typedef enum systemState_t {COUNTING, WINDING_START, WINDING, TARGET_REACHED} systemState_t; +extern const char* systemStateStr[4]; + void task_control(void *pvParameter); diff --git a/main/vfd.cpp b/main/vfd.cpp index 376d05f..8173613 100644 --- a/main/vfd.cpp +++ b/main/vfd.cpp @@ -3,10 +3,23 @@ #define CHECK_BIT(var,pos) (((var)>>(pos)) & 1) static const char *TAG = "vfd"; +uint8_t level = 0; //current speed level +bool state = false; //current state +//============================= +//========= setState ========== +//============================= +void vfd_setState(bool stateNew){ + //only proceed and send log output when state is actually changed + if (state == stateNew) { + //already at target state -> nothing todo + return; + } + + //update stored state + state = stateNew; -void vfd_setState(bool state){ //turn motor on/off if (state == true) { gpio_set_level(GPIO_VFD_FWD, 1); @@ -15,14 +28,28 @@ void vfd_setState(bool state){ gpio_set_level(GPIO_VFD_FWD, 0); gpio_set_level(GPIO_RELAY, 0); } - ESP_LOGI(TAG, "set state to %i", (int)state); + ESP_LOGI(TAG, "CHANGED state to %i", (int)state); } -void vfd_setSpeedLevel(uint8_t level){ +//============================= +//======= setSpeedLevel ======= +//============================= +void vfd_setSpeedLevel(uint8_t levelNew){ //set speed level of VFD + //only proceed and send log output when level is actually changed + if (level == levelNew) { + //already at target level -> nothing todo + return; + } + + //log change + ESP_LOGI(TAG, "CHANGING speed level from %i to %i", level, levelNew); + //update stored level + level = levelNew; + //bit:2 1 0 //lvl D2 D1 D0 Hz //0 0 0 0 default @@ -67,7 +94,6 @@ void vfd_setSpeedLevel(uint8_t level){ gpio_set_level(GPIO_VFD_D2, 0); } - //log - ESP_LOGI(TAG, "Set level to %d", level); - ESP_LOGI(TAG, "pin state: D2=%i, D1=%i, D0=%i", (int)D2, (int)D1, (int)D0); + //log pin state + ESP_LOGI(TAG, " - pin state: D2=%i, D1=%i, D0=%i", (int)D2, (int)D1, (int)D0); } diff --git a/main/vfd.hpp b/main/vfd.hpp index e281a79..4180b80 100644 --- a/main/vfd.hpp +++ b/main/vfd.hpp @@ -13,7 +13,7 @@ extern "C" //function for setting the state (motor on/off) -void vfd_setState(bool state); +void vfd_setState(bool stateNew); //function for setting the speed level (1-7) -void vfd_setSpeedLevel(uint8_t level = 0); +void vfd_setSpeedLevel(uint8_t levelNew = 0);