File content as of revision 23:de19f4304f11:

#define BEAT 140
#include "mbed.h"
#include "math.h"
#include "omni_wheel.h"
#include "motorsmlap.h"
#include "PID.h"
#include "Pixy.h"

PID pixydate_x(2.0,0,0,0.01);
PID pixydate_y(2.0,0,0,0.01);
Serial pc(USBTX, USBRX, 115200);
SPI spi(PB_15, PB_14, PB_13);
PixySPI pixy(&spi, &pc);
PwmOut beep(PA_0);
DigitalOut led0(PB_5);
DigitalOut led1(PA_1);
DigitalOut led2(LED2);

motorSmLap motor[] = {
    motorSmLap(PC_6, PC_9, PC_8),
    motorSmLap(PB_6, PA_9, PC_7),
    motorSmLap(PA_8, PB_4, PB_7)
    };

const double PII = 3.14159265358979;
OmniWheel omni(3);

void setup(){
        omni.wheel[0].setRadian(PII/4.0);
        omni.wheel[1].setRadian(3.0/4.0*PII);
        omni.wheel[2].setRadian(3.0/2.0*PII);

        beep.period(1.0/3136);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/2960);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/2489);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/1760);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/1661);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/2637);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/3322);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.period(1.0/4186);
        beep.write(0.4f);
        wait_ms(BEAT);

        beep.write(0.0f);
        wait_ms(BEAT);
}
int main() {
    setup();
    float x, y,now_angle;
    int pixy_x,pixy_y;

    pixydate_x.setInputLimits(0,255.0);
    pixydate_x.setOutputLimits(-1,1);
    pixydate_x.setBias(0);
    pixydate_x.setMode(1);
    pixydate_x.setSetPoint(175);

    pixydate_y.setInputLimits(0,255.0);
    pixydate_y.setOutputLimits(-1,1);
    pixydate_y.setBias(0);
    pixydate_y.setMode(1);
    pixydate_y.setSetPoint(180);

    ServoLoop panLoop(-150, -300);
    ServoLoop tiltLoop(200, 250);
    static int i = 0;
    int j;
    uint16_t blocks;
    int32_t panError, tiltError;
    pixy.init();

    while (true) {
        led0=1;
        led1=0;
        blocks = pixy.getBlocks();
        if (blocks) {
            panError = X_CENTER - pixy.blocks[0].x;
            tiltError = pixy.blocks[0].y - Y_CENTER;

            panLoop.update(panError);
            tiltLoop.update(tiltError);

            pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
            i++;

            if (i % 50 == 0 && pc != NULL) {
                pc.printf("Detected %d:\n", blocks);
                pc.printf("x=%d: ", pixy.getxxx(pixy.blocks[0]));
                pc.printf("Y=%d: ", pixy.getyyy(pixy.blocks[0]));
                pixy.printBlock(pixy.blocks[0]);
            }
        }
        
        pixydate_x.setProcessValue(pixy.getxxx(pixy.blocks[0]));
        pixydate_y.setProcessValue(pixy.getyyy(pixy.blocks[0]));
        
        omni.computeXY(
            pixydate_x.compute(),
            pixydate_y.compute(),
            0,
            0,
            0
            );
        for(int i=0; i<3; i++){
          motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
        }
      }
}