Realer Fischertechnik Motor wird mit 10kHz Pwm durch Interrupt angesteuert

Arduino/motor/src/KlappenSteuerung.cpp

@@ -1,5 +1,6 @@
 #include "KlappenSteuerung.h"
 
+
 // Constructors/Destructors
 //  
 
@@ -11,14 +12,16 @@
  * @param  pinSense
  * @param  id
  */
- KlappenSteuerung::KlappenSteuerung(int pin_A, int pin_B, int pin_Pwm, int pin_Sense, int i_d)
+ KlappenSteuerung::KlappenSteuerung(int pin_A, int pin_B, int pin_Sense, int i_d)
 {
   initAttributes();
   pinA = pin_A;
   pinB = pin_B;
-  pinPwm = pin_Pwm;
   pinSense = pin_Sense;
   id = i_d;
+  vorherKlappe = letzteKlappe;
+  letzteKlappe = this;
+  
 }
 
 KlappenSteuerung::~KlappenSteuerung()
@@ -37,12 +40,16 @@ KlappenSteuerung::~KlappenSteuerung()
 // Other methods
 //  
 
-
+KlappenSteuerung *KlappenSteuerung::letzteKlappe;
 
 /**
  */
 void KlappenSteuerung::setup()
 {
+  if(KlappenSteuerung::letzteKlappe){
+    KlappenSteuerung::letzteKlappe->localSetup();
+  }
+  Motor::setup();
 }
 
 
@@ -50,6 +57,32 @@ void KlappenSteuerung::setup()
  */
 void KlappenSteuerung::loop()
 {
+  if(KlappenSteuerung::letzteKlappe){
+    KlappenSteuerung::letzteKlappe->localLoop();
+  }
+  Motor::loop();
+}
+
+
+void KlappenSteuerung::localSetup()
+{
+  if(vorherKlappe){
+    vorherKlappe->localSetup();
+  }else{
+
+  }
+  motor.setPins(pinA , pinB);
+
+}
+
+void KlappenSteuerung::localLoop()
+{
+  if(vorherKlappe){
+    vorherKlappe->localLoop();
+  }else{
+
+  }
+  
 }
 
 
