Diff: main.cpp

Revision:

1:c706136652de

Parent:

0:63532ae95efe

--- a/main.cpp	Sun Nov 16 11:54:19 2014 +0000
+++ b/main.cpp	Fri Jul 31 19:25:45 2015 +0000
@@ -1,28 +1,108 @@
 #include "mbed.h"
 #include "Pixy.h"
+#include "Servo.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)
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 Pixy pixy(Pixy::SPI, D11, D12, D13);
 Serial pc(USBTX, USBRX);
+Servo myservo(p21);
+Servo myservo_y(p22);
 
-int main() {
+int main()
+{
     pixy.setSerialOutput(&pc);
     while (1) {
         static int i = 0;
         int j;
         uint16_t blocks;
-  
+
         blocks = pixy.getBlocks();
-  
+
+        // int t =  pixy.blocks[0].x;
+        // pc.printf("PanError_test %d:\n\r", t);
+
+
+
         if (blocks) {
             i++;
-    
+
             if (i % 50 == 0) {
+
                 pc.printf("Detected %d:\n\r", blocks);
                 for (j = 0; j < blocks; j++) {
                     pc.printf("  block %d: ", j);
                     pixy.blocks[j].print(pc);
                 }
             }
+            // Pan function &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&%%%
+            float p;// motor set value
+            float m_pos;
+            float panError ;
+            float prevError;
+            float kp = 0.25;
+            float kd = .001;
+            float vel;
+
+            panError = X_CENTER - pixy.blocks[0].x;
+            vel = panError*kp + (panError - prevError)*kd;
+            m_pos += vel;
+            if (m_pos<=0) {
+                m_pos = 0;
+            }
+            if (m_pos>=320) {
+                m_pos = 320;
+            }
+
+            p = m_pos/320;
+
+            myservo = p;// send position to p21( 0 - 1)
+            prevError = panError;
+            // Tilt function%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+            float p1;// motor set value
+            float m_pos_y;
+            float tiltError ;
+            float prevError_y;
+            float kp_y = 0.2;
+            float kd_y = 0.01;
+            float vel_y;
+
+            tiltError =  Y_CENTER - pixy.blocks[0].y;
+            float y = pixy.blocks[0].y;
+            vel_y = tiltError*kp_y + (tiltError - prevError_y)*kd_y;
+            m_pos_y += vel_y;
+            //int m_pos_y2 = m_pos_y + vel_y;
+            if (m_pos_y<=-200) {
+                m_pos_y = 0;
+            }
+            if (m_pos_y>=200) {
+                m_pos_y = 200;
+            }
+
+            p1 = (m_pos_y - 30)/-60;
+
+            myservo_y = p1;// send position to p22( 0 - 1)
+            prevError_y = tiltError;
+            //
+
+             //pc.printf("m_pos %f:\n\r",m_pos);
+           // pc.printf("PanError %f:\n\r", panError);
+            //pc.printf("Servo position %f:\n\r", p);
+            //pc.printf("Tilt Error %f : \n\r", tiltError);
+            //pc.printf("Y: %f  \n\r", y);
+            //pc.printf("vel_y %f : \n\r", vel_y);
+            //pc.printf("mpos_y: %f  \n\r", m_pos_y);
+           // pc.printf("Tilt Servo: %f  \n\r", p1);
+            // pc.printf("mpos_y2: %d  \n\r", m_pos_y2);
+
+
+
         }
-    }  
+    }
 }
\ No newline at end of file