Lucas Moreira / Mbed 2 deprecated pixy_pan_tilt

Pixy pan/tilt

Dependencies:   Servo mbed pixy

Fork of pixyHelloWorld by Arcadie Cracan

main.cpp@1:c706136652de, 2015-07-31 (annotated)

Committer:
LucasUIUC
Date:
Fri Jul 31 19:25:45 2015 +0000
Revision:
1:c706136652de
Parent:
0:63532ae95efe 
Pixy pan/tilt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:63532ae95efe 1 #include "mbed.h"
acracan 0:63532ae95efe 2 #include "Pixy.h"
LucasUIUC 1:c706136652de 3 #include "Servo.h"
LucasUIUC 1:c706136652de 4
LucasUIUC 1:c706136652de 5 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
LucasUIUC 1:c706136652de 6 #define X_CENTER 160
LucasUIUC 1:c706136652de 7 #define Y_CENTER 100
LucasUIUC 1:c706136652de 8 #define RCS_MIN_POS 0
LucasUIUC 1:c706136652de 9 #define RCS_MAX_POS 1
LucasUIUC 1:c706136652de 10 #define RCS_CENTER_POS ((RCS_MAX_POS-RCS_MIN_POS)/2)
LucasUIUC 1:c706136652de 11 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
acracan 0:63532ae95efe 12
acracan 0:63532ae95efe 13 Pixy pixy(Pixy::SPI, D11, D12, D13);
acracan 0:63532ae95efe 14 Serial pc(USBTX, USBRX);
LucasUIUC 1:c706136652de 15 Servo myservo(p21);
LucasUIUC 1:c706136652de 16 Servo myservo_y(p22);
acracan 0:63532ae95efe 17
LucasUIUC 1:c706136652de 18 int main()
LucasUIUC 1:c706136652de 19 {
acracan 0:63532ae95efe 20 pixy.setSerialOutput(&pc);
acracan 0:63532ae95efe 21 while (1) {
acracan 0:63532ae95efe 22 static int i = 0;
acracan 0:63532ae95efe 23 int j;
acracan 0:63532ae95efe 24 uint16_t blocks;
LucasUIUC 1:c706136652de 25
acracan 0:63532ae95efe 26 blocks = pixy.getBlocks();
LucasUIUC 1:c706136652de 27
LucasUIUC 1:c706136652de 28 // int t = pixy.blocks[0].x;
LucasUIUC 1:c706136652de 29 // pc.printf("PanError_test %d:\n\r", t);
LucasUIUC 1:c706136652de 30
LucasUIUC 1:c706136652de 31
LucasUIUC 1:c706136652de 32
acracan 0:63532ae95efe 33 if (blocks) {
acracan 0:63532ae95efe 34 i++;
LucasUIUC 1:c706136652de 35
acracan 0:63532ae95efe 36 if (i % 50 == 0) {
LucasUIUC 1:c706136652de 37
acracan 0:63532ae95efe 38 pc.printf("Detected %d:\n\r", blocks);
acracan 0:63532ae95efe 39 for (j = 0; j < blocks; j++) {
acracan 0:63532ae95efe 40 pc.printf(" block %d: ", j);
acracan 0:63532ae95efe 41 pixy.blocks[j].print(pc);
acracan 0:63532ae95efe 42 }
acracan 0:63532ae95efe 43 }
LucasUIUC 1:c706136652de 44 // Pan function &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&%%%
LucasUIUC 1:c706136652de 45 float p;// motor set value
LucasUIUC 1:c706136652de 46 float m_pos;
LucasUIUC 1:c706136652de 47 float panError ;
LucasUIUC 1:c706136652de 48 float prevError;
LucasUIUC 1:c706136652de 49 float kp = 0.25;
LucasUIUC 1:c706136652de 50 float kd = .001;
LucasUIUC 1:c706136652de 51 float vel;
LucasUIUC 1:c706136652de 52
LucasUIUC 1:c706136652de 53 panError = X_CENTER - pixy.blocks[0].x;
LucasUIUC 1:c706136652de 54 vel = panError*kp + (panError - prevError)*kd;
LucasUIUC 1:c706136652de 55 m_pos += vel;
LucasUIUC 1:c706136652de 56 if (m_pos<=0) {
LucasUIUC 1:c706136652de 57 m_pos = 0;
LucasUIUC 1:c706136652de 58 }
LucasUIUC 1:c706136652de 59 if (m_pos>=320) {
LucasUIUC 1:c706136652de 60 m_pos = 320;
LucasUIUC 1:c706136652de 61 }
LucasUIUC 1:c706136652de 62
LucasUIUC 1:c706136652de 63 p = m_pos/320;
LucasUIUC 1:c706136652de 64
LucasUIUC 1:c706136652de 65 myservo = p;// send position to p21( 0 - 1)
LucasUIUC 1:c706136652de 66 prevError = panError;
LucasUIUC 1:c706136652de 67 // Tilt function%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
LucasUIUC 1:c706136652de 68 float p1;// motor set value
LucasUIUC 1:c706136652de 69 float m_pos_y;
LucasUIUC 1:c706136652de 70 float tiltError ;
LucasUIUC 1:c706136652de 71 float prevError_y;
LucasUIUC 1:c706136652de 72 float kp_y = 0.2;
LucasUIUC 1:c706136652de 73 float kd_y = 0.01;
LucasUIUC 1:c706136652de 74 float vel_y;
LucasUIUC 1:c706136652de 75
LucasUIUC 1:c706136652de 76 tiltError = Y_CENTER - pixy.blocks[0].y;
LucasUIUC 1:c706136652de 77 float y = pixy.blocks[0].y;
LucasUIUC 1:c706136652de 78 vel_y = tiltError*kp_y + (tiltError - prevError_y)*kd_y;
LucasUIUC 1:c706136652de 79 m_pos_y += vel_y;
LucasUIUC 1:c706136652de 80 //int m_pos_y2 = m_pos_y + vel_y;
LucasUIUC 1:c706136652de 81 if (m_pos_y<=-200) {
LucasUIUC 1:c706136652de 82 m_pos_y = 0;
LucasUIUC 1:c706136652de 83 }
LucasUIUC 1:c706136652de 84 if (m_pos_y>=200) {
LucasUIUC 1:c706136652de 85 m_pos_y = 200;
LucasUIUC 1:c706136652de 86 }
LucasUIUC 1:c706136652de 87
LucasUIUC 1:c706136652de 88 p1 = (m_pos_y - 30)/-60;
LucasUIUC 1:c706136652de 89
LucasUIUC 1:c706136652de 90 myservo_y = p1;// send position to p22( 0 - 1)
LucasUIUC 1:c706136652de 91 prevError_y = tiltError;
LucasUIUC 1:c706136652de 92 //
LucasUIUC 1:c706136652de 93
LucasUIUC 1:c706136652de 94 //pc.printf("m_pos %f:\n\r",m_pos);
LucasUIUC 1:c706136652de 95 // pc.printf("PanError %f:\n\r", panError);
LucasUIUC 1:c706136652de 96 //pc.printf("Servo position %f:\n\r", p);
LucasUIUC 1:c706136652de 97 //pc.printf("Tilt Error %f : \n\r", tiltError);
LucasUIUC 1:c706136652de 98 //pc.printf("Y: %f \n\r", y);
LucasUIUC 1:c706136652de 99 //pc.printf("vel_y %f : \n\r", vel_y);
LucasUIUC 1:c706136652de 100 //pc.printf("mpos_y: %f \n\r", m_pos_y);
LucasUIUC 1:c706136652de 101 // pc.printf("Tilt Servo: %f \n\r", p1);
LucasUIUC 1:c706136652de 102 // pc.printf("mpos_y2: %d \n\r", m_pos_y2);
LucasUIUC 1:c706136652de 103
LucasUIUC 1:c706136652de 104
LucasUIUC 1:c706136652de 105
acracan 0:63532ae95efe 106 }
LucasUIUC 1:c706136652de 107 }
acracan 0:63532ae95efe 108 }