The second thing I wrote was “subscriptions”. I wanted the GrovePi hardware to read out analog (and digital) ports and do averaging on the GrovePi processors, so that the RPi can read out already finished values.
Right now I made two types, one for analog read and one for the ultrasonic sensor (pulse length read).
The Python API part of the subscription looks like this:
def setupSubscription(n, typ, pin, flag):
write_i2c_block(address, [100+n] + [typ,pin,flag])
return 1
def readSubscription(start=0,n=8):
write_i2c_block(address, [116] + [start,n,0]);
time.sleep(.05)
bus.read_byte(address)
number = bus.read_i2c_block_data(address, 1)
v=[ number[i*2:(i+1)*2] for i in range(n) ]
res=[]
for x in v: res.append((x[0]<<8)+x[1])
return res
Each subscription has a type, a pin, and flags. You can use subscription number 0 to and including 15.
When reading subscriptions they can be read grouped, so you can read out several at once (or even all).
In the application code they are used like this:
File Edit Options Buffers Tools Python Help
import time
import grovepi
ultrasonic_ranger = 7
infrared_sensor = 1
grovepi.pinMode(infrared_sensor,"INPUT")
grovepi.setupSubscription(1,3,infrared_sensor,0x10|0x08|0x07);
grovepi.setupSubscription(0,8,ultrasonic_ranger,0x10|0x08|0x05);
while True:
try:
time.sleep(.1)
numbers=grovepi.readSubscription(0,2)
print "ultrasonic " + repr(numbers[0]/(16*29.0*2))+" cm"
print "infrared " + repr(numbers[1]*(5.0/(1024*16.0)))+" V";
except IOError:
print "Error"
The flags here means:
0x10 : send number * 16 for higher precision
0x08 : run median filter to cancel out single bad readings
0x07 : run exponential average filter on the reading, “128” length (2<<7)
0x05 : run exponential average filter on the reading, “32” length (2<<5)
The ultrasonic reads with a pace of 10Hz. I found this made it non-glitchy.
The analog port reads with a pace of 100Hz.
With this I get a precision and noise level of the ultrasonic of ±0.5mm (it walks up and down in that interval; I’m unsure why).
Here is the patch (also in the zip above):
--- grovepi_cmd_queue.ino 2015-06-03 18:09:50.668564799 +0200
+++ grovepi_subscriptions.ino 2015-06-03 18:08:10.301826579 +0200
@@ -24,6 +24,152 @@
volatile int last_cmd=0;
+
+#define SUBSCRIPTIONS 16
+// subscriptions: number: result without flags:
+// analog read 3 analog read (1023 = Vref)
+// ultrasonic 7 return time (us) (n/29/2 for cm)
+
+// flags:
+// bit 0..2 (1..7) exponential average of 2^n = 2..128 samples
+// bit 3 (0x08) median filter of 3 (n = median of n_t-2, n_t-1, n)
+// bit 4 (0x10) send result as x16 for better precision
+// internal:
+// bit 8 (0x100) median filter initialized
+// bit 9 (0x200) average filter initialized
+
+struct subscription_s
+{
+ byte type;
+ byte pin;
+ byte median_w;
+ uint16_t flags;
+ uint16_t result;
+ float average;
+ float median[3];
+ unsigned long last; // pacing
+ unsigned long pace; // pacing
+} subscription[SUBSCRIPTIONS];
+
+static float exponential_average(float old, float y, int bits)
+{
+ float mul = 1.0 / (1<<bits);
+ return old*(1-mul) + y*mul;
+}
+
+void setup_subscription(int n,int type,int pin,int flags)
+{
+ subscription[n].type=type;
+ subscription[n].pin=pin;
+ subscription[n].flags=flags; // clears median/average init flag
+ subscription[n].median_w=0;
+ subscription[n].last=micros();
+ subscription[n].pace=0;
+ subscription[n].result=0xfffe;
+ subscription[n].average=0.0;
+}
+
+static long ultrasonic_read(int pin,int max)
+{
+ pinMode(pin, OUTPUT);
+ digitalWrite(pin, LOW);
+ delayMicroseconds(2);
+ digitalWrite(pin, HIGH);
+ delayMicroseconds(5);
+ digitalWrite(pin,LOW);
+ pinMode(pin,INPUT);
+ return pulseIn(pin,HIGH,0xffff);
+}
+
+void execute_subscription(struct subscription_s *sub)
+{
+ float value=0.0;
+
+ unsigned long now=micros();
+ if (sub->last && now-sub->last<sub->pace) return;
+ sub->last=now;
+
+ switch (sub->type)
+ {
+ case 3: // analog read
+ value = analogRead(sub->pin);
+ sub->pace = 10000; // 100Hz
+ break;
+ case 7: // ultrasonic read, long distance (~11 meters)
+ value = ultrasonic_read(sub->pin,0xffff);
+ sub->pace = 500000; // 2Hz
+ break;
+ case 8: // ultrasonic read, short distance (~1 meter)
+ value = ultrasonic_read(sub->pin,1000);
+ sub->pace = 100000; // 10Hz
+ break;
+
+ case 0:
+ default: // do nothing
+ return;
+ }
+
+ // value=500;
+
+ // value=sub->flags;
+ // value+=(sub->flags*256);
+
+#if 1
+ // median
+ if ( (sub->flags & 0x08) )
+ {
+ sub->median[sub->median_w++]=value;
+ if (sub->median_w>=3)
+ {
+ sub->median_w=0;
+ sub->flags|=0x100;
+ }
+ // leave value if we're not initialized
+ if (sub->flags&0x100)
+ {
+ // sort the three values, pick the middle
+ float a=sub->median[0];
+ float b=sub->median[1];
+ float c=sub->median[2];
+ float t;
+ if (b>a) t=a,a=b,b=t;
+ if (c>b) t=b,b=c,c=t;
+ if (b>a) t=a,a=b,b=t;
+ value=b;
+ }
+ }
+
+ // average
+ if ( (sub->flags & 0x07) )
+ {
+ if (!(sub->flags & 0x200)) // initialize
+ {
+ sub->average=value;
+ sub->flags|=0x200;
+ }
+ value = sub->average =
+ exponential_average(sub->average,value,sub->flags&0x07);
+ }
+
+ if ( (sub->flags & 0x10) )
+ value*=16;
+#endif
+
+ value=round(value);
+
+ if (value<0) value=0;
+ else if (value>65535) value=65535;
+
+ sub->result=(uint16_t)value;
+}
+
+
+void execute_subscriptions()
+{
+ for (int i=0; i<SUBSCRIPTIONS; i++)
+ execute_subscription(subscription+i);
+}
+
void setup()
{
Serial.begin(9600); // start serial for output
@@ -46,6 +192,8 @@
void loop()
{
+ execute_subscriptions();
+
while (cmdw!=cmdr)
{
byte cmd[4];
@@ -507,6 +655,30 @@
// end Grove Chainable RGB LED
+ // ----------------
+ // subscriptions
+
+ if (cmd[0]>=100 && cmd[0]<=115) // subscribe n-100, type, pin, flags
+ {
+ setup_subscription(cmd[0]-100,cmd[1],cmd[2],cmd[3]);
+ }
+
+ if (cmd[0]>=116) // read subscriptions cmd[1] to cmd[1]+cmd[2]
+ {
+ int base = cmd[1];
+ int nread = cmd[2];
+ if (base<0 || base>3) base=0;
+ if (SUBSCRIPTIONS-base<nread) nread=SUBSCRIPTIONS-base;
+ if (nread>16) nread=16; // 32 bytes
+
+ for (int i=0; i<nread; i++)
+ {
+ struct subscription_s *sub = subscription+i+base;
+ b[i*2 + 0] = (sub->result>>8)&0xff;
+ b[i*2 + 1] = (sub->result>>0)&0xff;
+ }
+ }
+
last_cmd=cmd[0];
}
@@ -548,5 +720,7 @@
Wire.write(b, 4);
else if(last_cmd == 30 || last_cmd == 40)
Wire.write(b, 9);
+ else if (last_cmd>=116 && last_cmd<=119)
+ Wire.write(b, 32);
}
I’d be happy if these ideas are added to the normal distributions. I’m sure others would find this useful. There should be nothing that will cause problems or backwards compatibility issues in these patches.