Diff: OB1203.cpp

Revision:

19:a27b98998edf

Parent:

16:d5fe5debb5d2

Child:

20:8d69cd11b8fa

Child:

25:ca9caacd0f9f

--- a/OB1203.cpp	Fri Oct 05 16:55:14 2018 +0000
+++ b/OB1203.cpp	Fri Oct 12 20:19:38 2018 +0000
@@ -1,11 +1,10 @@
 #include "OB1203.h"
 #include "mbed.h"
-//#include "SoftI2C.h"
 
 extern Serial pc;
 
-OB1203::OB1203(I2C *i2c_obj)
-//OB1203::OB1203(SoftI2C *i2c_obj)
+//  //
+OB1203::OB1203(I2C *i2c_obj) 
 {
     i2c = i2c_obj;
 }
@@ -462,4 +461,44 @@
 {
     readBlock(OB1203_ADDR,REG_PART_ID,data,1);
     return data[0];
+}
+
+
+void OB1203::do_agc(uint32_t data, bool ch)
+{
+    const uint32_t tol1 = TOL1;
+    const uint32_t tol2 = TOL2;
+    const uint16_t in_range_persist = IN_RANGE_PERSIST;
+    static uint16_t in_range[2] = {0,0};
+    const uint16_t maxCurrent[2] = {IR_MAX_CURRENT , R_MAX_CURRENT};
+    const uint16_t step = STEP;
+    const uint32_t targetCounts[2] = {IR_TARGET_COUNTS, R_TARGET_COUNTS};
+     //ch = 0 for IR, 1 for R (channel)
+     if( data > targetCounts[ch] + (in_range[ch]>in_range_persist ? tol2: tol1) )
+    {       
+        if(data>targetCounts[ch] + tol2)
+            in_range[ch]=0;
+        
+       
+        if( (ch ? r_current : ir_current)>step)
+        {
+            (ch ? r_current : ir_current) -= step;
+            update = 1;
+        }
+    }
+    else if( data < targetCounts[ch] - (in_range[ch]>in_range_persist ? tol2 : tol1) )
+    {
+        if(data<targetCounts[ch] - tol2)
+            in_range[ch]=0;
+        if( (ch ? r_current : ir_current) +step<maxCurrent[ch]) //no need to go to full current
+        {
+            (ch ? r_current : ir_current) += step;
+            update = 1;
+        }
+    }
+    else
+    {
+        if( (data > (targetCounts[ch]-tol1) ) && (data < (targetCounts[ch]+tol1)) )
+            in_range[ch]++;
+    }
 }
\ No newline at end of file