From 480af6cf8505ae1f09a11a20d95f1e9e343e4fcc Mon Sep 17 00:00:00 2001
From: HomeSpan <homespan@icloud.com>
Date: Sun, 18 Jun 2023 17:55:29 -0500
Subject: [PATCH] Added setPosition() to StepperControl

Needs testing and debugging
---
 src/src/extras/StepperControl.cpp | 11 +++++++++++
 src/src/extras/StepperControl.h   |  1 +
 src/src/extras/extras.ino         | 12 ++++++++----
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/src/extras/StepperControl.cpp b/src/src/extras/StepperControl.cpp
index 92f0e37..a491e6e 100644
--- a/src/src/extras/StepperControl.cpp
+++ b/src/src/extras/StepperControl.cpp
@@ -97,6 +97,17 @@ int StepperControl::position(){
 
 //////////////////////////
 
+void StepperControl::setPosition(int pos){
+  if(!stepsRemaining()){
+    downLinkData.position=pos;
+    xQueueOverwrite(downLinkQueue,&downLinkData);
+  } else {
+    ESP_LOGE(STEPPER_TAG,"can't set Position while motor is running");
+  }
+}
+
+//////////////////////////
+
 void StepperControl::brake(){
   move(0,10,BRAKE);
   while(stepsRemaining());
diff --git a/src/src/extras/StepperControl.h b/src/src/extras/StepperControl.h
index 5e325b7..12c5581 100644
--- a/src/src/extras/StepperControl.h
+++ b/src/src/extras/StepperControl.h
@@ -87,6 +87,7 @@ class StepperControl {
   void move(int nSteps, uint32_t msDelay, endAction_t endAction=NONE);
   void moveTo(int nPosition, uint32_t msDelay, endAction_t endAction=NONE);
   int position();
+  void setPosition(int pos);
   int stepsRemaining();
   void enable();
   void disable();
diff --git a/src/src/extras/extras.ino b/src/src/extras/extras.ino
index c80d93a..e72f5a8 100644
--- a/src/src/extras/extras.ino
+++ b/src/src/extras/extras.ino
@@ -41,13 +41,17 @@ void setup() {
   delay(1000);
   Serial.printf("\nHomeSpan Steppers\n\n");
 
-  motor=new Stepper_TB6612(23,32,22,14,33,27);
-//  motor=new Stepper_TB6612(23,32,22,14);
+//  motor=new Stepper_TB6612(23,32,22,14,33,27);
+  motor=new Stepper_TB6612(23,32,22,14);
 
-  motor->setStepType(StepperControl::EIGHTH_STEP);
+  motor->setStepType(StepperControl::HALF_STEP);
   motor->setAccel(10,100);
-  motor->move(1600,2);
+  motor->move(100,2);
+  motor->setPosition(200);
   while(motor->stepsRemaining());
+  motor->setPosition(200);
+  delay(10);
+  Serial.printf("Position=%d\n",motor->position());
   motor->moveTo(0,2,StepperControl::BRAKE);
 }