From e9a88c18f68f25c0489476f87f682e7dcb15adea Mon Sep 17 00:00:00 2001
From: rusEfi <russianefi@gmail.com>
Date: Sun, 3 Mar 2019 15:27:49 -0500
Subject: [PATCH] ETB progress

---
 firmware/config/engines/custom_engine.cpp         |  3 +++
 firmware/controllers/electronic_throttle.cpp      | 15 ++++++++++++---
 firmware/controllers/system/DcMotor.cpp           |  8 ++++++--
 firmware/controllers/system/DcMotor.h             |  1 +
 .../controllers/system/pwm_generator_logic.cpp    |  3 ---
 firmware/controllers/system/pwm_generator_logic.h |  3 +++
 6 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/firmware/config/engines/custom_engine.cpp b/firmware/config/engines/custom_engine.cpp
index 26d6eeda23..c6d8f971dc 100644
--- a/firmware/config/engines/custom_engine.cpp
+++ b/firmware/config/engines/custom_engine.cpp
@@ -296,6 +296,9 @@ void setEtbTestConfiguration(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 
 	// turning off other PWMs to simplify debugging
 	engineConfiguration->bc.triggerSimulatorFrequency = 0;
+	engineConfiguration->stepperEnablePin = GPIO_UNASSIGNED;
+	CONFIGB(idle).stepperStepPin = GPIO_UNASSIGNED;
+	CONFIGB(idle).stepperDirectionPin = GPIO_UNASSIGNED;
 	boardConfiguration->useStepperIdle = true;
 
 }
diff --git a/firmware/controllers/electronic_throttle.cpp b/firmware/controllers/electronic_throttle.cpp
index 0c81a50945..a8378a0071 100644
--- a/firmware/controllers/electronic_throttle.cpp
+++ b/firmware/controllers/electronic_throttle.cpp
@@ -111,8 +111,8 @@ static percent_t currentEtbDuty;
 //static bool wasEtbBraking = false;
 
 // looks like my H-bridge does not like 100% duty cycle and it just hangs up?
-// so we use 99.9% to indicate that things are alive
-#define PERCENT_TO_DUTY(X) (maxF(minI(X, 99.9), -99.9) / 100.0)
+// so we use 98% to indicate that things are alive and never use PM_FULL of PWM generator
+#define PERCENT_TO_DUTY(X) (maxF(minF((X / 100.0), FULL_PWM_THRESHOLD - 0.01), 0.01 - FULL_PWM_THRESHOLD))
 
 //#define PERCENT_TO_DUTY(X) ((X) / 100.0)
 
@@ -177,6 +177,9 @@ private:
 
 		feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH);
 
+		pid.iTermMin = engineConfiguration->etb_iTermMin;
+		pid.iTermMax = engineConfiguration->etb_iTermMax;
+
 		currentEtbDuty = feedForward +
 				pid.getValue(targetPosition, actualThrottlePosition);
 
@@ -229,7 +232,7 @@ static void showEthInfo(void) {
 			getPinNameByAdcChannel("tPedal", engineConfiguration->throttlePedalPositionAdcChannel, pinNameBuffer));
 
 	scheduleMsg(&logger, "TPS=%.2f", getTPS());
-	scheduleMsg(&logger, "dir=%d", dcMotor.isOpenDirection());
+	scheduleMsg(&logger, "dir=%d DC=%f", dcMotor.isOpenDirection(), dcMotor.Get());
 
 	scheduleMsg(&logger, "etbControlPin1=%s duty=%.2f freq=%d",
 			hwPortname(CONFIGB(etbControlPin1)),
@@ -249,6 +252,9 @@ void setEtbPFactor(float value) {
 }
 
 static void etbReset() {
+	// TODO: what is this about?
+	// I am experiencing some weird instability with my H-bridge with my Monte Carlo attempts
+	dcMotor.Break();
 	mockPedalPosition = MOCK_UNDEFINED;
 	pid.reset();
 }
@@ -271,6 +277,9 @@ void setEtbDFactor(float value) {
 	showEthInfo();
 }
 
+/**
+ * set etb_o X
+ */
 void setEtbOffset(int value) {
 	engineConfiguration->etb.offset = value;
 	pid.reset();
diff --git a/firmware/controllers/system/DcMotor.cpp b/firmware/controllers/system/DcMotor.cpp
index e3a9f8c9fe..d7d9053098 100644
--- a/firmware/controllers/system/DcMotor.cpp
+++ b/firmware/controllers/system/DcMotor.cpp
@@ -23,6 +23,11 @@ float TwoPinDcMotor::Get() {
 	return value;
 }
 
+void TwoPinDcMotor::Break() {
+    m_dir1->setValue(false);
+    m_dir2->setValue(false);
+}
+
 /**
  * @param duty value between -1.0 and 1.0
  */
@@ -54,8 +59,7 @@ bool TwoPinDcMotor::Set(float duty)
 
     if(duty < 0.01f)
     {
-        m_dir1->setValue(false);
-        m_dir2->setValue(false);
+    	Break();
     }
     else
     {
diff --git a/firmware/controllers/system/DcMotor.h b/firmware/controllers/system/DcMotor.h
index cf9d2ef6b1..f55c9db4e9 100644
--- a/firmware/controllers/system/DcMotor.h
+++ b/firmware/controllers/system/DcMotor.h
@@ -53,5 +53,6 @@ public:
 
     virtual bool Set(float duty) override;
     float Get();
+    void Break();
     bool isOpenDirection();
 };
diff --git a/firmware/controllers/system/pwm_generator_logic.cpp b/firmware/controllers/system/pwm_generator_logic.cpp
index 5db69e53a8..719ff93992 100644
--- a/firmware/controllers/system/pwm_generator_logic.cpp
+++ b/firmware/controllers/system/pwm_generator_logic.cpp
@@ -21,9 +21,6 @@
 // 1% duty cycle
 #define ZERO_PWM_THRESHOLD 0.01
 
-// 99% duty cycle
-#define FULL_PWM_THRESHOLD 0.99
-
 SimplePwm::SimplePwm() {
 	waveInstance.init(pinStates);
 	sr[0] = waveInstance;
diff --git a/firmware/controllers/system/pwm_generator_logic.h b/firmware/controllers/system/pwm_generator_logic.h
index b1d99bb1b3..90d5ca3ca8 100644
--- a/firmware/controllers/system/pwm_generator_logic.h
+++ b/firmware/controllers/system/pwm_generator_logic.h
@@ -15,6 +15,9 @@
 
 #define NAN_FREQUENCY_SLEEP_PERIOD_MS 100
 
+// 99% duty cycle
+#define FULL_PWM_THRESHOLD 0.99
+
 typedef struct {
 	/**
 	 * a copy so that all phases are executed on the same period, even if another thread