main.cpp

00001 #include "mbed.h"
00002 #include "Pixy.h"
00003 #include "PID_Control.h"
00004 
00005 //------------------------------------
00006 // Example for PIxy UART connection.
00007 // Driver version 0.1
00008 // BUGS::
00009 // DO NOT USE PRINTF IN MAIN LOOP FOR PROPER OPERATION
00010 // Pixy has to be configured for UART with 460800 Baud
00011 // Pixy connected by UART (PA_9 and PA_10) with 230400 Baud
00012 //------------------------------------
00013 
00014 //communication
00015 Serial pc(USBTX, USBRX);
00016 Serial cam(PA_9, PA_10);
00017 
00018 DigitalOut myled(LED1);
00019 
00020 //motor stuff
00021 DigitalOut enableMotorDriver(PB_2);
00022 PwmOut pwmL(PA_8);
00023 PwmOut pwmR(PA_9);
00024 
00025 Pixy pixy(cam);
00026 
00027 int main()
00028 {
00029     PID_Control pid;
00030     
00031     pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
00032     
00033     pc.baud( 115200 );
00034     pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
00035     pwmR.period(0.00005f);
00036     enableMotorDriver = 1;
00037 
00038     //int skipper = 0;
00039 
00040     while(1) {
00041         wait( 0.005f );
00042 
00043         //  if( ++skipper % 100 == 0)
00044         //      printf("x=%d\n\r", pixy.getX());
00045 
00046         float e = 160-pixy.getX();
00047         float diff = pid.calc( e, 0.005f );
00048         
00049         pwmL = 0.5f - diff;
00050         pwmR = 0.5f - diff;
00051     }
00052 }