Dominik Święch
main + engine
Fork of
Kola_Servo
by 
Dominik Święch
Revision 3:0de9cb065bee, committed 2016-06-14
- Comitter:
- yruiewyrui3
- Date:
- Tue Jun 14 07:39:51 2016 +0000
- Parent:
- 2:35dd310320e5
- Commit message:
- vol.2_milion
Changed in this revision
| Servo.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 35dd310320e5 -r 0de9cb065bee Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Tue Jun 14 07:39:51 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 35dd310320e5 -r 0de9cb065bee main.cpp
--- a/main.cpp Wed Jun 08 10:26:19 2016 +0000
+++ b/main.cpp Tue Jun 14 07:39:51 2016 +0000
@@ -1,23 +1,42 @@
#include "mbed.h"
#include "Engine.h"
+#include "Servo.h"
+//*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
Serial stm(PA_2, PA_3);
+Servo cam_poziom(PB_6);
+Servo cam_pion(PC_7);
+Engine eng_left = Engine(PB_13, PB_4, PB_10);
+Engine eng_right = Engine(PB_14, PB_5, PB_3);
+
+//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
const int BufferSize=17;
char bufor[BufferSize];
char prawa[3];
char lewa[3];
+char poziom[2];
+char pion[2];
int p=0;
int l=0;
+int pi=0;
+int po=0;
long counter=0;
Timer t;
-
- void cleanBuffer(char *buffor)
+//*************** Funkcja czyszczaca bufor *****************
+void cleanBuffer(char *buffor)
{
for(int i=0; i<BufferSize; i++)
buffor[i]=NULL;
+ buffor[BufferSize]=NULL;
}
+//by Michal 8-06-2016
+ //***************OPIS FUNKCJI isCorrectPacket******************************************************//
+ // funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ZCCZCC& gdzie Z={+/-} C={0,1,...9} //
+ //*******************KONIEC OPISU******************************************************************//
+//by Dominik 8-06-2016
+
bool isCorrectPacket(char *buffor){
if(bufor[0]!='@')
{
@@ -43,7 +62,7 @@
stm.printf("blad\n");
return false;
}
- if(bufor[16]!='&')
+ if(bufor[16]!='^')
{
// stm.printf("ZLY ZNAK KONCA\n");
stm.printf("blad\n");
@@ -62,30 +81,46 @@
stm.printf("blad\n");
return false;
}
- if(((int)buffor[i+8]) < 48 || ((int)buffor[i+8]) > 57)
- {
- // stm.printf("KAMERA GORA/DOL: NIE LICZBA\n");
+ }
+ if((buffor[10] != '+') && (buffor[10] != '-'))
+ {
+ // stm.printf("ZLY ZNAK +/- KAMERA PION\n");
+ stm.printf("blad\n");
+ return false;
+ }
+ if((buffor[13] != '+') && (buffor[13] != '-'))
+ {
+ // stm.printf("ZLY ZNAK +/- KAMERA POZIOM\n");
stm.printf("blad\n");
return false;
}
- if(((int)buffor[i+11]) < 48 || ((int)buffor[i+11]) > 57)
- {
- // stm.printf("KAMERA LEWO/PRAWO: NIE LICZBA\n");
+ for(int i=1; i< 3; i++){
+ if(((int)buffor[i+10]) < 48 || ((int)buffor[i+10]) > 57)
+ {
+ // stm.printf("KAMERA POZIOM: NIE LICZBA\n");
stm.printf("blad\n");
return false;
- }
+ }
+ if(((int)buffor[i+13]) < 48 || ((int)buffor[i+13]) > 57)
+ {
+ // stm.printf("KAMERA PION: NIE LICZBA\n");
+ stm.printf("blad\n");
+ return false;
+ }
}
- //stm.printf("pakiet poprawny\n");
+
+
+ //stm.printf("pakiet poprawny\n");]
+
return true;
}
int main(){
- Engine eng_left = Engine(PB_13, PB_4, PB_10);
- Engine eng_right = Engine(PB_14, PB_5, PB_3);
+
int i = 0;
stm.baud(115200);
-
+
while(true){
if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
@@ -94,8 +129,8 @@
//stm.printf("TIMEOUT-ROBOT STOP\n");
}
- //if((bufor[9] == NULL)&&(stm.readable()))
- if(bufor[9] == NULL)
+ //if((bufor[16] == NULL)&&(stm.readable()))
+ if(bufor[16] == NULL)
{
bufor[i] = stm.getc();
if(i==0){
@@ -109,7 +144,7 @@
i=1; //bo zerowy znak '@' juz zczytal
//end added part//
-
+
}else
i++;
@@ -124,24 +159,45 @@
t.reset();
t.start();
counter++;
- stm.printf("%ld\n", counter);
+ //stm.printf("%ld\n", counter);
-
+//*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
for(int j=0; j<=2; j++)
{
lewa[j]=bufor[j+2];
}
-
+
+//*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//
for(int k=0; k<=2; k++)
{
prawa[k]=bufor[k+6];
}
-
+
+//*******PRZYPISANIE DO KAMERY POZIOM*********//
+ for(int m=0; m<2; m++)
+ {
+
+ poziom[m]=bufor[m+11];
+ }
+
+//*******PRZYPISANIE DO KAMERY PION*********//
+ for(int n=0; n<2; n++)
+ {
+
+ pion[n]=bufor[n+14];
+ }
+
+//*******KONWERSJA CHAROW NA INTY*********//
sscanf(prawa, "%3d", &p);
sscanf(lewa, "%3d", &l);
-
+
+ sscanf(poziom, "%2d", &po);
+ sscanf(pion, "%2d", &pi);
+
+ //********KOREKCJA***************//
+
if(abs(l)>100)
{
l=100;
@@ -152,22 +208,48 @@
p=100;
}
+ if(abs(po)>90)
+ {
+ po=90;
+ }
+
+ if(abs(pi)>90)
+ {
+ pi=90;
+ }
+ //************WYPISYWANIE*******************//
+
// stm.printf("%ld\n", counter);
for(int j=0; j<=BufferSize; j++) //wypisz
{
- stm.printf("%c", bufor[j]);
+ //stm.printf("%c", bufor[j]);
+ stm.putc(bufor[j]);
}
- stm.printf("\n");
+ //stm.printf("\n");
//stm.printf("liczba lewa: %d \n", l);
//stm.printf("liczba prawa: %d \n", p);
if(bufor[1]=='-') l=-l;
if(bufor[5]=='-') p=-p;
-
+ if(bufor[10]=='-') po=-po;
+ if(bufor[13]=='-') pi=-pi;
+//**********ODPALANIE SILNIKOW******//
+ //stm.printf("poziom= %d\n", po);
+ //stm.printf("pion= %d\n", pi);
+ //stm.printf("lewy silnik= %d\n", l);
+ //stm.printf("prawy silnik= %d\n", p);
eng_left.move(l);
eng_right.move(p);
-
+
+ //for(int i=0; i<BufferSize; i++)
+ // {
+ stm.printf(bufor);
+ //}
+//**********RUCH KAMERĄ******//
+
+ cam_poziom.position(po);
+ cam_pion.position(pi);
}
i=0;