Mechatronics
Laboratory
Dependencies: mbed BufferedSerial LSM6DS33
Diff: main.cpp
- Revision:
- 1:6e9fe61d7074
- Parent:
- 0:a7c396f2ccf6
--- a/main.cpp Tue Feb 12 18:11:27 2019 +0000
+++ b/main.cpp Wed Feb 13 13:50:34 2019 +0000
@@ -1,3 +1,5 @@
+//Program na NUCLEO F429ZI
+
#include "mbed.h"
#include "BufferedSerial.h"
#include "LSM6DS33.h"
@@ -21,25 +23,25 @@
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(); //odczyt danych z czujnika Pololu MinIMU-9 v5
+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);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-InterruptIn button1(USER_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; // Drganie styków
-Timeout button1_timeout; // Drganie styków
+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;
+float sampling_time = 0.2; //częstotiwosc zapisu na konsolę PC (terminal) w funkcji przerwania
Ticker trigger1; //predkosc_kat
-Ticker trigger2; //stream
+Ticker trigger2; //stream na termial
//Wyjscia PWM dla trzech silników
-PwmOut MOTOR_1(PB_3); //ogniwo 1
+PwmOut MOTOR_1(PB_3); //ogniwo 1 - od zewnętrznej
PwmOut MOTOR_2(PB_4); //ogniwo 2
PwmOut MOTOR_3(PB_5); //ogniwo 3
@@ -49,24 +51,25 @@
//=============REGULACJA======================
int obecna_pozycja=0;
-float e_x=0, e_x_poprzednie=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;
-double pwm1, pwm2, pwm3;
+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;
+volatile double z_kat,x_kat,y_kat; //wartosc aktualna
-float Ix=0, Iy=0, Iz=0;
+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(PD_5, PD_6); //TX, RX aplikacja
-Serial device2(PC_10, PC_11); //TX, RX zadajnik
+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)
@@ -74,7 +77,8 @@
button1_enabled = true;
}
-int tryb = 0;
+int tryb = 0; //tryby pracy 1-5
+
// ISR handling button pressed event
void button1_onpressed_cb(void)
{
@@ -87,18 +91,18 @@
}
}
//===================KOMUNIKACJA Z CZUJNIKIEM===================================
-volatile double a_x; //deklaracja zmiennych dla akcelerometru
+volatile double a_x; //deklaracja zmiennych akcelerometru
volatile double a_y;
volatile double a_z;
-volatile double g_x; //deklaracja zmiennych dla żyroskopu
+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 pozyskane z UART
+volatile float X=0; //zmiennne globalne - kąty obrotu zadajnika przesłane przez UART
volatile float Y=0;
volatile float Z=0;
@@ -109,44 +113,44 @@
int main()
{
pc.baud(115200);
- device.baud(9600); //transmisja danych
- device2.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(30kHz)
- MOTOR_1.write(1.0f); // 50% duty cycle(1.0=0%, 0.0=100%)
- MOTOR_2.period(0.002f); // 2 milisecond period
- MOTOR_2.write(1.0f); // 50% duty cycle
- MOTOR_3.period(0.002f); // 2 milisecond period
- MOTOR_3.write(1.0f); // 50% duty cycle
+ 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);
- button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event
+ //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(.001);
-
- //PRZERWANIA================
- trigger2.attach(&stream_pc, sampling_time); //zbieranie danych w przerwaniu
+ 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)
+ while(1)
{
-
if(button1_pressed)
{
if(tryb==1) //ODBIORNIK - STREAM TELEFON
{
led1 = 1; led2 = 0; led3 = 0;
- //odczyt_pozycji();
+ //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();
+ odczyt_pozycji_gimbala();
stream_telefon();
+ //studenci wybieraja okreslana funkcje
+
//odbiornik();
//stabilizacja_smartfon();
@@ -156,35 +160,33 @@
//predkosc_kat();
//odbiornik2();
//zadajnik_odbiornik();
- }
- if(tryb==2) //SMARTFON-ODBIORNIK
+ }
+ if(tryb==2) //ZADAJNIK - STABILIZACJA "0"
{
- led1 = 0; led2 = 1; led3 = 0;
- odczyt_pozycji();
- odbiornik();
- stabilizacja_smartfon();
-
- }
- if(tryb==3) //ZADAJNIK - STABILIZACJA "0"
- {
- led2 = 0; led3 = 1;
- odczyt_pozycji();
- predkosc_kat();
+ led1 = 0; led2 = 0; led3 = 1;
+ odczyt_pozycji_gimbala();
stabilizacja_zero(); //!!!!
}
- if(tryb==4) //ZADAJNIK-ODBIORNIK
+ if(tryb==3) //ZADAJNIK-ODBIORNIK
{
led1 = 1; led2 = 0; led3 = 1;
- odczyt_pozycji();
+ odczyt_pozycji_gimbala();
odbiornik2();
zadajnik_odbiornik();
}
- if(tryb==5)
+ 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
@@ -200,6 +202,7 @@
//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);
@@ -210,30 +213,30 @@
//pc.printf(buffer);
}
- void stream_telefon(){
+ void stream_telefon(){ //wysłanie jednej ze wspolrzednych przez Bluetooth
//MODUŁ BLUETOOTH
- float dane = (arc_tan_2(a_y,a_z)+180);
+ float dane = (arc_tan_2(a_y,a_z)+180); //+180 stopni aby przesunąc wykres w aplikacji
- device.putc(0);
+ device_bt.putc(0);
sprintf((char*)buffer,"%.2f", dane);
- device.printf(buffer);
+ device_bt.printf(buffer);
wait(0.1);
}
//===================ODCZYT_POZYCJI=========================================
- void odczyt_pozycji(){
+ void odczyt_pozycji_gimbala(){
gimbal.begin();
gimbal.readAll();
- float *wsk1 = &gimbal.ax;
+ float *wsk1 = &gimbal.ax; //odczyt z czujnika przyspieszenia
float *wsk2 = &gimbal.ay;
float *wsk3 = &gimbal.az;
- float *wsk4 = &gimbal.gx;
+ float *wsk4 = &gimbal.gx; //odczyt z czujnika prędkosci
float *wsk5 = &gimbal.gy;
float *wsk6 = &gimbal.gz;
- a_x= *wsk1;
+ a_x= *wsk1;
a_y= *wsk2;
a_z= *wsk3;
@@ -241,11 +244,11 @@
g_y= *wsk5;
g_z= *wsk6;
- wait(0.005);
+ wait(0.005); //czas próbkowaia czujnika Pololu
}
//>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
-double arc_tan_2(double a, double b)
+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;
@@ -333,13 +336,13 @@
char str_y[1];
char str_z[1];
- device.scanf("%s",str_x);
- device.scanf("%s",str_y);
- device.scanf("%s",str_z);
+ 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);
+ X = atof(str_x); //zamiana na float
Y = atof(str_y);
- Z = atof(str_z)-180;
+ Z = atof(str_z)-180; //rozlozenie zakresu kata obrotu symetrycznie wokol zera
}
void odbiornik2()
{
@@ -347,9 +350,9 @@
char str_y[1];
char str_z[1];
- device2.scanf("%s",str_x);
- device2.scanf("%s",str_y);
- device2.scanf("%s",str_z);
+ 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);
@@ -357,7 +360,7 @@
}
//+++++++++++STABILIZACJA_ZERO++++++++++++++++++++++++++++++++++++++++++
- void 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();
@@ -438,12 +441,12 @@
//vvvvvvvvvvvvvvvvvvvvvv_ZADAJNIK_ODBIORNIK_vvvvvvvvvvvvvvvvvvvvvvvvv
void zadajnik_odbiornik() {
- regulacja();
+ regulacja(); //oblicza pwm1 - sygnał sterujacy
//OŚ X'''''''''''''''''''''''''''''
- if(arc_tan_2(a_x,a_z)<=X)
+ if(arc_tan_2(a_x,a_z)<=X) //X - wartosc zadana
{
- OBROTY_M3=1;
- MOTOR_3.write(-0.022*(abs(pwm1))+1); //y=-ax+1
+ 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)
{
@@ -483,14 +486,14 @@
MOTOR_1.write(i);
MOTOR_2.write(i);
//MOTOR_3.write(i);
- odczyt_pozycji();
+ odczyt_pozycji_gimbala();
wait(0.1);
}
for(double j=50; j>=0; j=j-1)
{
wait(0.1);
- odczyt_pozycji();
+ odczyt_pozycji_gimbala();
}
for(double i=0.85; i<=1; i=i+0.01)
@@ -498,7 +501,7 @@
MOTOR_1.write(i);
MOTOR_2.write(i);
//MOTOR_3.write(i);
- odczyt_pozycji();
+ odczyt_pozycji_gimbala();
wait(0.1);
}
//SZYBKIE OBROTY - 100% PWM