From c5a40c6e3467acca5d24f6905b21848063d2f5a5 Mon Sep 17 00:00:00 2001
From: Gregg <homespan@icloud.com>
Date: Thu, 22 Jun 2023 21:25:18 -0500
Subject: [PATCH] Added ability to set cpu and priority for motor task using
 pair {}

---
 src/src/extras/StepperControl.cpp |  1 +
 src/src/extras/StepperControl.h   |  2 +-
 src/src/extras/Stepper_A3967.h    |  2 +-
 src/src/extras/Stepper_TB6612.h   |  4 ++--
 src/src/extras/extras.ino         | 36 +++++++++++++++++++++++++------
 5 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/src/src/extras/StepperControl.cpp b/src/src/extras/StepperControl.cpp
index 6992c16..5887d7e 100644
--- a/src/src/extras/StepperControl.cpp
+++ b/src/src/extras/StepperControl.cpp
@@ -34,6 +34,7 @@ StepperControl::StepperControl(uint32_t priority, uint32_t cpu){
   upLinkQueue = xQueueCreate(1,sizeof(upLink_t));  
   downLinkQueue = xQueueCreate(1,sizeof(downLink_t));  
   xTaskCreateUniversal(motorTask, "motorTaskHandle", 8096, this, priority, &motorTaskHandle, cpu);
+  ESP_LOGI(STEPPER_TAG,"motor task started with priority %d on cpu %d",priority,cpu);
 }
 
 //////////////////////////
diff --git a/src/src/extras/StepperControl.h b/src/src/extras/StepperControl.h
index b4b4f9e..e876bd8 100644
--- a/src/src/extras/StepperControl.h
+++ b/src/src/extras/StepperControl.h
@@ -89,7 +89,7 @@ class StepperControl {
 
   public:
 
-  StepperControl(uint32_t priority=2, uint32_t cpu=0);
+  StepperControl(uint32_t priority=1, uint32_t cpu=0);
   virtual void setStepType(int mode){};
   void setAccel(float accelSize, float accelSteps);
   void move(int nSteps, uint32_t msDelay, endAction_t endAction=NONE);
diff --git a/src/src/extras/Stepper_A3967.h b/src/src/extras/Stepper_A3967.h
index 216859f..1dca3f6 100644
--- a/src/src/extras/Stepper_A3967.h
+++ b/src/src/extras/Stepper_A3967.h
@@ -53,7 +53,7 @@ struct Stepper_A3967 : StepperControl {
 
 //////////////////////////
 
-  Stepper_A3967(int m1Pin, int m2Pin, int stepPin, int dirPin, int enablePin) : StepperControl(){
+  Stepper_A3967(int m1Pin, int m2Pin, int stepPin, int dirPin, int enablePin, std::pair<uint32_t, uint32_t> taskParams = {1,0}) : StepperControl(taskParams.first,taskParams.second){
     this->m1Pin=m1Pin;
     this->m2Pin=m2Pin;
     this->stepPin=stepPin;
diff --git a/src/src/extras/Stepper_TB6612.h b/src/src/extras/Stepper_TB6612.h
index bf53133..2f243d1 100644
--- a/src/src/extras/Stepper_TB6612.h
+++ b/src/src/extras/Stepper_TB6612.h
@@ -60,7 +60,7 @@ struct Stepper_TB6612 : StepperControl {
 
 //////////////////////////
 
-  Stepper_TB6612(int AIN1, int AIN2, int BIN1, int BIN2) : StepperControl(){
+  Stepper_TB6612(int AIN1, int AIN2, int BIN1, int BIN2, std::pair<uint32_t, uint32_t> taskParams = {1,0}) : StepperControl(taskParams.first,taskParams.second){
 
     ain1=AIN1;
     ain2=AIN2;
@@ -77,7 +77,7 @@ struct Stepper_TB6612 : StepperControl {
 
 //////////////////////////
 
-  Stepper_TB6612(int AIN1, int AIN2, int BIN1, int BIN2, int PWMA, int PWMB) : Stepper_TB6612(AIN1,AIN2,BIN1,BIN2){
+  Stepper_TB6612(int AIN1, int AIN2, int BIN1, int BIN2, int PWMA, int PWMB, std::pair<uint32_t, uint32_t> taskParams = {1,0}) : Stepper_TB6612(AIN1,AIN2,BIN1,BIN2,taskParams){
 
     pwmA=new LedPin(PWMA,0,50000);
     pwmB=new LedPin(PWMB,0,50000);
diff --git a/src/src/extras/extras.ino b/src/src/extras/extras.ino
index 7dba94b..d6be4fc 100644
--- a/src/src/extras/extras.ino
+++ b/src/src/extras/extras.ino
@@ -41,15 +41,39 @@ 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,33,27,{1,1});
   motor=new Stepper_TB6612(23,32,22,14);
 
-  motor->setStepType(StepperControl::HALF_STEP);
-  motor->setAccel(10,30);
-  motor->move(400,2);
-  while(motor->stepsRemaining());
+  motor->setStepType(StepperControl::FULL_STEP_ONE_PHASE);
+  motor->setAccel(10,5);
+
+  motor->enable();
+  while(1);
+  
+  for(int i=0;i<100;i++){
+    motor->disable();
+    delay(10);
+    motor->enable();
+    delay(10);
+  }
+  Serial.printf("Done\n");
+  while(1);
+  
+  delay(1000);
+  for(int i=0;i<10;i++){
+    motor->move(1,100);
+    while(motor->stepsRemaining());
+    motor->move(-0,5);
+    while(motor->stepsRemaining());
+  }
+  while(1);
+    
+  delay(100);
+  motor->move(-400,5,StepperControl::BRAKE);
+  while(1);
   motor->setPosition(800);
-  motor->moveTo(0,2,StepperControl::BRAKE);
+  motor->moveTo(0,5,StepperControl::BRAKE);
 }
 
 //////////////////////////////////////