From a59f23bc5c67c648b36384645657d37d0d31555d Mon Sep 17 00:00:00 2001
From: Toberfra <toberfra@gmx.de>
Date: Fri, 11 Feb 2022 09:57:20 +0100
Subject: [PATCH] Am Model Funktionstuechtig Motor ueber Sigma Delta
 Ansteuerung Sanft in die Endschalter fahren

---
 Arduino/motor/src/KlappenAblauf.cpp | 42 ++++++++++++++++-----
 Arduino/motor/src/KlappenAblauf.h   |  7 +++-
 Arduino/motor/src/motor.cpp         | 57 ++++++++++++++---------------
 Arduino/motor/src/motor.h           |  4 +-
 Arduino/motor/src/pins.h            |  2 +
 5 files changed, 69 insertions(+), 43 deletions(-)

diff --git a/Arduino/motor/src/KlappenAblauf.cpp b/Arduino/motor/src/KlappenAblauf.cpp
index 5cfb7ed..ee4519c 100644
--- a/Arduino/motor/src/KlappenAblauf.cpp
+++ b/Arduino/motor/src/KlappenAblauf.cpp
@@ -62,27 +62,42 @@ void KlappenAblauf::fsmStep(){
   switch (aktuellerState)
   {
   case INIT:
-    aktuellerState = START_AUF;
+    aktuellerState = REF_FAHRT;
     setOpen(false);
     break;
 
+  case REF_FAHRT:
+    timer = 5000;
+    setSollSpeed(LANGSAMME_FAHRT);
+    aktuellerState = FAEHRT_AUF_LANGSAM;
+    break;  
+
   case WARTE_OFFEN:
   case WARTE_GESCHLOSSEN:
     break;
 
   case START_AUF:
-    setSollSpeed(800);
-    timer = 5000;
-    aktuellerState = FAEHRT_AUF;
+    setSollSpeed(SCHNELLE_FAHRT);
+    timer = 1800;
+    aktuellerState = FAEHRT_AUF_SCHNELL;
     break;
 
   case START_ZU:
-    setSollSpeed(-1023);
-    timer = 5000;
-    aktuellerState = FAEHRT_ZU;
+    setSollSpeed(-SCHNELLE_FAHRT);
+    timer = 1800;
+    aktuellerState = FAEHRT_ZU_SCHNELL;
     break;
 
-  case FAEHRT_AUF:
+  case FAEHRT_AUF_SCHNELL:
+    timer -= getDeltamsec();
+    if(timer <= 0){
+      timer = 2000;
+      setSollSpeed(LANGSAMME_FAHRT);
+      aktuellerState = FAEHRT_AUF_LANGSAM;
+    }
+    break;
+
+  case FAEHRT_AUF_LANGSAM:
     timer -= getDeltamsec();
     if(timer <= 0){
       setSollSpeed(0);
@@ -90,7 +105,16 @@ void KlappenAblauf::fsmStep(){
     }
     break;
 
-  case FAEHRT_ZU:
+  case FAEHRT_ZU_SCHNELL:
+    timer -= getDeltamsec();
+    if(timer <= 0){
+      timer = 2000;
+      setSollSpeed(-LANGSAMME_FAHRT);
+      aktuellerState = FAEHRT_ZU_LANGSAM;
+    }
+    break;
+
+  case FAEHRT_ZU_LANGSAM:
     timer -= getDeltamsec();
     if(timer <= 0){
       setSollSpeed(0);
diff --git a/Arduino/motor/src/KlappenAblauf.h b/Arduino/motor/src/KlappenAblauf.h
index 0e2eabf..93f4c6a 100644
--- a/Arduino/motor/src/KlappenAblauf.h
+++ b/Arduino/motor/src/KlappenAblauf.h
@@ -58,14 +58,17 @@ private:
   void openSpeicherAbfrage();
   typedef enum fsmStatesEnum{
     INIT,
+    REF_FAHRT,
 
     WARTE_OFFEN,
     START_ZU,
-    FAEHRT_ZU,
+    FAEHRT_ZU_SCHNELL,
+    FAEHRT_ZU_LANGSAM,
 
     WARTE_GESCHLOSSEN,
     START_AUF,
-    FAEHRT_AUF,
+    FAEHRT_AUF_SCHNELL,
+    FAEHRT_AUF_LANGSAM,
 
   }fsmStates;
   
diff --git a/Arduino/motor/src/motor.cpp b/Arduino/motor/src/motor.cpp
index a7054c6..a2987bd 100644
--- a/Arduino/motor/src/motor.cpp
+++ b/Arduino/motor/src/motor.cpp
@@ -21,7 +21,8 @@ void Motor::setMotorPwmPin(uint8_t pinPwm){
 void Motor::setup(){
   ArduinoAnpassung::setup();
   Timer1.initialize(100);     //10kHz
-  Timer1.pwm(pinPwm , 0);
+  pinMode(pinPwm , OUTPUT);
+  digitalWrite(pinPwm , 1);
   Timer1.attachInterrupt(Motor::interruptEinsprung);
 }
 
@@ -48,7 +49,27 @@ void Motor::interruptBerechnungen(){
     setIstSpeed(istSpeed);
   }*/
   
-    setIstSpeed(sollSpeed);
+  istSpeed = sollSpeed;
+
+
+  summeSpeed += istSpeed;
+  if(summeSpeed >= 256)
+  {
+    summeSpeed -= 256;
+    digitalWrite(pinA , 1);
+    digitalWrite(pinB , 0);
+
+  }else if (summeSpeed <= -256){
+    summeSpeed += 256;
+    digitalWrite(pinA , 0);
+    digitalWrite(pinB , 1);
+
+  }else{
+    digitalWrite(pinA , 0);
+    digitalWrite(pinB , 0);
+
+  }
+
   
 
 }
@@ -59,38 +80,14 @@ void Motor::interruptBerechnungen(){
 void Motor::setSollSpeed(int16_t speed){
   
 
- /* if(speed >1023)
-    speed = 1023;
-  if(speed < -1023)
-    speed = -1023; */
+  if(speed > 256)
+    speed = 256;
+  if(speed < -256)
+    speed = -256;
 
   sollSpeed = speed;
 
 }
-void Motor::setIstSpeed(int16_t speed){
-  uint16_t pwm;
-
-  if(speed > 0)
-   { 
-    pwm = speed ;
-    digitalWrite(pinA , 1);
-    digitalWrite(pinB , 0);
-
-  }else if (speed < 0){
-    pwm =  -speed ;
-    digitalWrite(pinA , 0);
-    digitalWrite(pinB , 1);
-
-  }else{
-    pwm = 1023;
-    digitalWrite(pinA , 0);
-    digitalWrite(pinB , 0);
-
-  }
-  Timer1.pwm(Motor::pinPwm, pwm);
- 
-
-}
 
 
 
diff --git a/Arduino/motor/src/motor.h b/Arduino/motor/src/motor.h
index 84b143c..86728db 100644
--- a/Arduino/motor/src/motor.h
+++ b/Arduino/motor/src/motor.h
@@ -24,7 +24,7 @@ public:
 protected:
 
  
-  void setIstSpeed(int16_t speed);
+
   static uint8_t pinPwm;
 
 
@@ -42,7 +42,7 @@ protected:
 
   int16_t sollSpeed; 
   int16_t istSpeed;
-  //int16_t summeSpeed;
+  int16_t summeSpeed;
 
   bool richtung;
   int16_t test;
diff --git a/Arduino/motor/src/pins.h b/Arduino/motor/src/pins.h
index a89bd6c..5dc09a5 100644
--- a/Arduino/motor/src/pins.h
+++ b/Arduino/motor/src/pins.h
@@ -31,6 +31,8 @@
 #define TASTER_AUF 11
 #define TASTER_ZU  12
 
+#define SCHNELLE_FAHRT (256)
+#define LANGSAMME_FAHRT (150)