File content as of revision 1:c706136652de:

#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()
{
    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);



        }
    }
}