File content as of revision 3:5b09cb69d476:

/*                                      PROBLEM
    Należy utworzyć wektor ze zmiennych X, Y pozwalający przypisać odpowiednie wartośc PWM do mostka H
    Wartości te muszą umożliwiać jazde robota w wybranym kierunku     
*/


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


AnalogIn joy_X(A0);
AnalogIn joy_Y(A1);
InterruptIn SW(USER_BUTTON);


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
        
            //ZMIENNA X
        if(X0>100 && X0<150){  //uwzględniam drganie styków
            X1=0;
        }else if((X0)<100){
            X1=map(X0,0, 100, -256, 0);               
        }else if((X0)>150){
            X1=map(X0,150, 256, 0, 256);                    
        } 
        X=X1/512;  
        
            //ZMIENNA Y
        if(Y0>100 && Y0<150){  //uwzględniam drganie styków
            Y1=0;
        }else if((Y0)<100){
            Y1=map(Y0,0, 100, -256, 0);               
        }else if((Y0)>150){
            Y1=map(Y0,150, 256, 0, 256);                    
        } 
        Y=Y1/512;  
                  
    //----------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--------
    
    
       if(SW!=1){
            mot_A.speed(Y); 
            mot_B.speed(Y); 
        }else{
            mot_A.speed(0); 
            mot_B.speed(0);
        }
    }
}