Dominik Święch / Mbed 2 deprecated STM_read

.

Dependencies:   HCSR04v2 Servo mbed

Fork of CZOSINHO by Dominik Święch

main.cpp@2:26a9f90142a0, 2016-06-21 (annotated)

Committer:
yruiewyrui3
Date:
Tue Jun 21 09:19:17 2016 +0000
Revision:
2:26a9f90142a0
Parent:
0:5919ea7b3b90 
.

Who changed what in which revision?

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