Mateusz Kociołek / Mbed 2 deprecated Samochodzik_sterowanie

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?

UserRevisionLine numberNew 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 }