Mateusz Kociołek / Mbed 2 deprecated Samochodzik_sterowanie

Opis znajduje sie w main.cpp

Dependencies:   mbed Motordriver

main1.cpp@2:ef1505fe5b96, 2019-03-02 (annotated)

Committer:
kociol1994
Date:
Sat Mar 02 18:59:32 2019 +0000
Revision:
2:ef1505fe5b96
Child:
3:5b09cb69d476
habababa;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kociol1994 2:ef1505fe5b96 1 /* PROBLEM
kociol1994 2:ef1505fe5b96 2 Należy utworzyć wektor ze zmiennych X, Y pozwalający przypisać odpowiednie wartośc PWM do mostka H
kociol1994 2:ef1505fe5b96 3 Wartości te muszą umożliwiać jazde robota w wybranym kierunku
kociol1994 2:ef1505fe5b96 4 */
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 2:ef1505fe5b96 12
kociol1994 2:ef1505fe5b96 13 Motor mot_A(D6, D7, D4, 1); // pwm, fwd, rev, can brake
kociol1994 2:ef1505fe5b96 14 Motor mot_B(D5, D2, D3, 1); // pwm, fwd, rev, can brake
kociol1994 2:ef1505fe5b96 15
kociol1994 2:ef1505fe5b96 16
kociol1994 2:ef1505fe5b96 17 AnalogIn joy_X(A0);
kociol1994 2:ef1505fe5b96 18 AnalogIn joy_Y(A1);
kociol1994 2:ef1505fe5b96 19 InterruptIn SW(USER_BUTTON);
kociol1994 2:ef1505fe5b96 20
kociol1994 2:ef1505fe5b96 21
kociol1994 2:ef1505fe5b96 22 Serial pc(SERIAL_TX, SERIAL_RX);
kociol1994 2:ef1505fe5b96 23
kociol1994 2:ef1505fe5b96 24
kociol1994 2:ef1505fe5b96 25 //--------funkcja mapująca zmienne X i Y-----------------
kociol1994 2:ef1505fe5b96 26 float map(float Q, float A, float B, float C, float D){
kociol1994 2:ef1505fe5b96 27 return ((D-C)/(B-A)*(Q-A)+C);
kociol1994 2:ef1505fe5b96 28 }
kociol1994 2:ef1505fe5b96 29
kociol1994 2:ef1505fe5b96 30 //---------------------------------------------------------------------------------------------
kociol1994 2:ef1505fe5b96 31 int main() {
kociol1994 2:ef1505fe5b96 32 while(1){
kociol1994 2:ef1505fe5b96 33
kociol1994 2:ef1505fe5b96 34 // --------pobierz dane z JOY----------
kociol1994 2:ef1505fe5b96 35 X0 = joy_X.read()*256;
kociol1994 2:ef1505fe5b96 36 Y0 = joy_Y.read()*256;
kociol1994 2:ef1505fe5b96 37
kociol1994 2:ef1505fe5b96 38 //--------mapowanie zmiennych ----------
kociol1994 2:ef1505fe5b96 39 //po wykonaniu tej części otrzymujemy zmienną X i Y z wartościami w zakresie -1 do 1
kociol1994 2:ef1505fe5b96 40
kociol1994 2:ef1505fe5b96 41 //ZMIENNA X
kociol1994 2:ef1505fe5b96 42 if(X0>100 && X0<150){ //uwzględniam drganie styków
kociol1994 2:ef1505fe5b96 43 X1=0;
kociol1994 2:ef1505fe5b96 44 }else if((X0)<100){
kociol1994 2:ef1505fe5b96 45 X1=map(X0,0, 100, -256, 0);
kociol1994 2:ef1505fe5b96 46 }else if((X0)>150){
kociol1994 2:ef1505fe5b96 47 X1=map(X0,150, 256, 0, 256);
kociol1994 2:ef1505fe5b96 48 }
kociol1994 2:ef1505fe5b96 49 X=X1/256;
kociol1994 2:ef1505fe5b96 50
kociol1994 2:ef1505fe5b96 51 //ZMIENNA Y
kociol1994 2:ef1505fe5b96 52 if(Y0>100 && Y0<150){ //uwzględniam drganie styków
kociol1994 2:ef1505fe5b96 53 Y1=0;
kociol1994 2:ef1505fe5b96 54 }else if((Y0)<100){
kociol1994 2:ef1505fe5b96 55 Y1=map(Y0,0, 100, -256, 0);
kociol1994 2:ef1505fe5b96 56 }else if((Y0)>150){
kociol1994 2:ef1505fe5b96 57 Y1=map(Y0,150, 256, 0, 256);
kociol1994 2:ef1505fe5b96 58 }
kociol1994 2:ef1505fe5b96 59 Y=Y1/256;
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 2:ef1505fe5b96 68 printf("X0=%.0f ", X0);
kociol1994 2:ef1505fe5b96 69 printf("X1=%.0f ", X1);
kociol1994 2:ef1505fe5b96 70 printf("X =%.0f ", X);
kociol1994 2:ef1505fe5b96 71
kociol1994 2:ef1505fe5b96 72 printf(" Y0=%.0f ", Y0);
kociol1994 2:ef1505fe5b96 73 printf("Y1=%.0f ", Y1);
kociol1994 2:ef1505fe5b96 74 printf("Y =%.0f \r\n", Y);
kociol1994 2:ef1505fe5b96 75
kociol1994 2:ef1505fe5b96 76 wait_ms(50);
kociol1994 2:ef1505fe5b96 77
kociol1994 2:ef1505fe5b96 78 // ------------wpisz wartosc PWM--------
kociol1994 2:ef1505fe5b96 79
kociol1994 2:ef1505fe5b96 80
kociol1994 2:ef1505fe5b96 81 if(SW!=1){
kociol1994 2:ef1505fe5b96 82 mot_A.speed(Y);
kociol1994 2:ef1505fe5b96 83 mot_B.speed(Y);
kociol1994 2:ef1505fe5b96 84 }else{
kociol1994 2:ef1505fe5b96 85 mot_A.speed(0);
kociol1994 2:ef1505fe5b96 86 mot_B.speed(0);
kociol1994 2:ef1505fe5b96 87 }
kociol1994 2:ef1505fe5b96 88 }
kociol1994 2:ef1505fe5b96 89 }