File content as of revision 2:aa102819471f:

//Program na NUCLEO F429ZI

#include "mbed.h"
#include "BufferedSerial.h"
#include "LSM6DS33.h"
#include "math.h"
#include <stdio.h>

/*PwmOut rled(LED_RED);
PwmOut gled(LED_GREEN);
PwmOut bled(LED_BLUE);*/

//deklaracje funkcji
void testy_czujnika();          //wolne obroty wybranego przegubu
void zadajnik_odbiornik();      //zadawanie położenia odebranego z zadajnika
void sekwencja_pokazowa();      //przykładowa ścieżka ruchu urządzenia
void stabilizacja_smartfon();   //zadawanie położenia odebranego z aplikacji
void predkosc_kat();            //zamiana prędkosci obrotowej na kąt
void regulacja();               //funkcja regulacji
void regulacja2();              //funkcja regulacji 2
void stream_pc();               //wysyłanie danych na port usb
void stream_telefon();          //wysyłanie danych do modułu Bluetooth
void odbiornik();               //odbiór danych z zadajnika
void odbiornik2();              //odbiór danych z aplikacji mobilnej
void stabilizacja_zero();       //stabilizacja położenia dla stałej wartości 0
void odczyt_pozycji_gimbala();          //odczyt danych z czujnika Pololu MinIMU-9 v5
double arc_tan_2(double a, double b);       //funkcja zamiany przyśpieszenia na kąt

//BUTTON
DigitalOut led1(LED1);  //żółta
DigitalOut led2(LED2);  //niebieska
DigitalOut led3(LED3);  //czerwona
InterruptIn button1(USER_BUTTON);   //niebiski przycisk
volatile bool button1_pressed = false; // Użyte w pętli głównej
volatile bool button1_enabled = true; // Eliminacja wpływu drgania styków
Timeout button1_timeout; // Drganie styków - zwłoka

//TICKER
float sampling_time = 0.2;  //częstotiwosc zapisu na konsolę PC (terminal) w funkcji przerwania
Ticker trigger1;    //predkosc_kat
Ticker trigger2;    //stream na termial

//Wyjscia PWM dla trzech silników
PwmOut MOTOR_1(PB_3);   //ogniwo 1 - od zewnętrznej
PwmOut MOTOR_2(PB_4);   //ogniwo 2
PwmOut MOTOR_3(PB_5);   //ogniwo 3

DigitalOut OBROTY_M1(PE_13);    //Kierunek obrotów dla trzech silników(stan 0/1)
DigitalOut OBROTY_M2(PE_14);
DigitalOut OBROTY_M3(PE_15);

//=============REGULACJA======================
int obecna_pozycja=0;
float e_x=0, e_x_poprzednie=0;  //uchyby położeń kątowych
float e_y=0, e_y_poprzednie=0;
float e_z=0, e_z_poprzednie=0;
int docelowa_pozycja_x=0, docelowa_pozycja_y=0, docelowa_pozycja_z=0;   //wartosc zadana
double pwm1, pwm2, pwm3;    //sygnały sterujace

volatile double z_kat,x_kat,y_kat;  //wartosc aktualna

float Ix=0, Iy=0, Iz=0; //Stałe regulatora PID
float Dx=0, Dy=0, Dz=0;
float Kp=0.2, Ki=0.05, Kd=0.01; //0.2, 0.05, 0.01

//===============STREAM=======================
//Serial pc(USBTX,USBRX);     //BLUETOOTH                              // komunikacja z PC
BufferedSerial pc(USBTX,USBRX); //PC
Serial device_bt(PD_5, PD_6);  //TX, RX aplikacja/modul Bluetooth
Serial device_zadajnik(PC_10, PC_11);  //TX, RX zadajnik
char buffer[24];

//===============BUTTON========================
// Enables button when bouncing is over
void button1_enabled_cb(void)
{
    button1_enabled = true;
}

int tryb = 0;   //tryby pracy 1-5

// ISR handling button pressed event
void button1_onpressed_cb(void)
{
    if (button1_enabled) { // Disabled while the button is bouncing
        button1_enabled = false;
        button1_pressed = true; // To be read by the main loop
        button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
        tryb++;
        if(tryb>=6){tryb=1;}
    }
}
//===================KOMUNIKACJA Z CZUJNIKIEM===================================
volatile double a_x;     //deklaracja zmiennych akcelerometru
volatile double a_y;
volatile double a_z;

volatile double g_x;     //deklaracja zmiennych żyroskopu
volatile double g_y;
volatile double g_z;

