Justyna Guzik
Dependencies: czosnekengine_17_06 Servo_fullrange mbed
Fork of
CZOSN_ostateczny_29_06
by 
Klaudia Różycka
Diff: main.cpp
- Revision:
- 4:5e3f03e75645
- Parent:
- 3:d78e7f6477d8
- Child:
- 5:87151d829108
--- a/main.cpp Fri Jun 17 08:58:05 2016 +0000
+++ b/main.cpp Wed Jun 29 09:04:34 2016 +0000
@@ -2,208 +2,199 @@
#include "Engine.h"
#include "Servo.h"
#include "hcsr04.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);
+Servo cam_horizontal(PB_6);
+Servo cam_vertical(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 **************
HCSR04 sensor(PB_9, PB_8, 11770);
const int BufferSize=10;
-char bufor[BufferSize];
-char prawa[3];
-char lewa[3];
-char poziom[2];
-char pion[2];
-int l=0;
-int p=0;
-int po=0;
-int pi=0;
+char buffer[BufferSize];
+char right[3];
+char left[3];
+char add_horizontal[2];
+char add_vertical[2];
+int act_vertical=0, act_horizontal=0;
+int l=0, r=0, h=0, v=0;
Timer t, t_sonar;
-long distance=0;
-
-//*************** Funkcja czyszczaca bufor *****************
-void cleanBuffer(char *buffor)
- {
- for(int i=0; i<BufferSize; i++)
- buffor[i]=NULL;
- buffor[BufferSize]=NULL;
+long distance;
+
+
+void cleanBuffer()
+{
+ for(int i=0; i<=BufferSize; i++)
+ buffer[i]=NULL;
}
-//***************OPIS FUNKCJI isCorrectPacket******************************************************//
-// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} //
-//*******************KONIEC OPISU******************************************************************//
+char* intToChar(int value, char tab[3])
+{
+ tab[0]= (char)(value/100 + 48);
+ tab[1]= (char)((value-(100*(value/100)))/10 + 48);
+ tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48);
+ return tab;
+}
-bool isCorrectPacket(char *buffor){
- if (bufor[0]!='@')
- {
- // stm.printf("ZLY ZNAK POCZATKU\n");
- stm.printf("blad\n");
- return false;
- }
- if((buffor[1] != '+') && (buffor[1] != '-'))
- {
- // stm.printf("ZLY ZNAK +/- LEWY\n");
- stm.printf("blad\n");
- return false;
- }
- if((buffor[5] != '+') && (buffor[5] != '-'))
- {
- // stm.printf("ZLY ZNAK +/- PRAWY\n");
- stm.printf("blad\n");
+//**************************OPIS FUNKCJI isCorrectPacket******************************************************//
+// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}
+
+bool isCorrectPacket(){
+ if (buffer[0]!='@')
return false;
- }
- if((bufor[9]!='$')&&(bufor[9]!='&'))
- {
- // stm.printf("ZLY ZNAK KONCA\n");
- stm.printf("blad\n");
+
+ if((buffer[1] != '+') && (buffer[1] != '-'))
return false;
- }
+
+ if((buffer[5] != '+') && (buffer[5] != '-'))
+ return false;
+
+ if((buffer[9]!='$')&&(buffer[9]!='&'))
+ return false;
+
for(int i=2; i< 5; i++){
- if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57)
- {
- // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
- stm.printf("blad\n");
- return false;
- }
- if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57)
- {
- // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
- stm.printf("blad\n");
- return false;
- }
+ if(((int)buffer[i]) < 48 || ((int)buffer[i]) > 57)
+ return false;
+ if(((int)buffer[i+4]) < 48 || ((int)buffer[i+4]) > 57)
+ return false;
}
- //stm.printf("pakiet poprawny\n");]
+
return true;
}
-void calculate_velocity()
+
+void actualize_speed()
{
//*******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];
+ left[j]=buffer[j+2];
}
- //*******KONWERSJA CHAROW NA INTY*********//
- sscanf(lewa, "%3d", &l);
- sscanf(prawa, "%3d", &p);
- //********KOREKCJA***************//
- if(abs(l)>100){
+ //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY********//
+ for(int k=0; k<=2; k++){
+ right[k]=buffer[k+6];
+ }
+ //*******KONWERSJA CHAROW NA INTY********************//
+ sscanf(left, "%3d", &l);
+ sscanf(right, "%3d", &r);
+ if(abs(l)>100)
l=100;
- }
- if(abs(p)>100){
- p=100;
- }
- //************WYPISYWANIE*******************//
- for(int j=0; j<=BufferSize; j++){
- stm.putc(bufor[j]);
- }
- //******KOREKCJA ZNAKU***********//
- if(bufor[1]=='-') l=-l;
- if(bufor[5]=='-') p=-p;
+
+ if(abs(r)>100)
+ r=100;
+
+ if(buffer[1]=='-') l=-l;
+ if(buffer[5]=='-') r=-r;
+}
+
+void move_wheels()
+{
+ eng_left.move(l);
+ eng_right.move(r);
}
-
-void move_wheels()
+
+void actualize_servo_val()
{
- eng_left.move(l);
- eng_right.move(p);
+ //*******PRZYPISANIE DO KAMERY POZIOM*********//
+ for(int m=0; m<2; m++){
+ add_horizontal[m]=buffer[m+3];
+ }
+ //*******PRZYPISANIE DO KAMERY PION**********//
+ for(int n=0; n<2; n++){
+ add_vertical[n]=buffer[n+7];
+ }
+ //*******KONWERSJA CHAROW NA INTY************//
+ sscanf(add_horizontal, "%2d", &h);
+ sscanf(add_vertical, "%2d", &v);
+
+ if(buffer[1]=='-') h=-h;
+ if(buffer[5]=='-') v=-v;
+
+ act_horizontal = act_horizontal + h;
+ act_vertical = act_vertical + v;
+
+ if(act_horizontal>90)
+ act_horizontal=90;
+ else if (act_horizontal<-90)
+ act_horizontal=-90;
+
+ if(act_vertical>90)
+ act_vertical=90;
+ else if (act_vertical<-90)
+ act_vertical=-90;
}
void move_camera()
{
- //*******PRZYPISANIE DO KAMERY POZIOM*********//
- for(int m=0; m<2; m++){
- poziom[m]=bufor[m+3];
- }
- //*******PRZYPISANIE DO KAMERY PION*********//
- for(int n=0; n<2; n++){
- pion[n]=bufor[n+3];
- }
- //*******KONWERSJA CHAROW NA INTY*********//
- sscanf(poziom, "%2d", &po);
- sscanf(pion, "%2d", &pi);
- //********KOREKCJA***************//
- if(abs(po)>90){
- po=90;
- }
- if(abs(pi)>90){
- pi=90;
- }
- //************WYPISYWANIE*******************//
- for(int j=0; j<=BufferSize; j++){
- stm.putc(bufor[j]);
- }
- if(bufor[1]=='-') po=-po;
- if(bufor[5]=='-') pi=-pi;
- //**********RUCH KAMERĄ******//
- cam_poziom.position(po);
- cam_pion.position(pi);
+ cam_horizontal.position(act_horizontal);
+ cam_vertical.position(act_vertical);
+
}
+
+
int main(){
int i = 0;
- cleanBuffer(bufor);
+ cleanBuffer();
stm.baud(115200);
- t_sonar.start();
+ t_sonar.start();
while(true){
- if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
+ if(t.read_ms()>500)//jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
+ {
eng_left.move(0);
eng_right.move(0);
}
- if(t_sonar.read_ms()>1200){
-
+ if(t_sonar.read_ms()>1200)//dczyt odleglosci co 1,2s
+ {
+ distance = sensor.distance();
+ stm.printf("%d\n",distance);
+ /*intToChar(distance, tablicaf);
+ for (int f=0; f<3; f++)
+ {
+ stm.putc(tablicaf[f]);
+ }*/
t_sonar.stop();
t_sonar.reset();
- t_sonar.start();
- distance = sensor.distance();
- stm.printf("dystans %d \n",distance);
- }
-
- if(bufor[9] == NULL){
- bufor[i] = stm.getc();
+ t_sonar.start();
+ }
+ if(buffer[9] == NULL){
+ buffer[i] = stm.getc();
if(i==0){
- if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
- ++i;
+ if(buffer[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
+ ++i;
}
- else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
- cleanBuffer(bufor);
- bufor[0]='@';
- i=1; //bo zerowy znak '@' juz zczytal
+ else if(buffer[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
+ cleanBuffer();
+ buffer[0]='@';
+ i=1; //bo zerowy znak '@' juz sczytal
}
else i++;
- continue;
- }
- if(isCorrectPacket(bufor)){
+ continue;
+ }
+ if(isCorrectPacket()){
t.stop();
t.reset();
- t.start();
+ t.start();
}
- if(bufor[9]=='$'){
- calculate_velocity();
- if(distance>25){
+ if(buffer[9]=='$'){
+ actualize_speed();
+ if(distance>25)
move_wheels();
- }
else if (distance<=25){
- if(l<=0 && p<=0 || l*p<=0){
+ if(l<=0 && r<=0 || l*r<=0){
move_wheels();
}
else{
- eng_left.move(0);
- eng_right.move(0);
+ eng_left.move(0);
+ eng_right.move(0);
}
}
}
- else if(bufor[9]=='&'){
- move_camera();
+ else if(buffer[9]=='&'){
+ actualize_servo_val();
+ move_camera();
}
i=0;
- cleanBuffer(bufor);
- }
-}
\ No newline at end of file
+ cleanBuffer();
+ }
+}