Dominik Święch / CZOSN

nowe + funkcje

Dependencies:   Servo

main.cpp@0:2e73d45d090e, 2016-06-14 (annotated)

Committer:
yruiewyrui3
Date:
Tue Jun 14 12:32:39 2016 +0000
Revision:
0:2e73d45d090e
nowe + funkcje

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yruiewyrui3 0:2e73d45d090e 1 #include "mbed.h"
yruiewyrui3 0:2e73d45d090e 2 #include "Engine.h"
yruiewyrui3 0:2e73d45d090e 3 #include "Servo.h"
yruiewyrui3 0:2e73d45d090e 4
yruiewyrui3 0:2e73d45d090e 5 //*************** Deklaracja wszystkich portów potrzebnych do obsługi**************
yruiewyrui3 0:2e73d45d090e 6 Serial stm(PA_2, PA_3);
yruiewyrui3 0:2e73d45d090e 7 Servo cam_poziom(PB_6);
yruiewyrui3 0:2e73d45d090e 8 Servo cam_pion(PC_7);
yruiewyrui3 0:2e73d45d090e 9 Engine eng_left = Engine(PB_13, PB_4, PB_10);
yruiewyrui3 0:2e73d45d090e 10 Engine eng_right = Engine(PB_14, PB_5, PB_3);
yruiewyrui3 0:2e73d45d090e 11
yruiewyrui3 0:2e73d45d090e 12 //*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************
yruiewyrui3 0:2e73d45d090e 13 const int BufferSize=10;
yruiewyrui3 0:2e73d45d090e 14 char bufor[BufferSize];
yruiewyrui3 0:2e73d45d090e 15 char prawa[3];
yruiewyrui3 0:2e73d45d090e 16 char lewa[3];
yruiewyrui3 0:2e73d45d090e 17 char poziom[2];
yruiewyrui3 0:2e73d45d090e 18 char pion[2];
yruiewyrui3 0:2e73d45d090e 19 int l=0;
yruiewyrui3 0:2e73d45d090e 20 int p=0;
yruiewyrui3 0:2e73d45d090e 21 int po=0;
yruiewyrui3 0:2e73d45d090e 22 int pi=0;
yruiewyrui3 0:2e73d45d090e 23 Timer t;
yruiewyrui3 0:2e73d45d090e 24
yruiewyrui3 0:2e73d45d090e 25 //*************** Funkcja czyszczaca bufor *****************
yruiewyrui3 0:2e73d45d090e 26 void cleanBuffer(char *buffor)
yruiewyrui3 0:2e73d45d090e 27 {
yruiewyrui3 0:2e73d45d090e 28 for(int i=0; i<BufferSize; i++)
yruiewyrui3 0:2e73d45d090e 29 buffor[i]=NULL;
yruiewyrui3 0:2e73d45d090e 30 buffor[BufferSize]=NULL;
yruiewyrui3 0:2e73d45d090e 31 }
yruiewyrui3 0:2e73d45d090e 32
yruiewyrui3 0:2e73d45d090e 33 //***************OPIS FUNKCJI isCorrectPacket******************************************************//
yruiewyrui3 0:2e73d45d090e 34 // funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} //
yruiewyrui3 0:2e73d45d090e 35 //*******************KONIEC OPISU******************************************************************//
yruiewyrui3 0:2e73d45d090e 36
yruiewyrui3 0:2e73d45d090e 37 bool isCorrectPacket(char *buffor){
yruiewyrui3 0:2e73d45d090e 38 if ((bufor[0]!='@') && (bufor[0]!='&'))
yruiewyrui3 0:2e73d45d090e 39 {
yruiewyrui3 0:2e73d45d090e 40 // stm.printf("ZLY ZNAK POCZATKU\n");
yruiewyrui3 0:2e73d45d090e 41 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 42 return false;
yruiewyrui3 0:2e73d45d090e 43 }
yruiewyrui3 0:2e73d45d090e 44 if((buffor[1] != '+') && (buffor[1] != '-'))
yruiewyrui3 0:2e73d45d090e 45 {
yruiewyrui3 0:2e73d45d090e 46 // stm.printf("ZLY ZNAK +/- LEWY\n");
yruiewyrui3 0:2e73d45d090e 47 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 48 return false;
yruiewyrui3 0:2e73d45d090e 49 }
yruiewyrui3 0:2e73d45d090e 50 if((buffor[5] != '+') && (buffor[5] != '-'))
yruiewyrui3 0:2e73d45d090e 51 {
yruiewyrui3 0:2e73d45d090e 52 // stm.printf("ZLY ZNAK +/- PRAWY\n");
yruiewyrui3 0:2e73d45d090e 53 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 54 return false;
yruiewyrui3 0:2e73d45d090e 55 }
yruiewyrui3 0:2e73d45d090e 56 if(bufor[9]!='$')
yruiewyrui3 0:2e73d45d090e 57 {
yruiewyrui3 0:2e73d45d090e 58 // stm.printf("ZLY ZNAK KONCA\n");
yruiewyrui3 0:2e73d45d090e 59 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 60 return false;
yruiewyrui3 0:2e73d45d090e 61 }
yruiewyrui3 0:2e73d45d090e 62 for(int i=2; i< 5; i++){
yruiewyrui3 0:2e73d45d090e 63 if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57)
yruiewyrui3 0:2e73d45d090e 64 {
yruiewyrui3 0:2e73d45d090e 65 // stm.printf("NA LEWE KOLO: NIE LICZBA\n");
yruiewyrui3 0:2e73d45d090e 66 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 67 return false;
yruiewyrui3 0:2e73d45d090e 68 }
yruiewyrui3 0:2e73d45d090e 69 if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57)
yruiewyrui3 0:2e73d45d090e 70 {
yruiewyrui3 0:2e73d45d090e 71 // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
yruiewyrui3 0:2e73d45d090e 72 stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 73 return false;
yruiewyrui3 0:2e73d45d090e 74 }
yruiewyrui3 0:2e73d45d090e 75 }
yruiewyrui3 0:2e73d45d090e 76 //stm.printf("pakiet poprawny\n");]
yruiewyrui3 0:2e73d45d090e 77 return true;
yruiewyrui3 0:2e73d45d090e 78 }
yruiewyrui3 0:2e73d45d090e 79
yruiewyrui3 0:2e73d45d090e 80 void move_wheels()
yruiewyrui3 0:2e73d45d090e 81 {
yruiewyrui3 0:2e73d45d090e 82 //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//
yruiewyrui3 0:2e73d45d090e 83 for(int j=0; j<=2; j++){
yruiewyrui3 0:2e73d45d090e 84 lewa[j]=bufor[j+2];
yruiewyrui3 0:2e73d45d090e 85 }
yruiewyrui3 0:2e73d45d090e 86 //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//
yruiewyrui3 0:2e73d45d090e 87 for(int k=0; k<=2; k++){
yruiewyrui3 0:2e73d45d090e 88 prawa[k]=bufor[k+6];
yruiewyrui3 0:2e73d45d090e 89 }
yruiewyrui3 0:2e73d45d090e 90 //*******KONWERSJA CHAROW NA INTY*********//
yruiewyrui3 0:2e73d45d090e 91 sscanf(lewa, "%3d", &l);
yruiewyrui3 0:2e73d45d090e 92 sscanf(prawa, "%3d", &p);
yruiewyrui3 0:2e73d45d090e 93 //********KOREKCJA***************//
yruiewyrui3 0:2e73d45d090e 94 if(abs(l)>100){
yruiewyrui3 0:2e73d45d090e 95 l=100;
yruiewyrui3 0:2e73d45d090e 96 }
yruiewyrui3 0:2e73d45d090e 97 if(abs(p)>100){
yruiewyrui3 0:2e73d45d090e 98 p=100;
yruiewyrui3 0:2e73d45d090e 99 }
yruiewyrui3 0:2e73d45d090e 100 //************WYPISYWANIE*******************//
yruiewyrui3 0:2e73d45d090e 101 for(int j=0; j<=BufferSize; j++){
yruiewyrui3 0:2e73d45d090e 102 stm.putc(bufor[j]);
yruiewyrui3 0:2e73d45d090e 103 }
yruiewyrui3 0:2e73d45d090e 104 //******KOREKCJA ZNAKU***********//
yruiewyrui3 0:2e73d45d090e 105 if(bufor[1]=='-') l=-l;
yruiewyrui3 0:2e73d45d090e 106 if(bufor[5]=='-') p=-p;
yruiewyrui3 0:2e73d45d090e 107 //**********ODPALANIE SILNIKOW******//
yruiewyrui3 0:2e73d45d090e 108 eng_left.move(l);
yruiewyrui3 0:2e73d45d090e 109 eng_right.move(p);
yruiewyrui3 0:2e73d45d090e 110
yruiewyrui3 0:2e73d45d090e 111 stm.printf(bufor);
yruiewyrui3 0:2e73d45d090e 112 }
yruiewyrui3 0:2e73d45d090e 113 void move_camera()
yruiewyrui3 0:2e73d45d090e 114 {
yruiewyrui3 0:2e73d45d090e 115 //*******PRZYPISANIE DO KAMERY POZIOM*********//
yruiewyrui3 0:2e73d45d090e 116 for(int m=0; m<2; m++){
yruiewyrui3 0:2e73d45d090e 117 poziom[m]=bufor[m+3];
yruiewyrui3 0:2e73d45d090e 118 }
yruiewyrui3 0:2e73d45d090e 119 //*******PRZYPISANIE DO KAMERY PION*********//
yruiewyrui3 0:2e73d45d090e 120 for(int n=0; n<2; n++){
yruiewyrui3 0:2e73d45d090e 121 pion[n]=bufor[n+3];
yruiewyrui3 0:2e73d45d090e 122 }
yruiewyrui3 0:2e73d45d090e 123 //*******KONWERSJA CHAROW NA INTY*********//
yruiewyrui3 0:2e73d45d090e 124 sscanf(poziom, "%2d", &po);
yruiewyrui3 0:2e73d45d090e 125 sscanf(pion, "%2d", &pi);
yruiewyrui3 0:2e73d45d090e 126 //********KOREKCJA***************//
yruiewyrui3 0:2e73d45d090e 127 if(abs(po)>90){
yruiewyrui3 0:2e73d45d090e 128 po=90;
yruiewyrui3 0:2e73d45d090e 129 }
yruiewyrui3 0:2e73d45d090e 130 if(abs(pi)>90){
yruiewyrui3 0:2e73d45d090e 131 pi=90;
yruiewyrui3 0:2e73d45d090e 132 }
yruiewyrui3 0:2e73d45d090e 133 //************WYPISYWANIE*******************//
yruiewyrui3 0:2e73d45d090e 134 for(int j=0; j<=BufferSize; j++){
yruiewyrui3 0:2e73d45d090e 135 stm.putc(bufor[j]);
yruiewyrui3 0:2e73d45d090e 136 }
yruiewyrui3 0:2e73d45d090e 137 if(bufor[1]=='-') po=-po;
yruiewyrui3 0:2e73d45d090e 138 if(bufor[5]=='-') pi=-pi;
yruiewyrui3 0:2e73d45d090e 139 //**********RUCH KAMERĄ******//
yruiewyrui3 0:2e73d45d090e 140 cam_poziom.position(po);
yruiewyrui3 0:2e73d45d090e 141 cam_pion.position(pi);
yruiewyrui3 0:2e73d45d090e 142 }
yruiewyrui3 0:2e73d45d090e 143 void fill_buffer()
yruiewyrui3 0:2e73d45d090e 144 {
yruiewyrui3 0:2e73d45d090e 145 bufor[i] = stm.getc();
yruiewyrui3 0:2e73d45d090e 146 if(i==0){
yruiewyrui3 0:2e73d45d090e 147 if((bufor[i]=='@') || (bufor[i]=='&')) //zaczynamy zapelniac bufor jak dostaniemy @
yruiewyrui3 0:2e73d45d090e 148 ++i;
yruiewyrui3 0:2e73d45d090e 149 }
yruiewyrui3 0:2e73d45d090e 150 else if(bufor[i]=='@'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
yruiewyrui3 0:2e73d45d090e 151 cleanBuffer(bufor);
yruiewyrui3 0:2e73d45d090e 152 //stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 153 bufor[0]='@';
yruiewyrui3 0:2e73d45d090e 154 i=1; //bo zerowy znak '@' juz zczytal
yruiewyrui3 0:2e73d45d090e 155 }
yruiewyrui3 0:2e73d45d090e 156 else if (bufor[i]=='&'){ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
yruiewyrui3 0:2e73d45d090e 157 cleanBuffer(bufor);
yruiewyrui3 0:2e73d45d090e 158 //stm.printf("blad\n");
yruiewyrui3 0:2e73d45d090e 159 bufor[0]='&';
yruiewyrui3 0:2e73d45d090e 160 i=1; //bo zerowy znak '@' juz zczytal
yruiewyrui3 0:2e73d45d090e 161 }
yruiewyrui3 0:2e73d45d090e 162 else
yruiewyrui3 0:2e73d45d090e 163 i++;
yruiewyrui3 0:2e73d45d090e 164 }
yruiewyrui3 0:2e73d45d090e 165
yruiewyrui3 0:2e73d45d090e 166 int main(){
yruiewyrui3 0:2e73d45d090e 167 int i = 0;
yruiewyrui3 0:2e73d45d090e 168 stm.baud(115200);
yruiewyrui3 0:2e73d45d090e 169 while(true){
yruiewyrui3 0:2e73d45d090e 170 if(t.read_ms()>200){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
yruiewyrui3 0:2e73d45d090e 171 eng_left.move(0);
yruiewyrui3 0:2e73d45d090e 172 eng_right.move(0);
yruiewyrui3 0:2e73d45d090e 173 }
yruiewyrui3 0:2e73d45d090e 174 //if((bufor[9] == NULL)&&(stm.readable()))
yruiewyrui3 0:2e73d45d090e 175 if(bufor[9] == NULL){
yruiewyrui3 0:2e73d45d090e 176 fill_buffer();
yruiewyrui3 0:2e73d45d090e 177 continue;
yruiewyrui3 0:2e73d45d090e 178 }
yruiewyrui3 0:2e73d45d090e 179 if(isCorrectPacket(bufor)){
yruiewyrui3 0:2e73d45d090e 180 t.stop();
yruiewyrui3 0:2e73d45d090e 181 t.reset();
yruiewyrui3 0:2e73d45d090e 182 t.start();
yruiewyrui3 0:2e73d45d090e 183 }
yruiewyrui3 0:2e73d45d090e 184 if(bufor[0]=='@'){
yruiewyrui3 0:2e73d45d090e 185 move_wheels();
yruiewyrui3 0:2e73d45d090e 186 }
yruiewyrui3 0:2e73d45d090e 187 else if(bufor[0]=='&'){
yruiewyrui3 0:2e73d45d090e 188 move_camera();
yruiewyrui3 0:2e73d45d090e 189 }
yruiewyrui3 0:2e73d45d090e 190 i=0;
yruiewyrui3 0:2e73d45d090e 191 cleanBuffer(bufor);
yruiewyrui3 0:2e73d45d090e 192 }
yruiewyrui3 0:2e73d45d090e 193 }