volatile double z_stopnie;

//===================KOMUNIKACJA Z ZADAJNIKIEM==================================
volatile float X=0;   //zmiennne globalne - kąty obrotu zadajnika przesłane przez UART
volatile float Y=0; 
volatile float Z=0;

//======================PINY DLA CZUJNIKA=======================================
LSM6DS33 gimbal(PB_9, PB_8, LSM6DS33_AG_I2C_ADDR(1));    //akcelerometr i żyroskop

//===========PĘTLA GŁÓWNA=======================================================
int main()
{
    pc.baud(115200);
    device_bt.baud(9600);    //transmisja danych Bluetooth
    device_zadajnik.baud(115200);   //zadajnik - odbiornik
    
    //prędkosci silników(wypełnienie PWM)
    MOTOR_1.period(0.002f);  // 2 milisecond period (500Hz)
    MOTOR_1.write(1.0f);  // wypełnienie (1.0=0%, 0.0=100%) z krokiem 0.01f
    MOTOR_2.period(0.002f);  // 2 ms
    MOTOR_2.write(1.0f);  
    MOTOR_3.period(0.002f);  // 2 ms
    MOTOR_3.write(1.0f);
    
    //LSM6DS33 gimbal(PB_9, PB_8);    //akcelerometr i żyroskop
    wait(0.001);
    
    //PRZERWANIA================
    button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event -zwolnienie niebieskiego przycisku
    // Delay for initial pullup to take effect
    wait(0.001);
    trigger2.attach(&stream_pc, sampling_time); //wsysyłanie danych w przerwaniu (stream PC) na terminal
    //trigger1.attach(&predkosc_kat, sampling_time);
    
    while(1)
    {
        if(button1_pressed)
        {  
            if(tryb==1) //ODBIORNIK - STREAM TELEFON
            {
                led1 = 1;   led2 = 0;   led3 = 0;
                //odczyt_pozycji_gimbala();
                //testy_czujnika();
                //pc.printf(" %.2f\n\r", z_stopnie);
                //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
                ///////
                odczyt_pozycji_gimbala();
                stream_telefon();
                
                //studenci wybieraja okreslana funkcje
                
                //odbiornik();
                //stabilizacja_smartfon();
                
                //predkosc_kat();
                //stabilizacja_zero();  //!!!!
                
                //predkosc_kat();
                //odbiornik2();
                //zadajnik_odbiornik();
            } 
            if(tryb==2)     //ZADAJNIK - STABILIZACJA "0"
                {
                led1 = 0; led2 = 0; led3 = 1; 
                odczyt_pozycji_gimbala();
                stabilizacja_zero();  //!!!!
                } 
            if(tryb==3)     //ZADAJNIK-ODBIORNIK
                {
                led1 = 1; led2 = 0; led3 = 1;
                odczyt_pozycji_gimbala();
                odbiornik2();
                zadajnik_odbiornik();
                }
            if(tryb==4)
                {
                led1 = 1; led2 = 1; led3 = 1;
                wait(0.02);
                sekwencja_pokazowa();    
                }
            if(tryb==5)     //SMARTFON-ODBIORNIK
                {
                led1 = 0; led2 = 1; led3 = 0;
                odczyt_pozycji_gimbala();
                odbiornik();    //wartosci zadane polozeniem telefonu
                stabilizacja_smartfon();
                }
        }//KONIEC WARUNKU BUTTON   
    }//KONIEC PĘTLI WHILE
}//KONIEC PĘTLI MAIN

    //===========_STREAM_=======================================================
    void stream_pc() {   //funkcja streamu
        //KONSOLA PC
        //pc.printf(" %.2f ", Y);
        //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
        //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
        //pc.printf(" %.2f\n\r", z_stopnie);
        
        //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",gimbal.ax, gimbal.ay, gimbal.az);    
        //pc.printf(buffer);
        
        //studenci moga wybrav sposrod trzech komend
        sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",a_x, a_y, a_z);    
        pc.printf(buffer);
        
        //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",gimbal.gx, gimbal.gy, gimbal.gz);    
        //pc.printf(buffer);
        
        //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",g_x, g_y, g_z);    
        //pc.printf(buffer);
  
    }
    void stream_telefon(){      //wysłanie jednej ze wspolrzednych przez Bluetooth
        //MODUŁ BLUETOOTH
        float dane = (arc_tan_2(a_y,a_z)+180); //+180 stopni aby przesunąc wykres w aplikacji

        device_bt.putc(0);
        sprintf((char*)buffer,"%.2f", dane);
        device_bt.printf(buffer);

        wait(0.1);
    }
    //===================ODCZYT_POZYCJI=========================================
    void odczyt_pozycji_gimbala(){
        gimbal.begin();
        gimbal.readAll();

        float *wsk1 = &gimbal.ax;   //odczyt z czujnika przyspieszenia
        float *wsk2 = &gimbal.ay;
        float *wsk3 = &gimbal.az;
        
        float *wsk4 = &gimbal.gx;   //odczyt z czujnika prędkosci
        float *wsk5 = &gimbal.gy;
        float *wsk6 = &gimbal.gz;
        
        a_x= *wsk1; 
        a_y= *wsk2;
        a_z= *wsk3;
        
        g_x= *wsk4;
        g_y= *wsk5;
        g_z= *wsk6;
        
        wait(0.005);    //czas próbkowaia czujnika Pololu
    }
    
    //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
