Version 2.0

SphereBot Arduino/SphereBot/SoftwareServo.cpp

@@ -0,0 +1,128 @@
+#include "SoftwareServo.h"
+
+SoftwareServo *SoftwareServo::first;
+
+#define NO_ANGLE (0xff)
+
+SoftwareServo::SoftwareServo() : pin(0),angle(NO_ANGLE),pulse0(0),min16(34),max16(150),next(0)
+{}
+
+void SoftwareServo::setMinimumPulse(uint16_t t)
+{
+    min16 = t/16;
+}
+
+void SoftwareServo::setMaximumPulse(uint16_t t)
+{
+    max16 = t/16;
+}
+
+uint8_t SoftwareServo::attach(int pinArg)
+{
+    pin = pinArg;
+    angle = NO_ANGLE;
+    pulse0 = 0;
+    next = first;
+    first = this;
+    digitalWrite(pin,0);
+    pinMode(pin,OUTPUT);
+    return 1;
+}
+
+void SoftwareServo::detach()
+{
+    for ( SoftwareServo **p = &first; *p != 0; p = &((*p)->next) ) {
+	if ( *p == this) {
+	    *p = this->next;
+	    this->next = 0;
+	    return;
+	}
+    }
+}
+
+void SoftwareServo::write(int angleArg)
+{
+    if ( angleArg < 0) angleArg = 0;
+    if ( angleArg > 180) angleArg = 180;
+    angle = angleArg;
+    // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
+    // That 64L on the end is the TCNT0 prescaler, it will need to change if the clock's prescaler changes,
+    // but then there will likely be an overflow problem, so it will have to be handled by a human.
+    pulse0 = (min16*16L*clockCyclesPerMicrosecond() + (max16-min16)*(16L*clockCyclesPerMicrosecond())*angle/180L)/64L;
+}
+
+uint8_t SoftwareServo::read()
+{
+    return angle;
+}
+
+uint8_t SoftwareServo::attached()
+{
+    for ( SoftwareServo *p = first; p != 0; p = p->next ) {
+	if ( p == this) return 1;
+    }
+    return 0;
+}
+
+void SoftwareServo::refresh()
+{
+    uint8_t count = 0, i = 0;
+    uint16_t base = 0;
+    SoftwareServo *p;
+    static unsigned long lastRefresh = 0;
+    unsigned long m = millis();
+
+    // if we haven't wrapped millis, and 20ms have not passed, then don't do anything
+    if ( m >= lastRefresh && m < lastRefresh + 20) return;
+    lastRefresh = m;
+
+    for ( p = first; p != 0; p = p->next ) if ( p->pulse0) count++;
+    if ( count == 0) return;
+
+    // gather all the SoftwareServos in an array
+    SoftwareServo *s[count];
+    for ( p = first; p != 0; p = p->next ) if ( p->pulse0) s[i++] = p;
+
+    // bubblesort the SoftwareServos by pulse time, ascending order
+    for(;;) {
+	uint8_t moved = 0;
+	for ( i = 1; i < count; i++) {
+	    if ( s[i]->pulse0 < s[i-1]->pulse0) {
+		SoftwareServo *t = s[i];
+		s[i] = s[i-1];
+		s[i-1] = t;
+		moved = 1;
+	    }
+	}
+	if ( !moved) break;
+    }
+
+    // turn on all the pins
+    // Note the timing error here... when you have many SoftwareServos going, the
+    // ones at the front will get a pulse that is a few microseconds too long.
+    // Figure about 4uS/SoftwareServo after them. This could be compensated, but I feel
+    // it is within the margin of error of software SoftwareServos that could catch
+    // an extra interrupt handler at any time.
+    for ( i = 0; i < count; i++) digitalWrite( s[i]->pin, 1);
+
+    uint8_t start = TCNT0;
+    uint8_t now = start;
+    uint8_t last = now;
+
+    // Now wait for each pin's time in turn..
+    for ( i = 0; i < count; i++) {
+	uint16_t go = start + s[i]->pulse0;
+
+	// loop until we reach or pass 'go' time
+	for (;;) {
+	    now = TCNT0;
+	    if ( now < last) base += 256;
+	    last = now;
+
+	    if ( base+now > go) {
+		digitalWrite( s[i]->pin,0);
+		break;
+	    }
+	}
+    }
+}