Opis znajduje sie w main.cpp
Dependencies: mbed Motordriver
main1.cpp@5:765e3090ce0c, 2019-03-05 (annotated)
- Committer:
- kociol1994
- Date:
- Tue Mar 05 10:14:36 2019 +0000
- Revision:
- 5:765e3090ce0c
- Parent:
- 4:2171346afc64
abc;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kociol1994 | 4:2171346afc64 | 1 | /* KocioŁ |
kociol1994 | 4:2171346afc64 | 2 | |
kociol1994 | 4:2171346afc64 | 3 | Program pobiera stan aktualny z Joy i przetwarza go na porządane wartości PWM |
kociol1994 | 4:2171346afc64 | 4 | umożliwiające przemieszczanie się pojazdu w kierunkach wskazanych przez operatora. |
kociol1994 | 2:ef1505fe5b96 | 5 | */ |
kociol1994 | 2:ef1505fe5b96 | 6 | |
kociol1994 | 2:ef1505fe5b96 | 7 | #include "mbed.h" |
kociol1994 | 2:ef1505fe5b96 | 8 | #include "motordriver.h" |
kociol1994 | 2:ef1505fe5b96 | 9 | |
kociol1994 | 2:ef1505fe5b96 | 10 | float X0, X1, X; |
kociol1994 | 2:ef1505fe5b96 | 11 | float Y0, Y1, Y; |
kociol1994 | 4:2171346afc64 | 12 | int dzielnik = 2; //dzielnik prędkosci PWM |
kociol1994 | 2:ef1505fe5b96 | 13 | |
kociol1994 | 4:2171346afc64 | 14 | Motor mot_A(D6, D4, D7, 1); // pwm, fwd, rev, can brake |
kociol1994 | 4:2171346afc64 | 15 | Motor mot_B(D5, D3, D2, 1); // pwm, fwd, rev, can brake |
kociol1994 | 2:ef1505fe5b96 | 16 | |
kociol1994 | 4:2171346afc64 | 17 | // Pobranie pozycji Joy-a |
kociol1994 | 2:ef1505fe5b96 | 18 | AnalogIn joy_X(A0); |
kociol1994 | 2:ef1505fe5b96 | 19 | AnalogIn joy_Y(A1); |
kociol1994 | 2:ef1505fe5b96 | 20 | |
kociol1994 | 4:2171346afc64 | 21 | // inicjacja portu szeregowego |
kociol1994 | 2:ef1505fe5b96 | 22 | Serial pc(SERIAL_TX, SERIAL_RX); |
kociol1994 | 2:ef1505fe5b96 | 23 | |
kociol1994 | 5:765e3090ce0c | 24 | //--------funkcja mapująca zmienne X i Y----------------- |
kociol1994 | 2:ef1505fe5b96 | 25 | float map(float Q, float A, float B, float C, float D){ |
kociol1994 | 2:ef1505fe5b96 | 26 | return ((D-C)/(B-A)*(Q-A)+C); |
kociol1994 | 2:ef1505fe5b96 | 27 | } |
kociol1994 | 2:ef1505fe5b96 | 28 | |
kociol1994 | 2:ef1505fe5b96 | 29 | //--------------------------------------------------------------------------------------------- |
kociol1994 | 2:ef1505fe5b96 | 30 | int main() { |
kociol1994 | 2:ef1505fe5b96 | 31 | while(1){ |
kociol1994 | 2:ef1505fe5b96 | 32 | |
kociol1994 | 2:ef1505fe5b96 | 33 | // --------pobierz dane z JOY---------- |
kociol1994 | 2:ef1505fe5b96 | 34 | X0 = joy_X.read()*256; |
kociol1994 | 2:ef1505fe5b96 | 35 | Y0 = joy_Y.read()*256; |
kociol1994 | 2:ef1505fe5b96 | 36 | |
kociol1994 | 2:ef1505fe5b96 | 37 | //--------mapowanie zmiennych ---------- |
kociol1994 | 4:2171346afc64 | 38 | // po wykonaniu tej części otrzymujemy zmienną X i Y z wartościami w zakresie -1 do 1 |
kociol1994 | 4:2171346afc64 | 39 | // oraz zmienne X1 i Y1 używane w diagnostyce z wartościami w zakresie -255 do 255 |
kociol1994 | 2:ef1505fe5b96 | 40 | |
kociol1994 | 2:ef1505fe5b96 | 41 | //ZMIENNA X |
kociol1994 | 4:2171346afc64 | 42 | if(X0>180 && X0<200){ //uwzględniam drganie styków |
kociol1994 | 2:ef1505fe5b96 | 43 | X1=0; |
kociol1994 | 4:2171346afc64 | 44 | }else if((X0)<180){ |
kociol1994 | 4:2171346afc64 | 45 | X1=map(X0,0, 180, -256, 0); |
kociol1994 | 4:2171346afc64 | 46 | }else if((X0)>200){ |
kociol1994 | 4:2171346afc64 | 47 | X1=map(X0,200, 256, 0, 256); |
kociol1994 | 2:ef1505fe5b96 | 48 | } |
kociol1994 | 4:2171346afc64 | 49 | X=X1/256; // zmiana wartości na zakres od -1 do 1 |
kociol1994 | 2:ef1505fe5b96 | 50 | |
kociol1994 | 2:ef1505fe5b96 | 51 | //ZMIENNA Y |
kociol1994 | 4:2171346afc64 | 52 | if(Y0>180 && Y0<200){ //uwzględniam drganie styków |
kociol1994 | 2:ef1505fe5b96 | 53 | Y1=0; |
kociol1994 | 4:2171346afc64 | 54 | }else if((Y0)<180){ |
kociol1994 | 4:2171346afc64 | 55 | Y1=map(Y0,0, 180, -256, 0); |
kociol1994 | 4:2171346afc64 | 56 | }else if((Y0)>200){ |
kociol1994 | 4:2171346afc64 | 57 | Y1=map(Y0,200, 256, 0, 256); |
kociol1994 | 2:ef1505fe5b96 | 58 | } |
kociol1994 | 4:2171346afc64 | 59 | Y=Y1/256; // zmiana wartości na zakres od -1 do 1 |
kociol1994 | 2:ef1505fe5b96 | 60 | |
kociol1994 | 2:ef1505fe5b96 | 61 | //----------wyświetl diagnostyke--------- |
kociol1994 | 2:ef1505fe5b96 | 62 | /* |
kociol1994 | 2:ef1505fe5b96 | 63 | X0/Y0 wartość prosto z wejść analogowych |
kociol1994 | 2:ef1505fe5b96 | 64 | X1/Y1 wartość PWM z zakresu -255 do 255 |
kociol1994 | 2:ef1505fe5b96 | 65 | X /Y wartość PWM z zakresu -1 do 1 |
kociol1994 | 2:ef1505fe5b96 | 66 | */ |
kociol1994 | 2:ef1505fe5b96 | 67 | printf("Pomiar: "); |
kociol1994 | 4:2171346afc64 | 68 | printf("X0=%.0f ", X0); |
kociol1994 | 4:2171346afc64 | 69 | printf("X1=%.0f ", X1); |
kociol1994 | 4:2171346afc64 | 70 | printf("X =%.0f ", X ); |
kociol1994 | 2:ef1505fe5b96 | 71 | |
kociol1994 | 2:ef1505fe5b96 | 72 | printf(" Y0=%.0f ", Y0); |
kociol1994 | 4:2171346afc64 | 73 | printf("Y1=%.0f ", Y1); |
kociol1994 | 4:2171346afc64 | 74 | printf("Y =%.0f \r\n ", Y ); |
kociol1994 | 2:ef1505fe5b96 | 75 | |
kociol1994 | 2:ef1505fe5b96 | 76 | wait_ms(50); |
kociol1994 | 2:ef1505fe5b96 | 77 | |
kociol1994 | 4:2171346afc64 | 78 | // --------wpisz wartosc PWM----------- |
kociol1994 | 4:2171346afc64 | 79 | //zależność między położeniem Joy a obrotami kół |
kociol1994 | 4:2171346afc64 | 80 | float L = Y + (X/2); |
kociol1994 | 4:2171346afc64 | 81 | float P = Y - (X/2); |
kociol1994 | 4:2171346afc64 | 82 | |
kociol1994 | 4:2171346afc64 | 83 | mot_A.speed(L/dzielnik); |
kociol1994 | 4:2171346afc64 | 84 | mot_B.speed(P/dzielnik); |
kociol1994 | 2:ef1505fe5b96 | 85 | } |
kociol1994 | 2:ef1505fe5b96 | 86 | } |