Lucas Moreira
Pixy pan/tilt
Fork of
pixyHelloWorld
by 
Arcadie Cracan
Revision 1:c706136652de, committed 2015-07-31
- Comitter:
- LucasUIUC
- Date:
- Fri Jul 31 19:25:45 2015 +0000
- Parent:
- 0:63532ae95efe
- Commit message:
- Pixy pan/tilt
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Jul 31 19:25:45 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- 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
--- a/pixy.lib Sun Nov 16 11:54:19 2014 +0000 +++ b/pixy.lib Fri Jul 31 19:25:45 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/acracan/code/pixy/#ed8dc4531ac1 +http://developer.mbed.org/users/LucasUIUC/code/pixy/#f8b6497870d3