![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Pixy pan/tilt
Fork of pixyHelloWorld by
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