main.cpp

00001 // Keep sweeping servo from left to right
00002 //dla serwa2=1700 puszczenie//dla windy 1650=max wysokosc
00003 //dla serwa2=1500 zatrzask // dla windy 2100=min wysokosc
00004 #include "mbed.h"
00005 #include "Servo.h"
00006 bool petla=0;
00007 float potencjometr_analog;
00008 bool jest_klocek=0;
00009 bool rozkaz_miejsce_pierwszego_klocka=1;
00010 bool rozkaz_miejsce_drugiego_klocka=1;
00011 bool rozkaz_miejsce_trzeciego_klocka=1;
00012 bool brak_klocka=0;
00013 bool zrozumial_brak_klocka;
00014 bool jest_cala_wieza;
00015 Servo Servo1(PC_9);
00016 Servo Servo2(PC_8);
00017 Servo Winda(PA_12);
00018 AnalogIn potencjometr_analog_odczyt(A0);
00019 
00020 int main()
00021 {
00022     Servo1.Enable(2500,20000);
00023     Servo2.Enable(2000,20000);
00024     Winda.Enable(2100,20000);
00025     while(1) {
00026 
00027 wait(1); // czas na ustawienie poczatkowe serw
00028         if (petla==0) {
00029             if (rozkaz_miejsce_pierwszego_klocka==1) {
00030                 Servo1.SetPosition(650); // zatrzask dolnego chwytaka
00031                 wait(1);
00032                 potencjometr_analog = potencjometr_analog_odczyt.read(); // odczyt wartosci z potencjometru
00033                 printf("%f\n", potencjometr_analog);
00034                 if (potencjometr_analog <0.782) brak_klocka=1;
00035                 if (potencjometr_analog >= 0.782) {
00036                     jest_klocek=1;
00037                     Servo2.SetPosition(1450); // zatrszask chwytaka windowego
00038                     Servo1.SetPosition(2500); // otwarcie dolnego chwytaka
00039                     jest_klocek=0;
00040                     wait_ms(300);// czas na zatrzask chwytaka windowego
00041                     Winda.SetPosition(1905); // wjazd winda z klockiem na poziom 7,5 cm
00042                     wait(3);// czas na dojechanie do drugiego klocka
00043                 }
00044 
00045             }
00046             if (rozkaz_miejsce_drugiego_klocka==1) {
00047                 Servo1.SetPosition(650); // zatrzask dolnego chwytaka
00048                 wait(1);
00049                 potencjometr_analog = potencjometr_analog_odczyt.read(); // odczyt wartosci z potencjometru
00050                 printf("%f\n", potencjometr_analog);
00051                 if (potencjometr_analog <0.782) brak_klocka=1;
00052                 if (potencjometr_analog >= 0.782) {
00053                     jest_klocek=1;
00054                     Winda.SetPosition(1915); // delikatne opuszczenie windy czyli polozenie pierwszego klocka na drugim
00055                     wait_ms(50);
00056                     Servo2.SetPosition(1650); // otwarcie chwytaka windowego
00057                     Winda.SetPosition(2100); // zjechanie winda na dol po drugiego klocka
00058                     wait(1); // czas na zjechanie windy po drugiego klocka
00059                     Servo2.SetPosition(1450); // zatrszask chwytaka windowego
00060                     Servo1.SetPosition(2500); // otwarcie dolnego chwytaka
00061                     jest_klocek=0;
00062                     wait_ms(300); // czas na zatrzask chwytaka windowego
00063                     Winda.SetPosition(1900); // wjazd winda z klockiem na poziom 7,5 cm
00064                     wait(3); // czas na dojechanie do drugiego klocka
00065                 }
00066 
00067             }
00068             if (rozkaz_miejsce_trzeciego_klocka==1) {
00069                 Servo1.SetPosition(650); // zatrzask dolnego chwytaka
00070                 wait(1);
00071                 potencjometr_analog = potencjometr_analog_odczyt.read(); // odczyt wartosci z potencjometru
00072                 printf("%f\n", potencjometr_analog);
00073                 if (potencjometr_analog <0.782) brak_klocka=1;
00074                 if (potencjometr_analog >= 0.782) {
00075                     jest_klocek=1;
00076                     Winda.SetPosition(1915); // delikatne opuszczenie windy czyli polozenie pierwszego klocka na drugim
00077                     jest_cala_wieza=1;
00078                     
00079 
00080                 }
00081 
00082             }
00083             if (zrozumial_brak_klocka==1) brak_klocka=0;
00084             petla=1;
00085         }
00086 
00087     }
00088 
00089 }