double arc_tan_2(double a, double b)    //a - x lub y, b - z (funkcja oblicza kąty wokół osi x, y)
    {
        double stopnie;
        double val = 180.0/3.14;
        stopnie = atan2(a,b)*val;
        return stopnie;
    }

        //@@@@@@@@@@@_PRĘDKOŚĆ_NA_KĄT_@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
    void predkosc_kat(){  
    
        double z_rad = g_z;      //przypisanie zmiennej z czujnika do zmiennej lokalnej
        if(z_rad>=-4.5&&z_rad<=-2.0){z_rad=0.0;} //dla wart 0 czujnik wskazuje ~-3,6
        double delta_t=0.02;    //funkcja wywoływana jest co 2,5 milisekund
        double delta_kat=(z_rad*delta_t);   //różnica kątowa= wart. czujnika * dt
        z_kat=z_kat+delta_kat;  //ostateczny kąt = kąt obecny + d kąta
        z_stopnie = z_kat*(180/3.14); //zamiana radianów na stopnie
        }

        //&&&&&&&&&&&&REGULACJA&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&         
    void regulacja(){
        Kp=0.2, Ki=0.05, Kd=0.01;        
        
        docelowa_pozycja_x=X;   //pozycja docelowa dla zadajników
        docelowa_pozycja_y=Y;
        docelowa_pozycja_z=Z;
        
        e_x=docelowa_pozycja_x-arc_tan_2(a_x,a_z); //uchyb regulacji
        e_y=docelowa_pozycja_y-arc_tan_2(a_y,a_z);
        e_z=docelowa_pozycja_z-z_stopnie;
    
        Ix=Ix+e_x;  
        Iy=Iy+e_y;
        Iz=Iz+e_z;
          
        Dx=e_x - e_x_poprzednie;
        Dy=e_y - e_y_poprzednie;
        Dz=e_z - e_z_poprzednie;
        
        //pwm1=(Kp*e_x);    //P
        //pwm1=(Kp*e_x)+(Ki*Ix);    //PI
        pwm1=(Kp*e_x)+(Kd*Dx);    //PD
        //pwm1=(Kp*e_x)+(Ki*Ix)+(Kd*Dx);    //PID          
        //pwm2=(Kp*e_y);    //P
        //pwm2=(Kp*e_y)+(Ki*Iy);    //PI
        pwm2=(Kp*e_y)+(Kd*Dy);    //PD
        //pwm2=(Kp*e_y)+(Ki*Iy)+(Kd*Dy);    //PID        
        //pwm3=(Kp*e_z);    //P
        //pwm3=(Kp*e_z)+(Ki*Iz);    //PI
        pwm3=(Kp*e_z)+(Kd*Dz);    //PD
        //pwm3=(Kp*e_z)+(Ki*Iz)+(Kd*Dz);    //PID   
    }
    
        void regulacja2(){        
        Kp=0.1, Ki=0.05, Kd=0.01;
        
        docelowa_pozycja_x=X;   //pozycja docelowa dla zadajników
        docelowa_pozycja_y=Y;
        docelowa_pozycja_z=Z;
        
        e_x=docelowa_pozycja_x-arc_tan_2(a_x,a_z); //uchyb regulacji
        e_y=docelowa_pozycja_y-arc_tan_2(a_y,a_z);
        e_z=docelowa_pozycja_z-z_stopnie;
        
        Ix=Ix+e_x;  
        Iy=Iy+e_y;
        Iz=Iz+e_z;
        
        Dx=e_x - e_x_poprzednie;
        Dy=e_y - e_y_poprzednie;
        Dz=e_z - e_z_poprzednie;
        
        pwm1=(Kp*e_x);    //P
        //pwm1=(Kp*e_x)+(Ki*Ix);    //PI        
        pwm2=(Kp*e_y);    //P
        //pwm2=(Kp*e_y)+(Ki*Iy);    //PI       
        pwm3=(Kp*e_z);    //P
        //pwm3=(Kp*e_z)+(Ki*Iz);    //PI  
    }
    
    
     //+++++++++++DANE Z ZADAJNIKA+++++++++++++++++++++++++++++++++++++++++++++
    void odbiornik()
        {      
        char str_x[1];      //definicja wektorów char      
        char str_y[1];
        char str_z[1];
        
        device_bt.scanf("%s",str_x);   //kąty pochylenia urzadzeia mobilnego
        device_bt.scanf("%s",str_y);
        device_bt.scanf("%s",str_z);
        
        X = atof(str_x);    //zamiana na float
        Y = atof(str_y);
        Z = atof(str_z)-180;  //rozlozenie zakresu kata obrotu symetrycznie wokol zera
    }
    void odbiornik2()
        {      
        char str_x[1];      //definicja wektorów char      
        char str_y[1];
        char str_z[1];
        
        device_zadajnik.scanf("%s",str_x);
        device_zadajnik.scanf("%s",str_y);
        device_zadajnik.scanf("%s",str_z);
        
        X = atof(str_x);
        Y = atof(str_y);
        Z = atof(str_z)-180;  
    }     

        //+++++++++++STABILIZACJA_ZERO++++++++++++++++++++++++++++++++++++++++++
    void stabilizacja_zero(){   //platforma stabilizuje się do położenia w którym odczyty z akcelerometru wynoszą: ax=0, ay=0;
        //OŚ X''''''''''''''''''''''''''''' 
        regulacja();

        if(a_x<=0.0)
            {
            OBROTY_M3=1;
            MOTOR_3.write(-0.022*(abs(pwm1))+1);
            }
        if(a_x>0.0)
            {
            OBROTY_M3=0;
            MOTOR_3.write(-0.022*(abs(pwm1))+1);    
            }
        //OŚ Y'''''''''''''''''''''''''''''''''''''
        if(a_y<=0.0)
            {
            OBROTY_M1=1;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);
            }
        if(a_y>0.0)
            {
            OBROTY_M1=0;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);    
            }
        
        //OŚ Z''''''''''''''''''''''''''''''''''''' 
