some minor fixes

MicroBahner · Aug 23, 2018 · 2a3ba28 · 2a3ba28
1 parent 73bd26e
commit 2a3ba28
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 17 deletions.
diff --git a/MobaTools.cpp b/MobaTools.cpp
@@ -205,10 +205,10 @@ static uint8_t spiInitialized = false;
 // Variables for servos
 static servoData_t* lastServoDataP = NULL; //start of ServoData-chain
 static byte servoCount = 0;
-static servoData_t* pulseP = 0;         // pulse Ptr in IRQ
-static servoData_t* activePulseP = 0;   // Ptr to pulse to stop
-static servoData_t* stopPulseP = 0;     // Ptr to Pulse whose stop time is already in OCR1
-static servoData_t* nextPulseP = 0;
+static servoData_t* pulseP = NULL;         // pulse Ptr in IRQ
+static servoData_t* activePulseP = NULL;   // Ptr to pulse to stop
+static servoData_t* stopPulseP = NULL;     // Ptr to Pulse whose stop time is already in OCR1
+static servoData_t* nextPulseP = NULL;
 static enum { PON, POFF } IrqType = PON; // Cycle starts with 'pulse on'
 static word activePulseOff = 0;     // OCR-value of pulse end 
 static word nextPulseLength = 0;
@@ -454,7 +454,7 @@ void ISR_Stepper(void)
                         ledDataP->actPulse = true;
                     }
                 pwm0end: // end of 'switch'
-                CLR_TP1;
+                ;//CLR_TP1;
             } // end of led loop
         } else { // is switchofftime within PWM cycle
             for ( ledDataP=ledRootP; ledDataP!=NULL; ledDataP = ledDataP->nextLedDataP ) {
@@ -530,7 +530,7 @@ void ISR_Stepper(void)
                 CLR_TP4;
             }
         }
-        CLR_TP1;
+        //CLR_TP1;
      } // end of softleds 
     CLR_TP3;
     nextCycle = min( nextCycle, ( ledNextCyc-ledCycleCnt ) );
@@ -589,8 +589,8 @@ void ISR_Servo( void) {
     uint16_t OCRxA;
 #endif
     // Timer1 Compare A, used for servo motor
-    SET_TP1; // Oszimessung Dauer der ISR-Routine
     if ( IrqType == POFF ) {
+        SET_TP1; // Oszimessung Dauer der ISR-Routine OFF
         IrqType = PON ; // it's always alternating
         // switch off previous started pulse
         #ifdef FAST_PORTWRT
@@ -608,7 +608,9 @@ void ISR_Servo( void) {
             // from the first servo. Pulses must be sorted in ascending order.
             OCRxA = FIRST_PULSE + (servoCount-1-pulseP->servoIx) * PULSESTEP;
         }
+        CLR_TP1; // Oszimessung Dauer der ISR-Routine OFF
     } else {
+        SET_TP2; // Oszimessung Dauer der ISR-Routine ON
         // look for next pulse to start
         if ( pulseP->soll < 0 ) {
             // no pulse to output, switch to next startpoint
@@ -645,11 +647,11 @@ void ISR_Servo( void) {
             }
             IrqType = POFF;
         } 
+        CLR_TP2; // Oszimessung Dauer der ISR-Routine ON
     } //end of 'pulse ON'
     #ifdef __STM32F1__
     timer_set_compare(MT_TIMER,  SERVO_CHN, OCRxA);
     #endif 
-    CLR_TP1; // Oszimessung Dauer der ISR-Routine
 }
 
 #else // create overlapping servo pulses
@@ -698,6 +700,7 @@ void ISR_Servo( void) {
 #endif
     // Timer1 Compare A, used for servo motor
     if ( IrqType == POFF ) { // Pulse OFF time
+        SET_TP1; // Oszimessung Dauer der ISR-Routine OFF
         //SET_TP3; // Oszimessung Dauer der ISR-Routine
         IrqType = PON ; // it's (nearly) always alternating
         // switch off previous started pulse
@@ -732,8 +735,10 @@ void ISR_Servo( void) {
                 OCRxA = FIRST_PULSE;
             }
         }
+        CLR_TP1; // Oszimessung Dauer der ISR-Routine OFF
     } else { // Pulse ON - time
-        //SET_TP1; // Oszimessung Dauer der ISR-Routine
+        SET_TP2; // Oszimessung Dauer der ISR-Routine ON
+        if ( pulseP == lastServoDataP ) SET_TP3;
         // look for next pulse to start
         // do we know the next pulse already?
         if ( nextPulseLength > 0 ) {
@@ -818,7 +823,7 @@ void ISR_Servo( void) {
                 IrqType = POFF;
             }
         }
-
+        CLR_TP2; CLR_TP3; // Oszimessung Dauer der ISR-Routine ON
     } //end of 'pulse ON'
     #ifdef __STM32F1__
     timer_set_compare(MT_TIMER,  SERVO_CHN, OCRxA);
