Mechatronics / Mbed 2 deprecated 1-NUCLEO-F429Zl-Lab

Laboratory

Dependencies:   mbed BufferedSerial LSM6DS33

main.cpp@2:aa102819471f, 2022-05-19 (annotated)

Committer:
abm_mechatronika
Date:
Thu May 19 14:01:44 2022 +0000
Revision:
2:aa102819471f
Parent:
1:6e9fe61d7074 
2022

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abm_mechatronika 1:6e9fe61d7074 1 //Program na NUCLEO F429ZI
abm_mechatronika 1:6e9fe61d7074 2
Lukasz_K 0:a7c396f2ccf6 3 #include "mbed.h"
Lukasz_K 0:a7c396f2ccf6 4 #include "BufferedSerial.h"
Lukasz_K 0:a7c396f2ccf6 5 #include "LSM6DS33.h"
Lukasz_K 0:a7c396f2ccf6 6 #include "math.h"
Lukasz_K 0:a7c396f2ccf6 7 #include <stdio.h>
Lukasz_K 0:a7c396f2ccf6 8
Lukasz_K 0:a7c396f2ccf6 9 /*PwmOut rled(LED_RED);
Lukasz_K 0:a7c396f2ccf6 10 PwmOut gled(LED_GREEN);
Lukasz_K 0:a7c396f2ccf6 11 PwmOut bled(LED_BLUE);*/
Lukasz_K 0:a7c396f2ccf6 12
Lukasz_K 0:a7c396f2ccf6 13 //deklaracje funkcji
Lukasz_K 0:a7c396f2ccf6 14 void testy_czujnika(); //wolne obroty wybranego przegubu
Lukasz_K 0:a7c396f2ccf6 15 void zadajnik_odbiornik(); //zadawanie położenia odebranego z zadajnika
Lukasz_K 0:a7c396f2ccf6 16 void sekwencja_pokazowa(); //przykładowa ścieżka ruchu urządzenia
Lukasz_K 0:a7c396f2ccf6 17 void stabilizacja_smartfon(); //zadawanie położenia odebranego z aplikacji
Lukasz_K 0:a7c396f2ccf6 18 void predkosc_kat(); //zamiana prędkosci obrotowej na kąt
Lukasz_K 0:a7c396f2ccf6 19 void regulacja(); //funkcja regulacji
Lukasz_K 0:a7c396f2ccf6 20 void regulacja2(); //funkcja regulacji 2
Lukasz_K 0:a7c396f2ccf6 21 void stream_pc(); //wysyłanie danych na port usb
Lukasz_K 0:a7c396f2ccf6 22 void stream_telefon(); //wysyłanie danych do modułu Bluetooth
Lukasz_K 0:a7c396f2ccf6 23 void odbiornik(); //odbiór danych z zadajnika
Lukasz_K 0:a7c396f2ccf6 24 void odbiornik2(); //odbiór danych z aplikacji mobilnej
Lukasz_K 0:a7c396f2ccf6 25 void stabilizacja_zero(); //stabilizacja położenia dla stałej wartości 0
abm_mechatronika 1:6e9fe61d7074 26 void odczyt_pozycji_gimbala(); //odczyt danych z czujnika Pololu MinIMU-9 v5
Lukasz_K 0:a7c396f2ccf6 27 double arc_tan_2(double a, double b); //funkcja zamiany przyśpieszenia na kąt
Lukasz_K 0:a7c396f2ccf6 28
Lukasz_K 0:a7c396f2ccf6 29 //BUTTON
abm_mechatronika 1:6e9fe61d7074 30 DigitalOut led1(LED1); //żółta
abm_mechatronika 1:6e9fe61d7074 31 DigitalOut led2(LED2); //niebieska
abm_mechatronika 1:6e9fe61d7074 32 DigitalOut led3(LED3); //czerwona
abm_mechatronika 1:6e9fe61d7074 33 InterruptIn button1(USER_BUTTON); //niebiski przycisk
Lukasz_K 0:a7c396f2ccf6 34 volatile bool button1_pressed = false; // Użyte w pętli głównej
abm_mechatronika 1:6e9fe61d7074 35 volatile bool button1_enabled = true; // Eliminacja wpływu drgania styków
abm_mechatronika 1:6e9fe61d7074 36 Timeout button1_timeout; // Drganie styków - zwłoka
Lukasz_K 0:a7c396f2ccf6 37
Lukasz_K 0:a7c396f2ccf6 38 //TICKER
abm_mechatronika 1:6e9fe61d7074 39 float sampling_time = 0.2; //częstotiwosc zapisu na konsolę PC (terminal) w funkcji przerwania
Lukasz_K 0:a7c396f2ccf6 40 Ticker trigger1; //predkosc_kat
abm_mechatronika 1:6e9fe61d7074 41 Ticker trigger2; //stream na termial
Lukasz_K 0:a7c396f2ccf6 42
Lukasz_K 0:a7c396f2ccf6 43 //Wyjscia PWM dla trzech silników
abm_mechatronika 1:6e9fe61d7074 44 PwmOut MOTOR_1(PB_3); //ogniwo 1 - od zewnętrznej
Lukasz_K 0:a7c396f2ccf6 45 PwmOut MOTOR_2(PB_4); //ogniwo 2
Lukasz_K 0:a7c396f2ccf6 46 PwmOut MOTOR_3(PB_5); //ogniwo 3
Lukasz_K 0:a7c396f2ccf6 47
Lukasz_K 0:a7c396f2ccf6 48 DigitalOut OBROTY_M1(PE_13); //Kierunek obrotów dla trzech silników(stan 0/1)
Lukasz_K 0:a7c396f2ccf6 49 DigitalOut OBROTY_M2(PE_14);
Lukasz_K 0:a7c396f2ccf6 50 DigitalOut OBROTY_M3(PE_15);
Lukasz_K 0:a7c396f2ccf6 51
Lukasz_K 0:a7c396f2ccf6 52 //=============REGULACJA======================
Lukasz_K 0:a7c396f2ccf6 53 int obecna_pozycja=0;
abm_mechatronika 1:6e9fe61d7074 54 float e_x=0, e_x_poprzednie=0; //uchyby położeń kątowych
Lukasz_K 0:a7c396f2ccf6 55 float e_y=0, e_y_poprzednie=0;
Lukasz_K 0:a7c396f2ccf6 56 float e_z=0, e_z_poprzednie=0;
abm_mechatronika 1:6e9fe61d7074 57 int docelowa_pozycja_x=0, docelowa_pozycja_y=0, docelowa_pozycja_z=0; //wartosc zadana
abm_mechatronika 1:6e9fe61d7074 58 double pwm1, pwm2, pwm3; //sygnały sterujace
Lukasz_K 0:a7c396f2ccf6 59
abm_mechatronika 1:6e9fe61d7074 60 volatile double z_kat,x_kat,y_kat; //wartosc aktualna
Lukasz_K 0:a7c396f2ccf6 61
abm_mechatronika 1:6e9fe61d7074 62 float Ix=0, Iy=0, Iz=0; //Stałe regulatora PID
Lukasz_K 0:a7c396f2ccf6 63 float Dx=0, Dy=0, Dz=0;
Lukasz_K 0:a7c396f2ccf6 64 float Kp=0.2, Ki=0.05, Kd=0.01; //0.2, 0.05, 0.01
Lukasz_K 0:a7c396f2ccf6 65
Lukasz_K 0:a7c396f2ccf6 66 //===============STREAM=======================
Lukasz_K 0:a7c396f2ccf6 67 //Serial pc(USBTX,USBRX); //BLUETOOTH // komunikacja z PC
Lukasz_K 0:a7c396f2ccf6 68 BufferedSerial pc(USBTX,USBRX); //PC
abm_mechatronika 1:6e9fe61d7074 69 Serial device_bt(PD_5, PD_6); //TX, RX aplikacja/modul Bluetooth
abm_mechatronika 1:6e9fe61d7074 70 Serial device_zadajnik(PC_10, PC_11); //TX, RX zadajnik
Lukasz_K 0:a7c396f2ccf6 71 char buffer[24];
abm_mechatronika 1:6e9fe61d7074 72
Lukasz_K 0:a7c396f2ccf6 73 //===============BUTTON========================
Lukasz_K 0:a7c396f2ccf6 74 // Enables button when bouncing is over
Lukasz_K 0:a7c396f2ccf6 75 void button1_enabled_cb(void)
Lukasz_K 0:a7c396f2ccf6 76 {
Lukasz_K 0:a7c396f2ccf6 77 button1_enabled = true;
Lukasz_K 0:a7c396f2ccf6 78 }
Lukasz_K 0:a7c396f2ccf6 79
abm_mechatronika 1:6e9fe61d7074 80 int tryb = 0; //tryby pracy 1-5
abm_mechatronika 1:6e9fe61d7074 81
Lukasz_K 0:a7c396f2ccf6 82 // ISR handling button pressed event
Lukasz_K 0:a7c396f2ccf6 83 void button1_onpressed_cb(void)
Lukasz_K 0:a7c396f2ccf6 84 {
Lukasz_K 0:a7c396f2ccf6 85 if (button1_enabled) { // Disabled while the button is bouncing
Lukasz_K 0:a7c396f2ccf6 86 button1_enabled = false;
Lukasz_K 0:a7c396f2ccf6 87 button1_pressed = true; // To be read by the main loop
Lukasz_K 0:a7c396f2ccf6 88 button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
Lukasz_K 0:a7c396f2ccf6 89 tryb++;
Lukasz_K 0:a7c396f2ccf6 90 if(tryb>=6){tryb=1;}
Lukasz_K 0:a7c396f2ccf6 91 }
Lukasz_K 0:a7c396f2ccf6 92 }
Lukasz_K 0:a7c396f2ccf6 93 //===================KOMUNIKACJA Z CZUJNIKIEM===================================
abm_mechatronika 1:6e9fe61d7074 94 volatile double a_x; //deklaracja zmiennych akcelerometru
Lukasz_K 0:a7c396f2ccf6 95 volatile double a_y;
Lukasz_K 0:a7c396f2ccf6 96 volatile double a_z;
Lukasz_K 0:a7c396f2ccf6 97
abm_mechatronika 1:6e9fe61d7074 98 volatile double g_x; //deklaracja zmiennych żyroskopu
Lukasz_K 0:a7c396f2ccf6 99 volatile double g_y;
Lukasz_K 0:a7c396f2ccf6 100 volatile double g_z;
Lukasz_K 0:a7c396f2ccf6 101
Lukasz_K 0:a7c396f2ccf6 102 volatile double z_stopnie;
Lukasz_K 0:a7c396f2ccf6 103
Lukasz_K 0:a7c396f2ccf6 104 //===================KOMUNIKACJA Z ZADAJNIKIEM==================================
abm_mechatronika 1:6e9fe61d7074 105 volatile float X=0; //zmiennne globalne - kąty obrotu zadajnika przesłane przez UART
Lukasz_K 0:a7c396f2ccf6 106 volatile float Y=0;
Lukasz_K 0:a7c396f2ccf6 107 volatile float Z=0;
Lukasz_K 0:a7c396f2ccf6 108
Lukasz_K 0:a7c396f2ccf6 109 //======================PINY DLA CZUJNIKA=======================================
Lukasz_K 0:a7c396f2ccf6 110 LSM6DS33 gimbal(PB_9, PB_8, LSM6DS33_AG_I2C_ADDR(1)); //akcelerometr i żyroskop
Lukasz_K 0:a7c396f2ccf6 111
Lukasz_K 0:a7c396f2ccf6 112 //===========PĘTLA GŁÓWNA=======================================================
Lukasz_K 0:a7c396f2ccf6 113 int main()
Lukasz_K 0:a7c396f2ccf6 114 {
Lukasz_K 0:a7c396f2ccf6 115 pc.baud(115200);
abm_mechatronika 1:6e9fe61d7074 116 device_bt.baud(9600); //transmisja danych Bluetooth
abm_mechatronika 1:6e9fe61d7074 117 device_zadajnik.baud(115200); //zadajnik - odbiornik
Lukasz_K 0:a7c396f2ccf6 118
Lukasz_K 0:a7c396f2ccf6 119 //prędkosci silników(wypełnienie PWM)
abm_mechatronika 1:6e9fe61d7074 120 MOTOR_1.period(0.002f); // 2 milisecond period (500Hz)
abm_mechatronika 1:6e9fe61d7074 121 MOTOR_1.write(1.0f); // wypełnienie (1.0=0%, 0.0=100%) z krokiem 0.01f
abm_mechatronika 1:6e9fe61d7074 122 MOTOR_2.period(0.002f); // 2 ms
abm_mechatronika 1:6e9fe61d7074 123 MOTOR_2.write(1.0f);
abm_mechatronika 1:6e9fe61d7074 124 MOTOR_3.period(0.002f); // 2 ms
abm_mechatronika 1:6e9fe61d7074 125 MOTOR_3.write(1.0f);
Lukasz_K 0:a7c396f2ccf6 126
Lukasz_K 0:a7c396f2ccf6 127 //LSM6DS33 gimbal(PB_9, PB_8); //akcelerometr i żyroskop
Lukasz_K 0:a7c396f2ccf6 128 wait(0.001);
Lukasz_K 0:a7c396f2ccf6 129
abm_mechatronika 1:6e9fe61d7074 130 //PRZERWANIA================
abm_mechatronika 1:6e9fe61d7074 131 button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event -zwolnienie niebieskiego przycisku
Lukasz_K 0:a7c396f2ccf6 132 // Delay for initial pullup to take effect
abm_mechatronika 1:6e9fe61d7074 133 wait(0.001);
abm_mechatronika 1:6e9fe61d7074 134 trigger2.attach(&stream_pc, sampling_time); //wsysyłanie danych w przerwaniu (stream PC) na terminal
Lukasz_K 0:a7c396f2ccf6 135 //trigger1.attach(&predkosc_kat, sampling_time);
Lukasz_K 0:a7c396f2ccf6 136
abm_mechatronika 1:6e9fe61d7074 137 while(1)
Lukasz_K 0:a7c396f2ccf6 138 {
Lukasz_K 0:a7c396f2ccf6 139 if(button1_pressed)
Lukasz_K 0:a7c396f2ccf6 140 {
Lukasz_K 0:a7c396f2ccf6 141 if(tryb==1) //ODBIORNIK - STREAM TELEFON
Lukasz_K 0:a7c396f2ccf6 142 {
Lukasz_K 0:a7c396f2ccf6 143 led1 = 1; led2 = 0; led3 = 0;
abm_mechatronika 1:6e9fe61d7074 144 //odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 145 //testy_czujnika();
Lukasz_K 0:a7c396f2ccf6 146 //pc.printf(" %.2f\n\r", z_stopnie);
Lukasz_K 0:a7c396f2ccf6 147 //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
Lukasz_K 0:a7c396f2ccf6 148 ///////
abm_mechatronika 1:6e9fe61d7074 149 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 150 stream_telefon();
Lukasz_K 0:a7c396f2ccf6 151
abm_mechatronika 1:6e9fe61d7074 152 //studenci wybieraja okreslana funkcje
abm_mechatronika 1:6e9fe61d7074 153
Lukasz_K 0:a7c396f2ccf6 154 //odbiornik();
Lukasz_K 0:a7c396f2ccf6 155 //stabilizacja_smartfon();
Lukasz_K 0:a7c396f2ccf6 156
Lukasz_K 0:a7c396f2ccf6 157 //predkosc_kat();
Lukasz_K 0:a7c396f2ccf6 158 //stabilizacja_zero(); //!!!!
Lukasz_K 0:a7c396f2ccf6 159
Lukasz_K 0:a7c396f2ccf6 160 //predkosc_kat();
Lukasz_K 0:a7c396f2ccf6 161 //odbiornik2();
Lukasz_K 0:a7c396f2ccf6 162 //zadajnik_odbiornik();
abm_mechatronika 1:6e9fe61d7074 163 }
abm_mechatronika 1:6e9fe61d7074 164 if(tryb==2) //ZADAJNIK - STABILIZACJA "0"
Lukasz_K 0:a7c396f2ccf6 165 {
abm_mechatronika 1:6e9fe61d7074 166 led1 = 0; led2 = 0; led3 = 1;
abm_mechatronika 1:6e9fe61d7074 167 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 168 stabilizacja_zero(); //!!!!
Lukasz_K 0:a7c396f2ccf6 169 }
abm_mechatronika 1:6e9fe61d7074 170 if(tryb==3) //ZADAJNIK-ODBIORNIK
Lukasz_K 0:a7c396f2ccf6 171 {
Lukasz_K 0:a7c396f2ccf6 172 led1 = 1; led2 = 0; led3 = 1;
abm_mechatronika 1:6e9fe61d7074 173 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 174 odbiornik2();
Lukasz_K 0:a7c396f2ccf6 175 zadajnik_odbiornik();
Lukasz_K 0:a7c396f2ccf6 176 }
abm_mechatronika 1:6e9fe61d7074 177 if(tryb==4)
Lukasz_K 0:a7c396f2ccf6 178 {
Lukasz_K 0:a7c396f2ccf6 179 led1 = 1; led2 = 1; led3 = 1;
Lukasz_K 0:a7c396f2ccf6 180 wait(0.02);
Lukasz_K 0:a7c396f2ccf6 181 sekwencja_pokazowa();
Lukasz_K 0:a7c396f2ccf6 182 }
abm_mechatronika 1:6e9fe61d7074 183 if(tryb==5) //SMARTFON-ODBIORNIK
abm_mechatronika 1:6e9fe61d7074 184 {
abm_mechatronika 1:6e9fe61d7074 185 led1 = 0; led2 = 1; led3 = 0;
abm_mechatronika 1:6e9fe61d7074 186 odczyt_pozycji_gimbala();
abm_mechatronika 1:6e9fe61d7074 187 odbiornik(); //wartosci zadane polozeniem telefonu
abm_mechatronika 1:6e9fe61d7074 188 stabilizacja_smartfon();
abm_mechatronika 1:6e9fe61d7074 189 }
Lukasz_K 0:a7c396f2ccf6 190 }//KONIEC WARUNKU BUTTON
Lukasz_K 0:a7c396f2ccf6 191 }//KONIEC PĘTLI WHILE
Lukasz_K 0:a7c396f2ccf6 192 }//KONIEC PĘTLI MAIN
Lukasz_K 0:a7c396f2ccf6 193
Lukasz_K 0:a7c396f2ccf6 194 //===========_STREAM_=======================================================
Lukasz_K 0:a7c396f2ccf6 195 void stream_pc() { //funkcja streamu
Lukasz_K 0:a7c396f2ccf6 196 //KONSOLA PC
Lukasz_K 0:a7c396f2ccf6 197 //pc.printf(" %.2f ", Y);
Lukasz_K 0:a7c396f2ccf6 198 //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
Lukasz_K 0:a7c396f2ccf6 199 //pc.printf(" %.2f\n\r", arc_tan_2(a_y,a_z));
Lukasz_K 0:a7c396f2ccf6 200 //pc.printf(" %.2f\n\r", z_stopnie);
Lukasz_K 0:a7c396f2ccf6 201
Lukasz_K 0:a7c396f2ccf6 202 //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",gimbal.ax, gimbal.ay, gimbal.az);
Lukasz_K 0:a7c396f2ccf6 203 //pc.printf(buffer);
Lukasz_K 0:a7c396f2ccf6 204
abm_mechatronika 1:6e9fe61d7074 205 //studenci moga wybrav sposrod trzech komend
Lukasz_K 0:a7c396f2ccf6 206 sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",a_x, a_y, a_z);
Lukasz_K 0:a7c396f2ccf6 207 pc.printf(buffer);
Lukasz_K 0:a7c396f2ccf6 208
Lukasz_K 0:a7c396f2ccf6 209 //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",gimbal.gx, gimbal.gy, gimbal.gz);
Lukasz_K 0:a7c396f2ccf6 210 //pc.printf(buffer);
Lukasz_K 0:a7c396f2ccf6 211
Lukasz_K 0:a7c396f2ccf6 212 //sprintf((char*)buffer, " %.2f %.2f %.2f\n\r",g_x, g_y, g_z);
Lukasz_K 0:a7c396f2ccf6 213 //pc.printf(buffer);
Lukasz_K 0:a7c396f2ccf6 214
Lukasz_K 0:a7c396f2ccf6 215 }
abm_mechatronika 1:6e9fe61d7074 216 void stream_telefon(){ //wysłanie jednej ze wspolrzednych przez Bluetooth
Lukasz_K 0:a7c396f2ccf6 217 //MODUŁ BLUETOOTH
abm_mechatronika 1:6e9fe61d7074 218 float dane = (arc_tan_2(a_y,a_z)+180); //+180 stopni aby przesunąc wykres w aplikacji
Lukasz_K 0:a7c396f2ccf6 219
abm_mechatronika 1:6e9fe61d7074 220 device_bt.putc(0);
Lukasz_K 0:a7c396f2ccf6 221 sprintf((char*)buffer,"%.2f", dane);
abm_mechatronika 1:6e9fe61d7074 222 device_bt.printf(buffer);
Lukasz_K 0:a7c396f2ccf6 223
Lukasz_K 0:a7c396f2ccf6 224 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 225 }
Lukasz_K 0:a7c396f2ccf6 226 //===================ODCZYT_POZYCJI=========================================
abm_mechatronika 1:6e9fe61d7074 227 void odczyt_pozycji_gimbala(){
Lukasz_K 0:a7c396f2ccf6 228 gimbal.begin();
Lukasz_K 0:a7c396f2ccf6 229 gimbal.readAll();
Lukasz_K 0:a7c396f2ccf6 230
abm_mechatronika 1:6e9fe61d7074 231 float *wsk1 = &gimbal.ax; //odczyt z czujnika przyspieszenia
Lukasz_K 0:a7c396f2ccf6 232 float *wsk2 = &gimbal.ay;
Lukasz_K 0:a7c396f2ccf6 233 float *wsk3 = &gimbal.az;
Lukasz_K 0:a7c396f2ccf6 234
abm_mechatronika 1:6e9fe61d7074 235 float *wsk4 = &gimbal.gx; //odczyt z czujnika prędkosci
Lukasz_K 0:a7c396f2ccf6 236 float *wsk5 = &gimbal.gy;
Lukasz_K 0:a7c396f2ccf6 237 float *wsk6 = &gimbal.gz;
Lukasz_K 0:a7c396f2ccf6 238
abm_mechatronika 1:6e9fe61d7074 239 a_x= *wsk1;
Lukasz_K 0:a7c396f2ccf6 240 a_y= *wsk2;
Lukasz_K 0:a7c396f2ccf6 241 a_z= *wsk3;
Lukasz_K 0:a7c396f2ccf6 242
Lukasz_K 0:a7c396f2ccf6 243 g_x= *wsk4;
Lukasz_K 0:a7c396f2ccf6 244 g_y= *wsk5;
Lukasz_K 0:a7c396f2ccf6 245 g_z= *wsk6;
Lukasz_K 0:a7c396f2ccf6 246
abm_mechatronika 1:6e9fe61d7074 247 wait(0.005); //czas próbkowaia czujnika Pololu
Lukasz_K 0:a7c396f2ccf6 248 }
Lukasz_K 0:a7c396f2ccf6 249
Lukasz_K 0:a7c396f2ccf6 250 //>>>>>>>>>>>>>>>>>>>_FUNKCJA_ATAN2_>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
abm_mechatronika 1:6e9fe61d7074 251 double arc_tan_2(double a, double b) //a - x lub y, b - z (funkcja oblicza kąty wokół osi x, y)
Lukasz_K 0:a7c396f2ccf6 252 {
Lukasz_K 0:a7c396f2ccf6 253 double stopnie;
Lukasz_K 0:a7c396f2ccf6 254 double val = 180.0/3.14;
Lukasz_K 0:a7c396f2ccf6 255 stopnie = atan2(a,b)*val;
Lukasz_K 0:a7c396f2ccf6 256 return stopnie;
Lukasz_K 0:a7c396f2ccf6 257 }
Lukasz_K 0:a7c396f2ccf6 258
Lukasz_K 0:a7c396f2ccf6 259 //@@@@@@@@@@@_PRĘDKOŚĆ_NA_KĄT_@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
Lukasz_K 0:a7c396f2ccf6 260 void predkosc_kat(){
Lukasz_K 0:a7c396f2ccf6 261
Lukasz_K 0:a7c396f2ccf6 262 double z_rad = g_z; //przypisanie zmiennej z czujnika do zmiennej lokalnej
Lukasz_K 0:a7c396f2ccf6 263 if(z_rad>=-4.5&&z_rad<=-2.0){z_rad=0.0;} //dla wart 0 czujnik wskazuje ~-3,6
Lukasz_K 0:a7c396f2ccf6 264 double delta_t=0.02; //funkcja wywoływana jest co 2,5 milisekund
Lukasz_K 0:a7c396f2ccf6 265 double delta_kat=(z_rad*delta_t); //różnica kątowa= wart. czujnika * dt
Lukasz_K 0:a7c396f2ccf6 266 z_kat=z_kat+delta_kat; //ostateczny kąt = kąt obecny + d kąta
Lukasz_K 0:a7c396f2ccf6 267 z_stopnie = z_kat*(180/3.14); //zamiana radianów na stopnie
Lukasz_K 0:a7c396f2ccf6 268 }
Lukasz_K 0:a7c396f2ccf6 269
Lukasz_K 0:a7c396f2ccf6 270 //&&&&&&&&&&&&REGULACJA&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
Lukasz_K 0:a7c396f2ccf6 271 void regulacja(){
Lukasz_K 0:a7c396f2ccf6 272 Kp=0.2, Ki=0.05, Kd=0.01;
Lukasz_K 0:a7c396f2ccf6 273
Lukasz_K 0:a7c396f2ccf6 274 docelowa_pozycja_x=X; //pozycja docelowa dla zadajników
Lukasz_K 0:a7c396f2ccf6 275 docelowa_pozycja_y=Y;
Lukasz_K 0:a7c396f2ccf6 276 docelowa_pozycja_z=Z;
Lukasz_K 0:a7c396f2ccf6 277
Lukasz_K 0:a7c396f2ccf6 278 e_x=docelowa_pozycja_x-arc_tan_2(a_x,a_z); //uchyb regulacji
Lukasz_K 0:a7c396f2ccf6 279 e_y=docelowa_pozycja_y-arc_tan_2(a_y,a_z);
Lukasz_K 0:a7c396f2ccf6 280 e_z=docelowa_pozycja_z-z_stopnie;
Lukasz_K 0:a7c396f2ccf6 281
Lukasz_K 0:a7c396f2ccf6 282 Ix=Ix+e_x;
Lukasz_K 0:a7c396f2ccf6 283 Iy=Iy+e_y;
Lukasz_K 0:a7c396f2ccf6 284 Iz=Iz+e_z;
Lukasz_K 0:a7c396f2ccf6 285
Lukasz_K 0:a7c396f2ccf6 286 Dx=e_x - e_x_poprzednie;
Lukasz_K 0:a7c396f2ccf6 287 Dy=e_y - e_y_poprzednie;
Lukasz_K 0:a7c396f2ccf6 288 Dz=e_z - e_z_poprzednie;
Lukasz_K 0:a7c396f2ccf6 289
Lukasz_K 0:a7c396f2ccf6 290 //pwm1=(Kp*e_x); //P
Lukasz_K 0:a7c396f2ccf6 291 //pwm1=(Kp*e_x)+(Ki*Ix); //PI
Lukasz_K 0:a7c396f2ccf6 292 pwm1=(Kp*e_x)+(Kd*Dx); //PD
Lukasz_K 0:a7c396f2ccf6 293 //pwm1=(Kp*e_x)+(Ki*Ix)+(Kd*Dx); //PID
Lukasz_K 0:a7c396f2ccf6 294 //pwm2=(Kp*e_y); //P
Lukasz_K 0:a7c396f2ccf6 295 //pwm2=(Kp*e_y)+(Ki*Iy); //PI
Lukasz_K 0:a7c396f2ccf6 296 pwm2=(Kp*e_y)+(Kd*Dy); //PD
Lukasz_K 0:a7c396f2ccf6 297 //pwm2=(Kp*e_y)+(Ki*Iy)+(Kd*Dy); //PID
Lukasz_K 0:a7c396f2ccf6 298 //pwm3=(Kp*e_z); //P
Lukasz_K 0:a7c396f2ccf6 299 //pwm3=(Kp*e_z)+(Ki*Iz); //PI
Lukasz_K 0:a7c396f2ccf6 300 pwm3=(Kp*e_z)+(Kd*Dz); //PD
Lukasz_K 0:a7c396f2ccf6 301 //pwm3=(Kp*e_z)+(Ki*Iz)+(Kd*Dz); //PID
Lukasz_K 0:a7c396f2ccf6 302 }
Lukasz_K 0:a7c396f2ccf6 303
Lukasz_K 0:a7c396f2ccf6 304 void regulacja2(){
Lukasz_K 0:a7c396f2ccf6 305 Kp=0.1, Ki=0.05, Kd=0.01;
Lukasz_K 0:a7c396f2ccf6 306
Lukasz_K 0:a7c396f2ccf6 307 docelowa_pozycja_x=X; //pozycja docelowa dla zadajników
Lukasz_K 0:a7c396f2ccf6 308 docelowa_pozycja_y=Y;
Lukasz_K 0:a7c396f2ccf6 309 docelowa_pozycja_z=Z;
Lukasz_K 0:a7c396f2ccf6 310
Lukasz_K 0:a7c396f2ccf6 311 e_x=docelowa_pozycja_x-arc_tan_2(a_x,a_z); //uchyb regulacji
Lukasz_K 0:a7c396f2ccf6 312 e_y=docelowa_pozycja_y-arc_tan_2(a_y,a_z);
Lukasz_K 0:a7c396f2ccf6 313 e_z=docelowa_pozycja_z-z_stopnie;
Lukasz_K 0:a7c396f2ccf6 314
Lukasz_K 0:a7c396f2ccf6 315 Ix=Ix+e_x;
Lukasz_K 0:a7c396f2ccf6 316 Iy=Iy+e_y;
Lukasz_K 0:a7c396f2ccf6 317 Iz=Iz+e_z;
Lukasz_K 0:a7c396f2ccf6 318
Lukasz_K 0:a7c396f2ccf6 319 Dx=e_x - e_x_poprzednie;
Lukasz_K 0:a7c396f2ccf6 320 Dy=e_y - e_y_poprzednie;
Lukasz_K 0:a7c396f2ccf6 321 Dz=e_z - e_z_poprzednie;
Lukasz_K 0:a7c396f2ccf6 322
Lukasz_K 0:a7c396f2ccf6 323 pwm1=(Kp*e_x); //P
Lukasz_K 0:a7c396f2ccf6 324 //pwm1=(Kp*e_x)+(Ki*Ix); //PI
Lukasz_K 0:a7c396f2ccf6 325 pwm2=(Kp*e_y); //P
Lukasz_K 0:a7c396f2ccf6 326 //pwm2=(Kp*e_y)+(Ki*Iy); //PI
Lukasz_K 0:a7c396f2ccf6 327 pwm3=(Kp*e_z); //P
Lukasz_K 0:a7c396f2ccf6 328 //pwm3=(Kp*e_z)+(Ki*Iz); //PI
Lukasz_K 0:a7c396f2ccf6 329 }
Lukasz_K 0:a7c396f2ccf6 330
Lukasz_K 0:a7c396f2ccf6 331
Lukasz_K 0:a7c396f2ccf6 332 //+++++++++++DANE Z ZADAJNIKA+++++++++++++++++++++++++++++++++++++++++++++
Lukasz_K 0:a7c396f2ccf6 333 void odbiornik()
Lukasz_K 0:a7c396f2ccf6 334 {
Lukasz_K 0:a7c396f2ccf6 335 char str_x[1]; //definicja wektorów char
Lukasz_K 0:a7c396f2ccf6 336 char str_y[1];
Lukasz_K 0:a7c396f2ccf6 337 char str_z[1];
Lukasz_K 0:a7c396f2ccf6 338
abm_mechatronika 1:6e9fe61d7074 339 device_bt.scanf("%s",str_x); //kąty pochylenia urzadzeia mobilnego
abm_mechatronika 1:6e9fe61d7074 340 device_bt.scanf("%s",str_y);
abm_mechatronika 1:6e9fe61d7074 341 device_bt.scanf("%s",str_z);
Lukasz_K 0:a7c396f2ccf6 342
abm_mechatronika 1:6e9fe61d7074 343 X = atof(str_x); //zamiana na float
Lukasz_K 0:a7c396f2ccf6 344 Y = atof(str_y);
abm_mechatronika 1:6e9fe61d7074 345 Z = atof(str_z)-180; //rozlozenie zakresu kata obrotu symetrycznie wokol zera
Lukasz_K 0:a7c396f2ccf6 346 }
Lukasz_K 0:a7c396f2ccf6 347 void odbiornik2()
Lukasz_K 0:a7c396f2ccf6 348 {
Lukasz_K 0:a7c396f2ccf6 349 char str_x[1]; //definicja wektorów char
Lukasz_K 0:a7c396f2ccf6 350 char str_y[1];
Lukasz_K 0:a7c396f2ccf6 351 char str_z[1];
Lukasz_K 0:a7c396f2ccf6 352
abm_mechatronika 1:6e9fe61d7074 353 device_zadajnik.scanf("%s",str_x);
abm_mechatronika 1:6e9fe61d7074 354 device_zadajnik.scanf("%s",str_y);
abm_mechatronika 1:6e9fe61d7074 355 device_zadajnik.scanf("%s",str_z);
Lukasz_K 0:a7c396f2ccf6 356
Lukasz_K 0:a7c396f2ccf6 357 X = atof(str_x);
Lukasz_K 0:a7c396f2ccf6 358 Y = atof(str_y);
Lukasz_K 0:a7c396f2ccf6 359 Z = atof(str_z)-180;
Lukasz_K 0:a7c396f2ccf6 360 }
Lukasz_K 0:a7c396f2ccf6 361
Lukasz_K 0:a7c396f2ccf6 362 //+++++++++++STABILIZACJA_ZERO++++++++++++++++++++++++++++++++++++++++++
abm_mechatronika 1:6e9fe61d7074 363 void stabilizacja_zero(){ //platforma stabilizuje się do położenia w którym odczyty z akcelerometru wynoszą: ax=0, ay=0;
Lukasz_K 0:a7c396f2ccf6 364 //OŚ X'''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 365 regulacja();
Lukasz_K 0:a7c396f2ccf6 366
Lukasz_K 0:a7c396f2ccf6 367 if(a_x<=0.0)
Lukasz_K 0:a7c396f2ccf6 368 {
Lukasz_K 0:a7c396f2ccf6 369 OBROTY_M3=1;
Lukasz_K 0:a7c396f2ccf6 370 MOTOR_3.write(-0.022*(abs(pwm1))+1);
Lukasz_K 0:a7c396f2ccf6 371 }
Lukasz_K 0:a7c396f2ccf6 372 if(a_x>0.0)
Lukasz_K 0:a7c396f2ccf6 373 {
Lukasz_K 0:a7c396f2ccf6 374 OBROTY_M3=0;
Lukasz_K 0:a7c396f2ccf6 375 MOTOR_3.write(-0.022*(abs(pwm1))+1);
Lukasz_K 0:a7c396f2ccf6 376 }
Lukasz_K 0:a7c396f2ccf6 377 //OŚ Y'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 378 if(a_y<=0.0)
Lukasz_K 0:a7c396f2ccf6 379 {
Lukasz_K 0:a7c396f2ccf6 380 OBROTY_M1=1;
Lukasz_K 0:a7c396f2ccf6 381 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 382 }
Lukasz_K 0:a7c396f2ccf6 383 if(a_y>0.0)
Lukasz_K 0:a7c396f2ccf6 384 {
Lukasz_K 0:a7c396f2ccf6 385 OBROTY_M1=0;
Lukasz_K 0:a7c396f2ccf6 386 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 387 }
Lukasz_K 0:a7c396f2ccf6 388
Lukasz_K 0:a7c396f2ccf6 389 //OŚ Z'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 390 /* if(z_stopnie<=0.0)
Lukasz_K 0:a7c396f2ccf6 391 {
Lukasz_K 0:a7c396f2ccf6 392 OBROTY_M2=0;
Lukasz_K 0:a7c396f2ccf6 393 MOTOR_2.write(-0.022*(abs(pwm3))+1);
Lukasz_K 0:a7c396f2ccf6 394 }
Lukasz_K 0:a7c396f2ccf6 395 if(z_stopnie>0.0)
Lukasz_K 0:a7c396f2ccf6 396 {
Lukasz_K 0:a7c396f2ccf6 397 OBROTY_M2=1;
Lukasz_K 0:a7c396f2ccf6 398 MOTOR_2.write(-0.022*(abs(pwm3))+1);
Lukasz_K 0:a7c396f2ccf6 399 } */
Lukasz_K 0:a7c396f2ccf6 400
Lukasz_K 0:a7c396f2ccf6 401 }
Lukasz_K 0:a7c396f2ccf6 402 //++++++++++STABILIZACJA_SMARTFON++++++++++++++++++++++++++++++++++++++++++
Lukasz_K 0:a7c396f2ccf6 403 void stabilizacja_smartfon(){
Lukasz_K 0:a7c396f2ccf6 404 //predkosc_kat();
Lukasz_K 0:a7c396f2ccf6 405 regulacja2();
Lukasz_K 0:a7c396f2ccf6 406 //OŚ X'''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 407 if(arc_tan_2(a_x,a_z)<=X)
Lukasz_K 0:a7c396f2ccf6 408 {
Lukasz_K 0:a7c396f2ccf6 409 OBROTY_M3=1;
Lukasz_K 0:a7c396f2ccf6 410 MOTOR_3.write(-0.022*(abs(pwm1))+1); //y=ax+b
Lukasz_K 0:a7c396f2ccf6 411 }
Lukasz_K 0:a7c396f2ccf6 412 if(arc_tan_2(a_x,a_z)>X)
Lukasz_K 0:a7c396f2ccf6 413 {
Lukasz_K 0:a7c396f2ccf6 414 OBROTY_M3=0;
Lukasz_K 0:a7c396f2ccf6 415 MOTOR_3.write(-0.022*(abs(pwm1))+1);
Lukasz_K 0:a7c396f2ccf6 416 }
Lukasz_K 0:a7c396f2ccf6 417 //OŚ Y'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 418 if(arc_tan_2(a_y,a_z)<=Y)
Lukasz_K 0:a7c396f2ccf6 419 {
Lukasz_K 0:a7c396f2ccf6 420 OBROTY_M1=1;
Lukasz_K 0:a7c396f2ccf6 421 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 422 }
Lukasz_K 0:a7c396f2ccf6 423 if(arc_tan_2(a_y,a_z)>Y)
Lukasz_K 0:a7c396f2ccf6 424 {
Lukasz_K 0:a7c396f2ccf6 425 OBROTY_M1=0;
Lukasz_K 0:a7c396f2ccf6 426 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 427 }
Lukasz_K 0:a7c396f2ccf6 428 //OŚ Z'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 429 /* if(z_stopnie<=Z)
Lukasz_K 0:a7c396f2ccf6 430 {
Lukasz_K 0:a7c396f2ccf6 431 OBROTY_M2=1;
Lukasz_K 0:a7c396f2ccf6 432 MOTOR_2.write(0.9);
Lukasz_K 0:a7c396f2ccf6 433 }
Lukasz_K 0:a7c396f2ccf6 434 if(z_stopnie>Z)
Lukasz_K 0:a7c396f2ccf6 435 {
Lukasz_K 0:a7c396f2ccf6 436 OBROTY_M2=0;
Lukasz_K 0:a7c396f2ccf6 437 MOTOR_2.write(0.9);
Lukasz_K 0:a7c396f2ccf6 438 }*/
Lukasz_K 0:a7c396f2ccf6 439 }
Lukasz_K 0:a7c396f2ccf6 440
Lukasz_K 0:a7c396f2ccf6 441 //vvvvvvvvvvvvvvvvvvvvvv_ZADAJNIK_ODBIORNIK_vvvvvvvvvvvvvvvvvvvvvvvvv
Lukasz_K 0:a7c396f2ccf6 442 void zadajnik_odbiornik() {
Lukasz_K 0:a7c396f2ccf6 443
abm_mechatronika 1:6e9fe61d7074 444 regulacja(); //oblicza pwm1 - sygnał sterujacy
Lukasz_K 0:a7c396f2ccf6 445 //OŚ X'''''''''''''''''''''''''''''
abm_mechatronika 1:6e9fe61d7074 446 if(arc_tan_2(a_x,a_z)<=X) //X - wartosc zadana
Lukasz_K 0:a7c396f2ccf6 447 {
abm_mechatronika 1:6e9fe61d7074 448 OBROTY_M3=1; //kierunek obrotu
abm_mechatronika 1:6e9fe61d7074 449 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
Lukasz_K 0:a7c396f2ccf6 450 }
Lukasz_K 0:a7c396f2ccf6 451 if(arc_tan_2(a_x,a_z)>X)
Lukasz_K 0:a7c396f2ccf6 452 {
Lukasz_K 0:a7c396f2ccf6 453 OBROTY_M3=0;
Lukasz_K 0:a7c396f2ccf6 454 MOTOR_3.write(-0.022*(abs(pwm1))+1);
Lukasz_K 0:a7c396f2ccf6 455 }
Lukasz_K 0:a7c396f2ccf6 456 //OŚ Y'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 457 if(arc_tan_2(a_y,a_z)<=Y)
Lukasz_K 0:a7c396f2ccf6 458 {
Lukasz_K 0:a7c396f2ccf6 459 OBROTY_M1=1;
Lukasz_K 0:a7c396f2ccf6 460 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 461 }
Lukasz_K 0:a7c396f2ccf6 462 if(arc_tan_2(a_y,a_z)>Y)
Lukasz_K 0:a7c396f2ccf6 463 {
Lukasz_K 0:a7c396f2ccf6 464 OBROTY_M1=0;
Lukasz_K 0:a7c396f2ccf6 465 MOTOR_1.write(-0.022*(abs(pwm2))+1);
Lukasz_K 0:a7c396f2ccf6 466 }
Lukasz_K 0:a7c396f2ccf6 467 //OŚ Z'''''''''''''''''''''''''''''''''''''
Lukasz_K 0:a7c396f2ccf6 468 /* if(z_stopnie<=Z)
Lukasz_K 0:a7c396f2ccf6 469 {
Lukasz_K 0:a7c396f2ccf6 470 OBROTY_M2=1;
Lukasz_K 0:a7c396f2ccf6 471 MOTOR_2.write(0.9);
Lukasz_K 0:a7c396f2ccf6 472 }
Lukasz_K 0:a7c396f2ccf6 473 if(z_stopnie>Z)
Lukasz_K 0:a7c396f2ccf6 474 {
Lukasz_K 0:a7c396f2ccf6 475 OBROTY_M2=0;
Lukasz_K 0:a7c396f2ccf6 476 MOTOR_2.write(0.9);
Lukasz_K 0:a7c396f2ccf6 477 }*/
Lukasz_K 0:a7c396f2ccf6 478 }
Lukasz_K 0:a7c396f2ccf6 479 //vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
Lukasz_K 0:a7c396f2ccf6 480
Lukasz_K 0:a7c396f2ccf6 481 //&&&&&&&&&&&&&&&&&&&&TESTY CZUJNIKA&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
Lukasz_K 0:a7c396f2ccf6 482 void testy_czujnika(){
Lukasz_K 0:a7c396f2ccf6 483 // WOLNE OBROTY - 10 % PWM
Lukasz_K 0:a7c396f2ccf6 484 for(double i=1; i>=0.85; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 485 {
Lukasz_K 0:a7c396f2ccf6 486 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 487 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 488 //MOTOR_3.write(i);
abm_mechatronika 1:6e9fe61d7074 489 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 490 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 491 }
Lukasz_K 0:a7c396f2ccf6 492
Lukasz_K 0:a7c396f2ccf6 493 for(double j=50; j>=0; j=j-1)
Lukasz_K 0:a7c396f2ccf6 494 {
Lukasz_K 0:a7c396f2ccf6 495 wait(0.1);
abm_mechatronika 1:6e9fe61d7074 496 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 497 }
Lukasz_K 0:a7c396f2ccf6 498
Lukasz_K 0:a7c396f2ccf6 499 for(double i=0.85; i<=1; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 500 {
Lukasz_K 0:a7c396f2ccf6 501 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 502 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 503 //MOTOR_3.write(i);
abm_mechatronika 1:6e9fe61d7074 504 odczyt_pozycji_gimbala();
Lukasz_K 0:a7c396f2ccf6 505 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 506 }
Lukasz_K 0:a7c396f2ccf6 507 //SZYBKIE OBROTY - 100% PWM
Lukasz_K 0:a7c396f2ccf6 508 /* for(double i=1; i>=0.01; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 509 {
Lukasz_K 0:a7c396f2ccf6 510 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 511 //MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 512 //MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 513 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 514 }
Lukasz_K 0:a7c396f2ccf6 515
Lukasz_K 0:a7c396f2ccf6 516 for(double j=50; j>=0; j=j-1)
Lukasz_K 0:a7c396f2ccf6 517 {
Lukasz_K 0:a7c396f2ccf6 518 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 519 }
Lukasz_K 0:a7c396f2ccf6 520
Lukasz_K 0:a7c396f2ccf6 521 for(double i=0.00; i<=1; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 522 {
Lukasz_K 0:a7c396f2ccf6 523 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 524 //MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 525 //MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 526 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 527 }*/
Lukasz_K 0:a7c396f2ccf6 528 }
Lukasz_K 0:a7c396f2ccf6 529
Lukasz_K 0:a7c396f2ccf6 530 //$$$$$$$$$$$$$SEKWENCJE_POKAZ$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
Lukasz_K 0:a7c396f2ccf6 531 //SEKWENCJA 2
Lukasz_K 0:a7c396f2ccf6 532 void sekwencja_pokazowa(){ // funkcja sekwencja pokazowa
Lukasz_K 0:a7c396f2ccf6 533 for(double i=1; i>=0.6; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 534 {
Lukasz_K 0:a7c396f2ccf6 535 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 536 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 537 MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 538 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 539 }
Lukasz_K 0:a7c396f2ccf6 540 //WTRĄCENIE 2.1
Lukasz_K 0:a7c396f2ccf6 541 for(double i=1; i>=0.6; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 542 {
Lukasz_K 0:a7c396f2ccf6 543 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 544 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 545 }
Lukasz_K 0:a7c396f2ccf6 546 for(double i=0.6; i<=1.0; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 547 {
Lukasz_K 0:a7c396f2ccf6 548 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 549 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 550 }
Lukasz_K 0:a7c396f2ccf6 551 OBROTY_M2=1; //zmiana obrotów
Lukasz_K 0:a7c396f2ccf6 552 for(double i=1; i>=0.6; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 553 {
Lukasz_K 0:a7c396f2ccf6 554 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 555 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 556 }
Lukasz_K 0:a7c396f2ccf6 557 for(double i=10.6; i<=1.0; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 558 {
Lukasz_K 0:a7c396f2ccf6 559 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 560 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 561 }
Lukasz_K 0:a7c396f2ccf6 562 //===============
Lukasz_K 0:a7c396f2ccf6 563 wait(2.0);
Lukasz_K 0:a7c396f2ccf6 564 for(double i=0.6; i<=1.0; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 565 {
Lukasz_K 0:a7c396f2ccf6 566 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 567 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 568 MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 569 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 570 }
Lukasz_K 0:a7c396f2ccf6 571 wait(1.0);
Lukasz_K 0:a7c396f2ccf6 572 //SEKWENCJA 1
Lukasz_K 0:a7c396f2ccf6 573 for(double i=1; i>=0; i=i-0.01)
Lukasz_K 0:a7c396f2ccf6 574 {
Lukasz_K 0:a7c396f2ccf6 575 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 576 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 577 MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 578 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 579 }
Lukasz_K 0:a7c396f2ccf6 580 for(double i=0; i<=1; i=i+0.01)
Lukasz_K 0:a7c396f2ccf6 581 {
Lukasz_K 0:a7c396f2ccf6 582 MOTOR_1.write(i);
Lukasz_K 0:a7c396f2ccf6 583 MOTOR_2.write(i);
Lukasz_K 0:a7c396f2ccf6 584 MOTOR_3.write(i);
Lukasz_K 0:a7c396f2ccf6 585 wait(0.1);
Lukasz_K 0:a7c396f2ccf6 586 }
Lukasz_K 0:a7c396f2ccf6 587
Lukasz_K 0:a7c396f2ccf6 588 wait(2.0);
Lukasz_K 0:a7c396f2ccf6 589 }
Lukasz_K 0:a7c396f2ccf6 590 //$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
Lukasz_K 0:a7c396f2ccf6 591