@@ -64,7 +97,6 @@ void KlappenSteuerung::initAttributes()
 {
   pinA = -1;
   pinB = -1;
-  pinPwm = -1;
   pinSense = -1;
   id = -1;
   status = NICHTS;

Arduino/motor/src/KlappenSteuerung.h

@@ -3,6 +3,7 @@
 #define KLAPPENSTEUERUNG_H
 
 #include "statusKlappen.h"
+#include "motor.h"
 
 /**
   * class KlappenSteuerung
@@ -37,21 +38,20 @@ public:
   /**
    * @param  pinA
    * @param  pinB
-   * @param  pinPwm
    * @param  pinSense
    * @param  id
    */
-   KlappenSteuerung(int pinA, int pinB, int pinPwm, int pinSense, int id);
+   KlappenSteuerung(int pinA, int pinB, int pinSense, int id);
 
 
   /**
    */
-  void setup();
+  static void setup();
 
 
   /**
    */
-  void loop();
+  static void loop();
 
 
   /**
@@ -60,25 +60,24 @@ public:
   void setOpen(bool open);
 
 protected:
-  // Static Protected attributes
-  //  
-
-  // Protected attributes
-  //  
-
+ 
   int pinA;
   int pinB;
-  int pinPwm;
   int pinSense;
   int id;
   statusKlappen status;
 
-private:
-  // Static Private attributes
-  //  
+  void localSetup();
+  void localLoop();
 
-  // Private attributes
-  //  
+  Motor motor;
+
+
+private:
+
+  static KlappenSteuerung *letzteKlappe;
+  KlappenSteuerung *vorherKlappe = 0;
+ 
 
 
   void initAttributes();

Arduino/motor/src/main.cpp

@@ -7,27 +7,21 @@
 void Ablauf();
 void AblaufSekunde();
 
-KlappenSteuerung klappe1 (M1A , M1B , M1PWM , M1SENSE , 5  );
-KlappenSteuerung klappe2 (M2A , M2B , M2PWM , M2SENSE , 2  );
-KlappenSteuerung klappe3 (M3A , M3B , M3PWM , M3SENSE , 23 );
-KlappenSteuerung klappe4 (M4A , M4B , M4PWM , M4SENSE , 1  );
+KlappenSteuerung klappe1 (M1A , M1B , M1SENSE , 5  );
+KlappenSteuerung klappe2 (M2A , M2B , M2SENSE , 2  );
+KlappenSteuerung klappe3 (M3A , M3B , M3SENSE , 23 );
+KlappenSteuerung klappe4 (M4A , M4B , M4SENSE , 1  );
 
 
 
 void setup() {
   // put your setup code here, to run once:
-  klappe1.setup();
-  klappe2.setup();
-  klappe3.setup();
-  klappe4.setup();
+  KlappenSteuerung::setup();
 }
 
 void loop() {
   // put your main code here, to run repeatedly:
-  klappe1.loop();
-  klappe2.loop();
-  klappe3.loop();
-  klappe4.loop();
+  KlappenSteuerung::loop();
   Ablauf();
 }
 
@@ -47,12 +41,14 @@ void Ablauf(){
     summeZeit -= 1000;
     AblaufSekunde();  
   }
+
+  
 }
 
 void AblaufSekunde(){
   static unsigned int sekunden;
   sekunden ++;
-
+  //digitalWrite(LED_DEBUG , !(sekunden & 3));
   switch(sekunden){
     case 1:
       klappe1.setOpen(true);
@@ -62,7 +58,7 @@ void AblaufSekunde(){
     klappe1.setOpen(false);
       break;
 
-    case 19:
+    case 20:
       sekunden = 0;
       break;
   }

Arduino/motor/src/motor.cpp

@@ -0,0 +1,130 @@
+#include "motor.h"
+
+Motor *Motor::letztesElement;
+
+Motor::Motor(){
+
+  vorherElement = Motor::letztesElement;
+  Motor::letztesElement = this;
+  this->pinA = pinA;
+  this->pinB = pinB;
+
+}
+
+
+Motor::~Motor(){
+
+}
+
+void Motor::setPins(uint8_t pinA , uint8_t pinB){
+  this->pinA = pinA;
+  this->pinB = pinB;
+}
+
+void Motor::setup(){
+  if(Motor::letztesElement){
+    Motor::letztesElement->localSetup();
+  }
+  Timer1.initialize(100); // 10kHz
+  Timer1.attachInterrupt(Motor::interruptEinsprung);
+
+}
+
+void Motor::loop(){
+  if(Motor::letztesElement){
+    Motor::letztesElement->localLoop();
+  }
+}
+
+void Motor::localSetup(){
+  mySetup();
+  if(vorherElement){
+    vorherElement->localSetup();
+  }else{
+
+  }
+  
+}
+
+void Motor::mySetup(){
+  pinMode(pinA , OUTPUT);
+  digitalWrite( pinA , 0);
+
+  pinMode(pinB , OUTPUT);
+  digitalWrite(pinB , 0);
+}
+
+
+void Motor::localLoop(){
+  myLoop();
+  if(vorherElement){
+    vorherElement->localLoop();
+  }else{
+
+  }
+}
+
+void Motor::myLoop(){
+  if(richtung){
+    if(test == 32767)
+      richtung = !richtung;
+    else
+      test ++;
+  }else{
+    if(test == -32767)
+      richtung = !richtung;
+    else
+      test --;
+  }
+  setSollSpeed( test / 64);
+  //setSollSpeed(100);
+}
+
+
+void Motor::setSollSpeed(int16_t speed){
+  if(speed > 256)
+    speed = 256;
+  if((speed < 100) && (speed > -100))
+    speed = 0;
+  if(speed < -256)
+    speed = -256;
+
+  sollSpeed = speed;
+
+}
+
+
+void Motor::interruptEinsprung(){
+  if(letztesElement)
+    letztesElement->localInterruptEinsprung();
+}
+void Motor::localInterruptEinsprung(){
+  interruptBerechnungen();
+  if(vorherElement){
+    vorherElement->localInterruptEinsprung();
+  }else{
+    timer ++;
+    //digitalWrite(LED_DEBUG , (timer & 0x100) != 0 );
+  }
+}
+
+void Motor::interruptBerechnungen(){
+  summeSpeed += sollSpeed;
+  if(summeSpeed >= 256)
+  {
+    summeSpeed -= 256;
+    digitalWrite(pinA , 0);
+    digitalWrite(pinB , 1);
+
+  }else if (summeSpeed <= -256){
+    summeSpeed += 256;
+    digitalWrite(pinA , 1);
+    digitalWrite(pinB , 0);
+
+  }else{
+    digitalWrite(pinA , 0);
+    digitalWrite(pinB , 0);
+
+  }
+  
+}

Arduino/motor/src/motor.h

@@ -0,0 +1,51 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include <TimerOne.h>
+#include "pins.h"
+
+class Motor{
+
+public:
+  Motor();
+  Motor(uint8_t pinA , uint8_t pinB);
+
+  virtual ~Motor();
+
+  static void setup();
+  static void loop();
+
+  void setPins(uint8_t pinA , uint8_t pinB);
+
+  void setSollSpeed(int16_t speed);
+
+protected:
+
+  static Motor *letztesElement;
+  Motor *vorherElement;
+
+  void localSetup();
+  void localLoop();
+
+  void mySetup();
+  void myLoop();
+
+  static void interruptEinsprung();
+  void localInterruptEinsprung();
+
+  void interruptBerechnungen();
+
+  uint8_t pinA , pinB;
+  uint16_t timer;
+
+  int16_t sollSpeed; 
+  int16_t summeSpeed;
+
+  bool richtung;
+  int16_t test;
+  
+};
+
+
+
+#endif

Arduino/motor/src/pins.h

@@ -5,23 +5,29 @@
 
 #define M1A       2
 #define M1B       3
-#define M1PWM     5
-#define M1SENSE   1
+#define M1SENSE  A1
 
 #define M2A       4
-#define M2B       7
-#define M2PWM     6
-#define M2SENSE   2
+#define M2B       5
+#define M2SENSE  A2
+
+#define M3A       6
+#define M3B       7
+#define M3SENSE  A3
+
+#define M4A       8
+#define M4B       9
+#define M4SENSE  A4
+
+#define M5A      10
+#define M5B      11
+#define M5SENSE  A5
+
+#define M6A      12
+#define M6B      13
+#define M6SENSE  A6
 
-#define M3A       8
-#define M3B      11
-#define M3PWM     9
-#define M3SENSE   3
 
-#define M4A      12
-#define M4B      13
-#define M4PWM    10
-#define M4SENSE   4