main1.cpp

00001 /*                            KocioŁ
00002           
00003     Program pobiera stan aktualny z Joy i przetwarza go na porządane wartości PWM
00004     umożliwiające przemieszczanie się pojazdu w kierunkach wskazanych przez operatora. 
00005 */
00006 
00007 #include "mbed.h"
00008 #include "motordriver.h"
00009     
00010 float X0, X1, X;
00011 float Y0, Y1, Y;    
00012 int dzielnik = 2;  //dzielnik prędkosci PWM
00013     
00014 Motor mot_A(D6, D4, D7, 1); // pwm, fwd, rev, can brake 
00015 Motor mot_B(D5, D3, D2, 1); // pwm, fwd, rev, can brake
00016 
00017     // Pobranie pozycji Joy-a
00018 AnalogIn joy_X(A0);
00019 AnalogIn joy_Y(A1);
00020 
00021     // inicjacja portu szeregowego 
00022 Serial pc(SERIAL_TX, SERIAL_RX);
00023 
00024     //--------funkcja mapująca zmienne X i Y-----------------
00025     float map(float Q, float A, float B, float C, float D){
00026             return ((D-C)/(B-A)*(Q-A)+C);                    
00027 }  
00028 
00029 //---------------------------------------------------------------------------------------------
00030 int main() {
00031         while(1){
00032             
00033     // --------pobierz dane z JOY----------
00034         X0 = joy_X.read()*256;
00035         Y0 = joy_Y.read()*256;
00036         
00037     //--------mapowanie zmiennych ----------
00038         // po wykonaniu tej części otrzymujemy zmienną X i Y z wartościami w zakresie -1 do 1
00039         // oraz zmienne X1 i Y1 używane w diagnostyce z wartościami w zakresie -255 do 255
00040         
00041             //ZMIENNA X
00042         if(X0>180 && X0<200){  //uwzględniam drganie styków
00043             X1=0;
00044         }else if((X0)<180){
00045             X1=map(X0,0, 180, -256, 0);               
00046         }else if((X0)>200){
00047             X1=map(X0,200, 256, 0, 256);                    
00048         } 
00049         X=X1/256;  // zmiana wartości na zakres od -1 do 1
00050         
00051             //ZMIENNA Y
00052         if(Y0>180 && Y0<200){  //uwzględniam drganie styków
00053             Y1=0;
00054         }else if((Y0)<180){
00055             Y1=map(Y0,0, 180, -256, 0);               
00056         }else if((Y0)>200){
00057             Y1=map(Y0,200, 256, 0, 256);                    
00058         } 
00059         Y=Y1/256;  // zmiana wartości na zakres od -1 do 1
00060                   
00061     //----------wyświetl diagnostyke---------
00062             /*
00063             X0/Y0 wartość prosto z wejść analogowych
00064             X1/Y1 wartość PWM z zakresu -255 do 255
00065             X /Y  wartość PWM z zakresu -1   do 1 
00066             */
00067         printf("Pomiar: ");
00068         printf("X0=%.0f    ",      X0);
00069         printf("X1=%.0f    ",      X1);
00070         printf("X =%.0f    ",      X );
00071         
00072         printf("     Y0=%.0f    ", Y0);
00073         printf("Y1=%.0f    ",      Y1);
00074         printf("Y =%.0f    \r\n ", Y );
00075         
00076         wait_ms(50);    
00077     
00078     // --------wpisz wartosc PWM-----------
00079             //zależność między położeniem Joy a obrotami kół
00080             float L = Y + (X/2);
00081             float P = Y - (X/2);
00082             
00083             mot_A.speed(L/dzielnik); 
00084             mot_B.speed(P/dzielnik); 
00085     }
00086 }