Dominik Święch / Mbed 2 deprecated CZOSINHO

.

Dependencies:   HCSR04 Servo mbed

main.cpp@1:b57f208d1d95, 2016-06-21 (annotated)

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