/*        if(z_stopnie<=0.0)
            {
            OBROTY_M2=0;
            MOTOR_2.write(-0.022*(abs(pwm3))+1);
            }
        if(z_stopnie>0.0)
            {
            OBROTY_M2=1;
            MOTOR_2.write(-0.022*(abs(pwm3))+1);
            } */

    }
    //++++++++++STABILIZACJA_SMARTFON++++++++++++++++++++++++++++++++++++++++++
        void stabilizacja_smartfon(){
        //predkosc_kat();
        regulacja2();
        //OŚ X'''''''''''''''''''''''''''''          
        if(arc_tan_2(a_x,a_z)<=X)
            {
            OBROTY_M3=1;
            MOTOR_3.write(-0.022*(abs(pwm1))+1);    //y=ax+b
            }
            if(arc_tan_2(a_x,a_z)>X) 
            {
            OBROTY_M3=0;
            MOTOR_3.write(-0.022*(abs(pwm1))+1);     
        }
        //OŚ Y'''''''''''''''''''''''''''''''''''''
        if(arc_tan_2(a_y,a_z)<=Y)
            {
            OBROTY_M1=1;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);
            }
            if(arc_tan_2(a_y,a_z)>Y)
            {
            OBROTY_M1=0;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);   
        }
        //OŚ Z'''''''''''''''''''''''''''''''''''''
