Justyna Guzik / Mbed 2 deprecated STM_read24_06

Dependencies:   HCSR04v2 Servo mbed

Fork of STM_read by Dominik Święch

main.cpp@4:84f836aaf390, 2016-06-28 (annotated)

Committer:
jguzik
Date:
Tue Jun 28 12:52:35 2016 +0000
Revision:
4:84f836aaf390
Parent:
3:1a391dd91a35 

        

        
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);





jguzik

4:84f836aaf390


     8

Servo cam_poziom = Servo(PB_6);





jguzik

4:84f836aaf390


     9

Servo cam_pion = Servo(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







jguzik

4:84f836aaf390


    13







yruiewyrui3

0:5919ea7b3b90


    14

//*************** Deklaracja zmiennych globalnych, tablic, bufora na ramke znaków **************





yruiewyrui3

0:5919ea7b3b90


    15

HCSR04 sensor(PB_9, PB_8, 11770);





yruiewyrui3

0:5919ea7b3b90


    16

const int BufferSize=10;





yruiewyrui3

0:5919ea7b3b90


    17

char bufor[BufferSize];





yruiewyrui3

0:5919ea7b3b90


    18

char prawa[3];





yruiewyrui3

0:5919ea7b3b90


    19

char lewa[3];





jguzik

3:1a391dd91a35


    20

char dodaj_poziom[2];





jguzik

3:1a391dd91a35


    21

char dodaj_pion[2];





jguzik

3:1a391dd91a35


    22

int akt_pion=0, akt_poziom=0;





yruiewyrui3

0:5919ea7b3b90


    23

int l=0;





yruiewyrui3

0:5919ea7b3b90


    24

int p=0;





yruiewyrui3

0:5919ea7b3b90


    25

int po=0;





yruiewyrui3

0:5919ea7b3b90


    26

int pi=0;





jguzik

4:84f836aaf390


    27

Timer t, t_sonar, czas;





yruiewyrui3

2:26a9f90142a0


    28

long distance;





jguzik

4:84f836aaf390


    29

int counter=0, wypisz_czas;





jguzik

4:84f836aaf390


    30







yruiewyrui3

0:5919ea7b3b90


    31

//*************** Funkcja czyszczaca bufor *****************





yruiewyrui3

0:5919ea7b3b90


    32

void cleanBuffer(char *buffor)





yruiewyrui3

0:5919ea7b3b90


    33

 {





yruiewyrui3

0:5919ea7b3b90


    34

    for(int i=0; i<BufferSize; i++)     





yruiewyrui3

0:5919ea7b3b90


    35

        buffor[i]=NULL;





yruiewyrui3

0:5919ea7b3b90


    36

        buffor[BufferSize]=NULL;





yruiewyrui3

0:5919ea7b3b90


    37

}





yruiewyrui3

0:5919ea7b3b90


    38







jguzik

3:1a391dd91a35


    39

char* intToChar(int value, char tab[3])





jguzik

3:1a391dd91a35


    40

{





jguzik

3:1a391dd91a35


    41

        tab[0]= (char)(value/100 + 48);





jguzik

3:1a391dd91a35


    42

        tab[1]= (char)((value-(100*(value/100)))/10 + 48);





jguzik

3:1a391dd91a35


    43

        tab[2]= (char)(value-(100*(value/100))-(10*((value-(100*(value/100)))/10)) + 48);





jguzik

3:1a391dd91a35


    44

        /*stm.printf("Tablica z distance to: ");





jguzik

3:1a391dd91a35


    45

        for (int i=0; i<3; ++i)





jguzik

3:1a391dd91a35


    46

            stm.printf("%c",tab[i]);





jguzik

3:1a391dd91a35


    47

        stm.printf("\n");*/





jguzik

3:1a391dd91a35


    48

        return tab;





jguzik

3:1a391dd91a35


    49

}





jguzik

3:1a391dd91a35


    50







jguzik

3:1a391dd91a35


    51

void send2rpi(char tag, int wartosc){





jguzik

3:1a391dd91a35


    52

    char tablicaf[3]; 





jguzik

3:1a391dd91a35


    53

    stm.putc(tag);





jguzik

3:1a391dd91a35


    54

    intToChar(wartosc, tablicaf);





jguzik

3:1a391dd91a35


    55

    for (int f=0; f<3; f++){





jguzik

3:1a391dd91a35


    56

         stm.putc(tablicaf[f]);





jguzik

3:1a391dd91a35


    57

    }       





jguzik

3:1a391dd91a35


    58

}





jguzik

3:1a391dd91a35


    59







yruiewyrui3

0:5919ea7b3b90


    60

//***************OPIS FUNKCJI isCorrectPacket******************************************************//





yruiewyrui3

0:5919ea7b3b90


    61

//  funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}  //





yruiewyrui3

0:5919ea7b3b90


    62

//*******************KONIEC OPISU******************************************************************//





yruiewyrui3

0:5919ea7b3b90


    63







yruiewyrui3

0:5919ea7b3b90


    64

bool isCorrectPacket(char *buffor){





yruiewyrui3

0:5919ea7b3b90


    65

    if (bufor[0]!='@')





yruiewyrui3

0:5919ea7b3b90


    66

    {





jguzik

3:1a391dd91a35


    67

       // stm.printf("ZLY ZNAK POCZATKU\n");       





yruiewyrui3

0:5919ea7b3b90


    68

        return false;





yruiewyrui3

0:5919ea7b3b90


    69

    }





yruiewyrui3

0:5919ea7b3b90


    70

    if((buffor[1] != '+') && (buffor[1] != '-'))





yruiewyrui3

0:5919ea7b3b90


    71

        {





jguzik

3:1a391dd91a35


    72

       // stm.printf("ZLY ZNAK +/- LEWY\n");       





yruiewyrui3

0:5919ea7b3b90


    73

        return false;





yruiewyrui3

0:5919ea7b3b90


    74

    }





yruiewyrui3

0:5919ea7b3b90


    75

    if((buffor[5] != '+') && (buffor[5] != '-'))





yruiewyrui3

0:5919ea7b3b90


    76

    {





jguzik

3:1a391dd91a35


    77

       // stm.printf("ZLY ZNAK +/- PRAWY\n");       





yruiewyrui3

0:5919ea7b3b90


    78

        return false;





yruiewyrui3

0:5919ea7b3b90


    79

    }





yruiewyrui3

0:5919ea7b3b90


    80

    if((bufor[9]!='$')&&(bufor[9]!='&'))





yruiewyrui3

0:5919ea7b3b90


    81

    {





yruiewyrui3

0:5919ea7b3b90


    82

       // stm.printf("ZLY ZNAK KONCA\n");





jguzik

3:1a391dd91a35


    83

       return false;





yruiewyrui3

0:5919ea7b3b90


    84

    }





yruiewyrui3

0:5919ea7b3b90


    85

    for(int i=2; i< 5; i++){





yruiewyrui3

0:5919ea7b3b90


    86

        if(((int)buffor[i]) < 48 || ((int)buffor[i]) > 57) 





yruiewyrui3

0:5919ea7b3b90


    87

        {





yruiewyrui3

0:5919ea7b3b90


    88

       // stm.printf("NA LEWE KOLO: NIE LICZBA\n");





yruiewyrui3

0:5919ea7b3b90


    89

        return false;





yruiewyrui3

0:5919ea7b3b90


    90

        } 





yruiewyrui3

0:5919ea7b3b90


    91

        if(((int)buffor[i+4]) < 48 || ((int)buffor[i+4]) > 57) 





yruiewyrui3

0:5919ea7b3b90


    92

            {





jguzik

3:1a391dd91a35


    93

       // stm.printf("NA PRAWE KOLO: NIE LICZBA\n");       





yruiewyrui3

0:5919ea7b3b90


    94

        return false;





yruiewyrui3

0:5919ea7b3b90


    95

        } 





yruiewyrui3

0:5919ea7b3b90


    96

    }





yruiewyrui3

0:5919ea7b3b90


    97

    //stm.printf("pakiet poprawny\n");]





yruiewyrui3

0:5919ea7b3b90


    98

    return true;





yruiewyrui3

0:5919ea7b3b90


    99

}





yruiewyrui3

0:5919ea7b3b90


   100

 





jguzik

3:1a391dd91a35


   101

void actualize_speed()





yruiewyrui3

0:5919ea7b3b90


   102

{





yruiewyrui3

0:5919ea7b3b90


   103

        //*******PRZYPISANIE DO TABLICY LEWEJ LICZBY*********//





yruiewyrui3

0:5919ea7b3b90


   104

            for(int j=0; j<=2; j++){





yruiewyrui3

0:5919ea7b3b90


   105

                lewa[j]=bufor[j+2];   





yruiewyrui3

0:5919ea7b3b90


   106

            }





yruiewyrui3

0:5919ea7b3b90


   107

        //*******PRZYPISANIE DO TABLICY PRAWEJ LICZBY*********//   





yruiewyrui3

0:5919ea7b3b90


   108

            for(int k=0; k<=2; k++){





yruiewyrui3

0:5919ea7b3b90


   109

                prawa[k]=bufor[k+6];





yruiewyrui3

0:5919ea7b3b90


   110

            }





yruiewyrui3

0:5919ea7b3b90


   111

        //*******KONWERSJA CHAROW NA INTY*********//                    





yruiewyrui3

0:5919ea7b3b90


   112

            sscanf(lewa, "%3d", &l);





yruiewyrui3

0:5919ea7b3b90


   113

            sscanf(prawa, "%3d", &p);





yruiewyrui3

0:5919ea7b3b90


   114

        //********KOREKCJA***************//  





yruiewyrui3

0:5919ea7b3b90


   115

            if(abs(l)>100){





yruiewyrui3

0:5919ea7b3b90


   116

                l=100;





yruiewyrui3

0:5919ea7b3b90


   117

            }





yruiewyrui3

0:5919ea7b3b90


   118

            if(abs(p)>100){





yruiewyrui3

0:5919ea7b3b90


   119

                p=100;





jguzik

3:1a391dd91a35


   120

            }        





yruiewyrui3

0:5919ea7b3b90


   121

        //******KOREKCJA ZNAKU***********//





yruiewyrui3

0:5919ea7b3b90


   122

            if(bufor[1]=='-') l=-l;





jguzik

3:1a391dd91a35


   123

            if(bufor[5]=='-') p=-p;    





jguzik

3:1a391dd91a35


   124

    





jguzik

3:1a391dd91a35


   125

}





jguzik

3:1a391dd91a35


   126







jguzik

3:1a391dd91a35


   127







jguzik

3:1a391dd91a35


   128

void move_wheels()





jguzik

3:1a391dd91a35


   129

{





yruiewyrui3

0:5919ea7b3b90


   130

            eng_left.move(l);





yruiewyrui3

0:5919ea7b3b90


   131

            eng_right.move(p);





yruiewyrui3

0:5919ea7b3b90


   132

}





jguzik

3:1a391dd91a35


   133







jguzik

3:1a391dd91a35


   134

void actualize_servo_values()





jguzik

3:1a391dd91a35


   135

{    





yruiewyrui3

0:5919ea7b3b90


   136

        //*******PRZYPISANIE DO KAMERY POZIOM*********//   





yruiewyrui3

0:5919ea7b3b90


   137

            for(int m=0; m<2; m++){             





jguzik

3:1a391dd91a35


   138

                dodaj_poziom[m]=bufor[m+3];





yruiewyrui3

0:5919ea7b3b90


   139

            }





yruiewyrui3

0:5919ea7b3b90


   140

        //*******PRZYPISANIE DO KAMERY PION*********//   





yruiewyrui3

0:5919ea7b3b90


   141

            for(int n=0; n<2; n++){





jguzik

3:1a391dd91a35


   142

                dodaj_pion[n]=bufor[n+7];





yruiewyrui3

0:5919ea7b3b90


   143

            }





yruiewyrui3

0:5919ea7b3b90


   144

        //*******KONWERSJA CHAROW NA INTY*********//                    





jguzik

3:1a391dd91a35


   145

            sscanf(dodaj_poziom, "%2d", &po);





jguzik

3:1a391dd91a35


   146

            sscanf(dodaj_pion, "%2d", &pi);





jguzik

3:1a391dd91a35


   147

        //********KOREKCJA***************// 





yruiewyrui3

0:5919ea7b3b90


   148

            if(bufor[1]=='-') po=-po;





yruiewyrui3

0:5919ea7b3b90


   149

            if(bufor[5]=='-') pi=-pi;





jguzik

3:1a391dd91a35


   150

            





jguzik

3:1a391dd91a35


   151

            akt_poziom = akt_poziom + po;





jguzik

3:1a391dd91a35


   152

            akt_pion = akt_pion + pi;





jguzik

3:1a391dd91a35


   153

            





jguzik

3:1a391dd91a35


   154

            if(akt_poziom>90)





jguzik

3:1a391dd91a35


   155

                akt_poziom=90;





jguzik

3:1a391dd91a35


   156

            else if (akt_poziom<-90)





jguzik

3:1a391dd91a35


   157

                akt_poziom=-90;





jguzik

3:1a391dd91a35


   158

                                   





jguzik

3:1a391dd91a35


   159

            if(akt_pion>90)





jguzik

3:1a391dd91a35


   160

                akt_pion=90;





jguzik

3:1a391dd91a35


   161

            else if (akt_pion<-90)





jguzik

4:84f836aaf390


   162

                akt_poziom=-90;                      





yruiewyrui3

0:5919ea7b3b90


   163

}





yruiewyrui3

0:5919ea7b3b90


   164







jguzik

3:1a391dd91a35


   165







jguzik

3:1a391dd91a35


   166

void move_camera()





jguzik

3:1a391dd91a35


   167

{               





jguzik

4:84f836aaf390


   168

            cam_pion.SetPosition(akt_pion*5+1500);  





jguzik

4:84f836aaf390


   169

            cam_poziom.SetPosition(akt_poziom*5+1500);





jguzik

4:84f836aaf390


   170

             /*while(1) {





jguzik

4:84f836aaf390


   171

                  for (int pos = 1000; pos < 2100; pos += 10) {





jguzik

4:84f836aaf390


   172

                     cam_pion.SetPosition(pos);  





jguzik

4:84f836aaf390


   173

                     stm.printf("position %d\n", pos);





jguzik

4:84f836aaf390


   174

                     wait_ms(10);





jguzik

4:84f836aaf390


   175

                  }





jguzik

4:84f836aaf390


   176

                  for (int pos = 2100; pos > 1000; pos -= 10) {





jguzik

4:84f836aaf390


   177

                      cam_pion.SetPosition(pos); 





jguzik

4:84f836aaf390


   178

                      wait_ms(10); 





jguzik

4:84f836aaf390


   179

                  }





jguzik

4:84f836aaf390


   180

              }*/





jguzik

3:1a391dd91a35


   181

}





jguzik

3:1a391dd91a35


   182







jguzik

3:1a391dd91a35


   183







jguzik

3:1a391dd91a35


   184







yruiewyrui3

0:5919ea7b3b90


   185

int main(){





yruiewyrui3

0:5919ea7b3b90


   186

    int i = 0;





yruiewyrui3

0:5919ea7b3b90


   187

    cleanBuffer(bufor);





yruiewyrui3

0:5919ea7b3b90


   188

    stm.baud(115200);





yruiewyrui3

2:26a9f90142a0


   189

    t_sonar.start();





jguzik

3:1a391dd91a35


   190

    distance = 0;





jguzik

4:84f836aaf390


   191

    





jguzik

4:84f836aaf390


   192

    cam_poziom.Enable(1450,20000);





jguzik

4:84f836aaf390


   193

    cam_pion.Enable(1450,20000);





jguzik

3:1a391dd91a35


   194

    move_camera();





jguzik

4:84f836aaf390


   195

    czas.start();





jguzik

3:1a391dd91a35


   196

    





jguzik

3:1a391dd91a35


   197

    while(true)





jguzik

3:1a391dd91a35


   198

    {





jguzik

3:1a391dd91a35


   199

        





jguzik

3:1a391dd91a35


   200

        if(t.read_ms()>500){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota





yruiewyrui3

0:5919ea7b3b90


   201

            eng_left.move(0);





yruiewyrui3

0:5919ea7b3b90


   202

            eng_right.move(0);





yruiewyrui3

0:5919ea7b3b90


   203

            }





jguzik

4:84f836aaf390


   204

            





yruiewyrui3

0:5919ea7b3b90


   205

        if(t_sonar.read_ms()>1200){





yruiewyrui3

2:26a9f90142a0


   206

            distance = sensor.distance(); 





yruiewyrui3

0:5919ea7b3b90


   207

            t_sonar.stop();





yruiewyrui3

0:5919ea7b3b90


   208

            t_sonar.reset();





yruiewyrui3

0:5919ea7b3b90


   209

            t_sonar.start(); 





yruiewyrui3

0:5919ea7b3b90


   210

            }





jguzik

3:1a391dd91a35


   211

        if(bufor[9] == NULL)





jguzik

3:1a391dd91a35


   212

        {





yruiewyrui3

0:5919ea7b3b90


   213

            bufor[i] = stm.getc();           





jguzik

3:1a391dd91a35


   214

            if(i==0)





jguzik

3:1a391dd91a35


   215

            {





yruiewyrui3

0:5919ea7b3b90


   216

                if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @





yruiewyrui3

0:5919ea7b3b90


   217

                   ++i;    





yruiewyrui3

0:5919ea7b3b90


   218

            }





jguzik

3:1a391dd91a35


   219

            else if(bufor[i]=='@')





jguzik

3:1a391dd91a35


   220

            { // i != 0 //interpretujemy jakby potencjalny poczatek ramki





yruiewyrui3

0:5919ea7b3b90


   221

                cleanBuffer(bufor);





yruiewyrui3

0:5919ea7b3b90


   222

                bufor[0]='@';





yruiewyrui3

0:5919ea7b3b90


   223

                i=1; //bo zerowy znak '@' juz zczytal





jguzik

3:1a391dd91a35


   224

            }





yruiewyrui3

0:5919ea7b3b90


   225

            else i++;





jguzik

3:1a391dd91a35


   226

             





jguzik

3:1a391dd91a35


   227

            continue;





yruiewyrui3

0:5919ea7b3b90


   228

        }         





jguzik

3:1a391dd91a35


   229

        if(isCorrectPacket(bufor))





jguzik

3:1a391dd91a35


   230

        {   





yruiewyrui3

0:5919ea7b3b90


   231

            t.stop();





yruiewyrui3

0:5919ea7b3b90


   232

            t.reset();





jguzik

4:84f836aaf390


   233

            t.start();   





jguzik

4:84f836aaf390


   234

            counter++;





jguzik

4:84f836aaf390


   235

            if(czas.read_ms()>1000){





jguzik

4:84f836aaf390


   236

                stm.printf("counter %d\n", counter);





jguzik

4:84f836aaf390


   237

                counter=0;





jguzik

4:84f836aaf390


   238

                czas.stop();





jguzik

4:84f836aaf390


   239

                czas.reset();





jguzik

4:84f836aaf390


   240

                czas.start();   





jguzik

4:84f836aaf390


   241

            }           





yruiewyrui3

0:5919ea7b3b90


   242

        }





yruiewyrui3

0:5919ea7b3b90


   243

        if(bufor[9]=='$'){





jguzik

3:1a391dd91a35


   244

            actualize_speed();





jguzik

3:1a391dd91a35


   245

            if(distance>25){





jguzik

3:1a391dd91a35


   246

                move_wheels();





jguzik

3:1a391dd91a35


   247

            }





jguzik

3:1a391dd91a35


   248

            else if (distance<=25){





jguzik

3:1a391dd91a35


   249

                if(l<=0 && p<=0 || l*p<0){                





jguzik

3:1a391dd91a35


   250

                    move_wheels();





jguzik

3:1a391dd91a35


   251

                }





jguzik

3:1a391dd91a35


   252

                else{





jguzik

3:1a391dd91a35


   253

                eng_left.move(0);





jguzik

3:1a391dd91a35


   254

                eng_right.move(0);                





jguzik

3:1a391dd91a35


   255

                }





jguzik

3:1a391dd91a35


   256

            }





yruiewyrui3

0:5919ea7b3b90


   257

        }





jguzik

4:84f836aaf390


   258

        else if(bufor[9]=='&'){            





jguzik

3:1a391dd91a35


   259

            actualize_servo_values();





jguzik

3:1a391dd91a35


   260

            move_camera();





jguzik

4:84f836aaf390


   261

           /* counter++;





jguzik

4:84f836aaf390


   262

            if(czas.read_ms()>1000){





jguzik

4:84f836aaf390


   263

               // stm.printf("counter %d\n", counter);





jguzik

4:84f836aaf390


   264

                counter=0;





jguzik

4:84f836aaf390


   265

                czas.stop();





jguzik

4:84f836aaf390


   266

                czas.reset();





jguzik

4:84f836aaf390


   267

                czas.start();   





jguzik

4:84f836aaf390


   268

            }*/





yruiewyrui3

0:5919ea7b3b90


   269

        }





yruiewyrui3

0:5919ea7b3b90


   270

        i=0;





jguzik

3:1a391dd91a35


   271

        //send2rpi('s', distance);





yruiewyrui3

0:5919ea7b3b90


   272

        cleanBuffer(bufor);





jguzik

3:1a391dd91a35


   273

        





yruiewyrui3

0:5919ea7b3b90


   274

    }    





jguzik

3:1a391dd91a35


   275

}