main1.cpp

00001 /*                                      PROBLEM
00002     Należy utworzyć wektor ze zmiennych X, Y pozwalający przypisać odpowiednie wartośc PWM do mostka H
00003     Wartości te muszą umożliwiać jazde robota w wybranym kierunku     
00004 */
00005 
00006 
00007 #include "mbed.h"
00008 #include "motordriver.h"
00009     
00010 float X0, X1, X;
00011 float Y0, Y1, Y;    
00012     
00013 Motor mot_A(D6, D7, D4, 1); // pwm, fwd, rev, can brake 
00014 Motor mot_B(D5, D2, D3, 1); // pwm, fwd, rev, can brake
00015 
00016 
00017 AnalogIn joy_X(A0);
00018 AnalogIn joy_Y(A1);
00019 InterruptIn SW(USER_BUTTON);
00020 
00021 
00022 Serial pc(SERIAL_TX, SERIAL_RX);
00023 
00024 
00025 //--------funkcja mapująca zmienne X i Y-----------------
00026     float map(float Q, float A, float B, float C, float D){
00027             return ((D-C)/(B-A)*(Q-A)+C);                    
00028 }  
00029 
00030 //---------------------------------------------------------------------------------------------
00031 int main() {
00032         while(1){
00033             
00034     // --------pobierz dane z JOY----------
00035         X0 = joy_X.read()*256;
00036         Y0 = joy_Y.read()*256;
00037         
00038     //--------mapowanie zmiennych ----------
00039         //po wykonaniu tej części otrzymujemy zmienną X i Y z wartościami w zakresie -1 do 1
00040         
00041             //ZMIENNA X
00042         if(X0>100 && X0<150){  //uwzględniam drganie styków
00043             X1=0;
00044         }else if((X0)<100){
00045             X1=map(X0,0, 100, -256, 0);               
00046         }else if((X0)>150){
00047             X1=map(X0,150, 256, 0, 256);                    
00048         } 
00049         X=X1/512;  
00050         
00051             //ZMIENNA Y
00052         if(Y0>100 && Y0<150){  //uwzględniam drganie styków
00053             Y1=0;
00054         }else if((Y0)<100){
00055             Y1=map(Y0,0, 100, -256, 0);               
00056         }else if((Y0)>150){
00057             Y1=map(Y0,150, 256, 0, 256);                    
00058         } 
00059         Y=Y1/512;  
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     
00080     
00081        if(SW!=1){
00082             mot_A.speed(Y); 
00083             mot_B.speed(Y); 
00084         }else{
00085             mot_A.speed(0); 
00086             mot_B.speed(0);
00087         }
00088     }
00089 }