/*        if(z_stopnie<=Z)
            {
            OBROTY_M2=1;
            MOTOR_2.write(0.9);
            }
        if(z_stopnie>Z)
            {
            OBROTY_M2=0;
            MOTOR_2.write(0.9);
            }*/    
        }
        
        //vvvvvvvvvvvvvvvvvvvvvv_ZADAJNIK_ODBIORNIK_vvvvvvvvvvvvvvvvvvvvvvvvv 
      void zadajnik_odbiornik() {
        
        regulacja();    //oblicza pwm1 - sygnał sterujacy
        //OŚ X'''''''''''''''''''''''''''''          
        if(arc_tan_2(a_x,a_z)<=X)   //X - wartosc zadana
            {
            OBROTY_M3=1;    //kierunek obrotu
            MOTOR_3.write(-0.022*(abs(pwm1))+1);    //y=-ax+1 (odejmujemy od wypełnienia z uwagi na odwrotnosc fali PWM: 1 - 0% wypełnienia, 0 - 100% wypełnienia sygnału
            }
            if(arc_tan_2(a_x,a_z)>X)
            {
            OBROTY_M3=0;
            MOTOR_3.write(-0.022*(abs(pwm1))+1);     
        }
        //OŚ Y'''''''''''''''''''''''''''''''''''''
        if(arc_tan_2(a_y,a_z)<=Y)
            {
            OBROTY_M1=1;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);
            }
            if(arc_tan_2(a_y,a_z)>Y)
            {
            OBROTY_M1=0;
            MOTOR_1.write(-0.022*(abs(pwm2))+1);   
        }
        //OŚ Z'''''''''''''''''''''''''''''''''''''
/*        if(z_stopnie<=Z)
            {
            OBROTY_M2=1;
            MOTOR_2.write(0.9);
            }
        if(z_stopnie>Z)
            {
            OBROTY_M2=0;
            MOTOR_2.write(0.9);
            }*/
    }
        //vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
        
        //&&&&&&&&&&&&&&&&&&&&TESTY CZUJNIKA&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
    void testy_czujnika(){
        // WOLNE OBROTY - 10 % PWM  
        for(double i=1; i>=0.85; i=i-0.01)
        {
            MOTOR_1.write(i);
            MOTOR_2.write(i);
            //MOTOR_3.write(i);
            odczyt_pozycji_gimbala();
            wait(0.1);
        }
        
        for(double j=50; j>=0; j=j-1)
        {
            wait(0.1);
            odczyt_pozycji_gimbala();
        }
        
        for(double i=0.85; i<=1; i=i+0.01)
        {
            MOTOR_1.write(i);
            MOTOR_2.write(i);
            //MOTOR_3.write(i);
            odczyt_pozycji_gimbala();
            wait(0.1);
        }
        //SZYBKIE OBROTY - 100% PWM
/*        for(double i=1; i>=0.01; i=i-0.01)
        {
            MOTOR_1.write(i);
            //MOTOR_2.write(i);
            //MOTOR_3.write(i);
            wait(0.1);
        }
        
        for(double j=50; j>=0; j=j-1)
        {
            wait(0.1);
        }
        
        for(double i=0.00; i<=1; i=i+0.01)
        {
            MOTOR_1.write(i);
            //MOTOR_2.write(i);
            //MOTOR_3.write(i);
            wait(0.1);
        }*/
    }
            
        //$$$$$$$$$$$$$SEKWENCJE_POKAZ$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$    
                //SEKWENCJA 2               
    void sekwencja_pokazowa(){      // funkcja sekwencja pokazowa
        for(double i=1; i>=0.6; i=i-0.01)
        {
            MOTOR_1.write(i);
            MOTOR_2.write(i);
            MOTOR_3.write(i);
            wait(0.1);
        }
        //WTRĄCENIE 2.1
        for(double i=1; i>=0.6; i=i-0.01)
            {
                MOTOR_1.write(i);
                wait(0.1);
            }
        for(double i=0.6; i<=1.0; i=i+0.01)
            {
                MOTOR_1.write(i);
                wait(0.1);
            }
            OBROTY_M2=1;    //zmiana obrotów
        for(double i=1; i>=0.6; i=i-0.01)
            {
                MOTOR_1.write(i);
                wait(0.1);
            }
        for(double i=10.6; i<=1.0; i=i+0.01)
            {
                MOTOR_1.write(i);
                wait(0.1);
            }
        //===============
        wait(2.0);
        for(double i=0.6; i<=1.0; i=i+0.01)
            {
                MOTOR_1.write(i);
                MOTOR_2.write(i);
                MOTOR_3.write(i);
                wait(0.1);
            }
         wait(1.0);
        //SEKWENCJA 1
        for(double i=1; i>=0; i=i-0.01)
            {
                MOTOR_1.write(i);
                MOTOR_2.write(i);
                MOTOR_3.write(i);
                wait(0.1);
            }
        for(double i=0; i<=1; i=i+0.01)
            {
                MOTOR_1.write(i);
                MOTOR_2.write(i);
                MOTOR_3.write(i);
                wait(0.1);
            }
        
        wait(2.0);
    }     
        //$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$