From ad0723a766413334c8b136db8a1272e9f605b78d Mon Sep 17 00:00:00 2001 From: jonny_ji7 Date: Mon, 25 Jul 2022 11:02:52 +0200 Subject: [PATCH] Update MASSAGE: 4 modes (each quadrant) with hysteresis Modified function that generates commands in massage mode (joystick.cpp) - define variable stickQuadrant with axis as hysteresis from actual joystick data/pos - prepare switch case for implementation of 4 modes (each quadrant) - add basic content to modes for testing (different motor directions) TODO: remove old code with pulsing shake variable and implement actual modes --- main/joystick.cpp | 133 +++++++++++++++++++++++++++++++--------------- 1 file changed, 91 insertions(+), 42 deletions(-) diff --git a/main/joystick.cpp b/main/joystick.cpp index fa4bd12..27c709f 100644 --- a/main/joystick.cpp +++ b/main/joystick.cpp @@ -376,6 +376,9 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data){ uint32_t shake_timestamp_turnedOn = 0; uint32_t shake_timestamp_turnedOff = 0; bool shake_state = false; +joystickPos_t lastStickPos = joystickPos_t::CENTER; +//stick position quadrant only with "X_AXIS and Y_AXIS" as hysteresis +joystickPos_t stickQuadrant = joystickPos_t::CENTER; //--- configure shake mode --- TODO: move this to config uint32_t shake_msOffMax = 90; @@ -385,25 +388,17 @@ float dutyShake = 60; //function that generates commands for both motors from the joystick data motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ - - //struct with current data of the joystick - //typedef struct joystickData_t { - // joystickPos_t position; - // float x; - // float y; - // float radius; - // float angle; - //} joystickData_t; - - + //--- handle pulsing shake variable --- + //TODO remove this, make individual per mode? + //TODO only run this when not CENTER anyways? motorCommands_t commands; float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0 - //--- calculate on/off duration --- + //calculate on/off duration uint32_t msOn = shake_msOnMax * data.radius; uint32_t msOff = shake_msOffMax * data.radius; - //--- evaluate state (on/off) --- + //evaluate state (on/off) if (data.radius > 0 ){ //currently off if (shake_state == false){ @@ -433,44 +428,97 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ + //struct with current data of the joystick + //typedef struct joystickData_t { + // joystickPos_t position; + // float x; + // float y; + // float radius; + // float angle; + //} joystickData_t; - if (shake_state){ - switch (data.position){ - default: - commands.left.state = motorstate_t::IDLE; - commands.right.state = motorstate_t::IDLE; - commands.left.duty = 0; - commands.right.duty = 0; - break; + //--- evaluate stick position --- + //4 quadrants and center only - with X and Y axis as hysteresis + switch (data.position){ - case joystickPos_t::Y_AXIS: + case joystickPos_t::CENTER: + //immediately set to center at center + stickQuadrant = joystickPos_t::CENTER; + break; + + case joystickPos_t::Y_AXIS: + //when moving from center to axis initially start in a certain quadrant + if (stickQuadrant == joystickPos_t::CENTER) { if (data.y > 0){ - commands.left.state = motorstate_t::FWD; - commands.right.state = motorstate_t::FWD; + stickQuadrant = joystickPos_t::TOP_RIGHT; } else { - commands.left.state = motorstate_t::REV; - commands.right.state = motorstate_t::REV; + stickQuadrant = joystickPos_t::BOTTOM_RIGHT; } - //set duty to shake - commands.left.duty = dutyShake; - commands.right.duty = dutyShake; - break; + } - case joystickPos_t::X_AXIS: - if (data.x > 0) { - commands.left.state = motorstate_t::FWD; - commands.right.state = motorstate_t::REV; + case joystickPos_t::X_AXIS: + //when moving from center to axis initially start in a certain quadrant + if (stickQuadrant == joystickPos_t::CENTER) { + if (data.x > 0){ + stickQuadrant = joystickPos_t::TOP_RIGHT; } else { - commands.left.state = motorstate_t::REV; - commands.right.state = motorstate_t::FWD; + stickQuadrant = joystickPos_t::TOP_LEFT; } - //set duty to shake - commands.left.duty = dutyShake; - commands.right.duty = dutyShake; - break; - } - } else { //shake state off + } + break; + + case joystickPos_t::TOP_RIGHT: + case joystickPos_t::TOP_LEFT: + case joystickPos_t::BOTTOM_LEFT: + case joystickPos_t::BOTTOM_RIGHT: + //update/change evaluated pos when in one of the 4 quadrants + stickQuadrant = data.position; + //TODO: maybe beep when switching mode? (difficult because beep object has to be passed to function) + break; + } + + + + //--- handle different modes (joystich in any of 4 quadrants) --- + switch (stickQuadrant){ + case joystickPos_t::CENTER: + case joystickPos_t::X_AXIS: //never true + case joystickPos_t::Y_AXIS: //never true + commands.left.state = motorstate_t::IDLE; + commands.right.state = motorstate_t::IDLE; + commands.left.duty = 0; + commands.right.duty = 0; + ESP_LOGI(TAG_CMD, "generate shake commands: CENTER -> idle"); + return commands; + break; + //4 different modes + case joystickPos_t::TOP_RIGHT: + commands.left.state = motorstate_t::FWD; + commands.right.state = motorstate_t::FWD; + break; + case joystickPos_t::TOP_LEFT: + commands.left.state = motorstate_t::FWD; + commands.right.state = motorstate_t::REV; + break; + case joystickPos_t::BOTTOM_LEFT: + commands.left.state = motorstate_t::REV; + commands.right.state = motorstate_t::FWD; + break; + case joystickPos_t::BOTTOM_RIGHT: + commands.left.state = motorstate_t::REV; + commands.right.state = motorstate_t::REV; + break; + } + + + //--- turn motors on/off depending on pulsing shake variable --- + if (shake_state == true){ + //set duty to shake + commands.left.duty = dutyShake; + commands.right.duty = dutyShake; + //directions are defined above depending on mode + } else { commands.left.state = motorstate_t::IDLE; commands.right.state = motorstate_t::IDLE; commands.left.duty = 0; @@ -482,5 +530,6 @@ motorCommands_t joystick_generateCommandsShaking(joystickData_t data){ joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); + return commands; }