File content as of revision 5:765e3090ce0c:

/*                            KocioŁ
          
    Program pobiera stan aktualny z Joy i przetwarza go na porządane wartości PWM
    umożliwiające przemieszczanie się pojazdu w kierunkach wskazanych przez operatora. 
*/

#include "mbed.h"
#include "motordriver.h"
    
float X0, X1, X;
float Y0, Y1, Y;    
int dzielnik = 2;  //dzielnik prędkosci PWM
    
Motor mot_A(D6, D4, D7, 1); // pwm, fwd, rev, can brake 
Motor mot_B(D5, D3, D2, 1); // pwm, fwd, rev, can brake

    // Pobranie pozycji Joy-a
AnalogIn joy_X(A0);
AnalogIn joy_Y(A1);

    // inicjacja portu szeregowego 
Serial pc(SERIAL_TX, SERIAL_RX);

    //--------funkcja mapująca zmienne X i Y-----------------
    float map(float Q, float A, float B, float C, float D){
            return ((D-C)/(B-A)*(Q-A)+C);                    
}  

//---------------------------------------------------------------------------------------------
int main() {
        while(1){
            
    // --------pobierz dane z JOY----------
        X0 = joy_X.read()*256;
        Y0 = joy_Y.read()*256;
        
    //--------mapowanie zmiennych ----------
        // po wykonaniu tej części otrzymujemy zmienną X i Y z wartościami w zakresie -1 do 1
        // oraz zmienne X1 i Y1 używane w diagnostyce z wartościami w zakresie -255 do 255
        
            //ZMIENNA X
        if(X0>180 && X0<200){  //uwzględniam drganie styków
            X1=0;
        }else if((X0)<180){
            X1=map(X0,0, 180, -256, 0);               
        }else if((X0)>200){
            X1=map(X0,200, 256, 0, 256);                    
        } 
        X=X1/256;  // zmiana wartości na zakres od -1 do 1
        
            //ZMIENNA Y
        if(Y0>180 && Y0<200){  //uwzględniam drganie styków
            Y1=0;
        }else if((Y0)<180){
            Y1=map(Y0,0, 180, -256, 0);               
        }else if((Y0)>200){
            Y1=map(Y0,200, 256, 0, 256);                    
        } 
        Y=Y1/256;  // zmiana wartości na zakres od -1 do 1
                  
    //----------wyświetl diagnostyke---------
            /*
            X0/Y0 wartość prosto z wejść analogowych
            X1/Y1 wartość PWM z zakresu -255 do 255
            X /Y  wartość PWM z zakresu -1   do 1 
            */
        printf("Pomiar: ");
        printf("X0=%.0f    ",      X0);
        printf("X1=%.0f    ",      X1);
        printf("X =%.0f    ",      X );
        
        printf("     Y0=%.0f    ", Y0);
        printf("Y1=%.0f    ",      Y1);
        printf("Y =%.0f    \r\n ", Y );
        
        wait_ms(50);    
    
    // --------wpisz wartosc PWM-----------
            //zależność między położeniem Joy a obrotami kół
            float L = Y + (X/2);
            float P = Y - (X/2);
            
            mot_A.speed(L/dzielnik); 
            mot_B.speed(P/dzielnik); 
    }
}