Diff: main.cpp

Revision:

0:f533d77ff012

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 04 00:04:38 2015 +0000
@@ -0,0 +1,61 @@
+// Hello World to sweep a servo through its full range
+ 
+#include "mbed.h"
+#include "Servo.h"
+#include "Pixy.h"
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+#define X_CENTER 160
+#define Y_CENTER 100
+#define RCS_MIN_POS 0
+#define RCS_MAX_POS 1
+#define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)
+
+Ticker loop;
+Pixy pixy(Pixy::SPI, D11, D12, D13);
+Servo myservo(p21);
+Serial pc(USBTX, USBRX);
+ float vel;
+ float panError;
+float previousError;
+float panIntegral;
+void PID (){
+            uint16_t blocks;
+            blocks = pixy.getBlocks();
+            if (blocks){ 
+            previousError = panError;
+            panError =  (float)(pixy.blocks[0].x - X_CENTER);
+            if( (panError < 8) && (panError > -8) ) 
+            {
+                panError = 0;
+                }
+            panIntegral = panIntegral + panError;
+            vel = 0.485 + panError*0.0008 + (panError-previousError)*0.001 + panIntegral*0.000001;
+           
+            if (vel>0.9){
+                vel = 0.9;
+            }
+            if (vel<0.1){
+                vel=0.1;
+            }
+            }
+            if(panIntegral > 10)
+            {
+                panIntegral = 10;
+            }else if(panIntegral <-10)
+            
+            {
+                panIntegral = -10;
+            }
+                
+        
+    }
+    
+int main() {    
+    panIntegral=0;
+    loop.attach(&PID, 0.02);
+     pc.printf("Pan Error %f\n",vel);
+     while(1){
+    myservo = vel;
+    }
+
+}
\ No newline at end of file