![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Pixy pan/tilt
Fork of pixyHelloWorld by
main.cpp
- Committer:
- LucasUIUC
- Date:
- 2015-07-31
- Revision:
- 1:c706136652de
- Parent:
- 0:63532ae95efe
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); } } }