@@ -1278,15 +1283,21 @@ uint8_t Servo8::attach( int pinArg, int pmin, int pmax, bool autoOff ) {
     digitalWrite(pin,LOW);
 
     if ( !timerInitialized) seizeTimer1();
+    // initialize servochain pointer if not done already
+    noInterrupts();
+    if ( pulseP == NULL ) pulseP = lastServoDataP;
+    interrupts();
+
     // enable compare-A interrupt
     #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
     TIMSK |=  _BV(OCIExA);   
     #elif defined __AVR_MEGA__
+    DB_PRINT( "IniOCR: %d", OCRxA );
     TIMSKx |=  _BV(OCIExA) ; 
+    DB_PRINT( "AttOCR: %d", OCRxA );
     #elif defined __STM32F1__
         timer_cc_enable(MT_TIMER, SERVO_CHN);
     #endif
-
     return 1;
 }
 
@@ -1304,7 +1315,9 @@ void Servo8::write(int angleArg)
     // values between 0 and 180 are interpreted as degrees,
     // values between MINPULSEWIDTH and MAXPULSEWIDTH are interpreted as microseconds
     static int newpos;
-	//DB_PRINT( "Write: angleArg=%d, Soll=%d", angleArg, servoData.soll );
+    #ifdef __AVR_MEGA__
+	DB_PRINT( "Write: angleArg=%d, Soll=%d, OCR=%u", angleArg, servoData.soll, OCRxA );
+    #endif
     if ( pin > 0 ) { // only if servo is attached
         //Serial.print( "Pin:" );Serial.print(pin);Serial.print("Wert:");Serial.println(angleArg);
         #ifdef __AVR_MEGA__

diff --git a/MobaTools.h b/MobaTools.h
@@ -115,7 +115,7 @@
 #ifdef FIXED_POSITION_SERVO_PULSES
     #define MAX_SERVOS  8
 #else
-    #define OVLMARGIN           180     // Overlap margin ( Overlap is MINPULSEWIDTH - OVLMARGIN )
+    #define OVLMARGIN           280     // Overlap margin ( Overlap is MINPULSEWIDTH - OVLMARGIN )
     #define OVL_TICS       ( ( MINPULSEWIDTH - OVLMARGIN ) * TICS_PER_MICROSECOND )
     #define MARGINTICS      ( OVLMARGIN * TICS_PER_MICROSECOND )
     #define MAX_SERVOS  16  
@@ -321,8 +321,8 @@ class Servo8
     uint8_t read();          // current position in degrees (0...180)
     int   readMicroseconds();// current pulsewidth in microseconds
     uint8_t attached();
-    void setMinimumPulse(uint16_t);  // pulse length for 0 degrees in microseconds, 540uS default
-    void setMaximumPulse(uint16_t);  // pulse length for 180 degrees in microseconds, 2400uS default
+    void setMinimumPulse(uint16_t);  // pulse length for 0 degrees in microseconds, 700uS default
+    void setMaximumPulse(uint16_t);  // pulse length for 180 degrees in microseconds, 2300uS default
 };
 
 //////////////////////////////////////////////////////////////////////////////////////////////

diff --git a/examples/Servo_01/Servo_01.ino b/examples/Servo_01/Servo_01.ino
@@ -26,14 +26,14 @@ const int servoPin =  9;  // Anschluß für den Servo
 // werden
 
 
-int tasterStatus1, tasterStatus2;
+bool tasterStatus1, tasterStatus2;
 Servo8 meinServo;
 
 void setup() {
     pinMode(tasterPin1, INPUT_PULLUP); // so ist kein externer pullup Widerstand am 
     pinMode(tasterPin2, INPUT_PULLUP); // Taster erforderlich
 
-    meinServo.attach(servoPin); //Servo an Pin 12
+    meinServo.attach(servoPin); //Servo an Pin 9
     meinServo.setSpeed( 5 );    // Verfahrgeschwindigkeit einstellen
 }