STM_read24_06 -

Users » jguzik » Code » STM_read24_06

Justyna Guzik / Mbed 2 deprecated STM_read24_06

main.cpp@3:1a391dd91a35, 2016-06-24 (annotated)

Committer:: jguzik
Date:: Fri Jun 24 13:43:08 2016 +0000
Revision:: 3:1a391dd91a35
Parent:: 2:26a9f90142a0
Child:: 4:84f836aaf390

Who changed what in which revision?

User	Revision	Line number	New 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];
jguzik	3:1a391dd91a35	19	char dodaj_poziom[2];
jguzik	3:1a391dd91a35	20	char dodaj_pion[2];
jguzik	3:1a391dd91a35	21	int akt_pion=0, akt_poziom=0;
yruiewyrui3	0:5919ea7b3b90	22	int l=0;
yruiewyrui3	0:5919ea7b3b90	23	int p=0;
yruiewyrui3	0:5919ea7b3b90	24	int po=0;
yruiewyrui3	0:5919ea7b3b90	25	int pi=0;
yruiewyrui3	0:5919ea7b3b90	26	Timer t, t_sonar;
yruiewyrui3	2:26a9f90142a0	27	long distance;
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
jguzik	3:1a391dd91a35	37	char* intToChar(int value, char tab[3])
jguzik	3:1a391dd91a35	38	{
jguzik	3:1a391dd91a35	39	tab[0]= (char)(value/100 + 48);
jguzik	3:1a391dd91a35	40	tab[1]= (char)((value-(100*(value/100)))/10 + 48);
jguzik	3:1a391dd91a35	41	tab[2]= (char)(value-(100(value/100))-(10((value-(100*(value/100)))/10)) + 48);
jguzik	3:1a391dd91a35	42	/*stm.printf("Tablica z distance to: ");
jguzik	3:1a391dd91a35	43	for (int i=0; i<3; ++i)
jguzik	3:1a391dd91a35	44	stm.printf("%c",tab[i]);
jguzik	3:1a391dd91a35	45	stm.printf("\n");*/
jguzik	3:1a391dd91a35	46	return tab;
jguzik	3:1a391dd91a35	47	}
jguzik	3:1a391dd91a35	48
jguzik	3:1a391dd91a35	49	void send2rpi(char tag, int wartosc){
jguzik	3:1a391dd91a35	50	char tablicaf[3];
jguzik	3:1a391dd91a35	51	stm.putc(tag);
jguzik	3:1a391dd91a35	52	intToChar(wartosc, tablicaf);
jguzik	3:1a391dd91a35	53	for (int f=0; f<3; f++){
jguzik	3:1a391dd91a35	54	stm.putc(tablicaf[f]);
jguzik	3:1a391dd91a35	55	}
jguzik	3:1a391dd91a35	56	}
jguzik	3:1a391dd91a35	57
yruiewyrui3	0:5919ea7b3b90	58	//*************OPIS FUNKCJI isCorrectPacket****************************************************//
yruiewyrui3	0:5919ea7b3b90	59	// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9} //
yruiewyrui3	0:5919ea7b3b90	60	//*****************KONIEC OPISU****************************************************************//
yruiewyrui3	0:5919ea7b3b90	61
yruiewyrui3	0:5919ea7b3b90	62	bool isCorrectPacket(char *buffor){
yruiewyrui3	0:5919ea7b3b90	63	if (bufor[0]!='@')
yruiewyrui3	0:5919ea7b3b90	64	{
jguzik	3:1a391dd91a35	65	// stm.printf("ZLY ZNAK POCZATKU\n");
yruiewyrui3	0:5919ea7b3b90	66	return false;
yruiewyrui3	0:5919ea7b3b90	67	}
yruiewyrui3	0:5919ea7b3b90	68	if((buffor[1] != '+') && (buffor[1] != '-'))
yruiewyrui3	0:5919ea7b3b90	69	{
jguzik	3:1a391dd91a35	70	// stm.printf("ZLY ZNAK +/- LEWY\n");
yruiewyrui3	0:5919ea7b3b90	71	return false;
yruiewyrui3	0:5919ea7b3b90	72	}
yruiewyrui3	0:5919ea7b3b90	73	if((buffor[5] != '+') && (buffor[5] != '-'))
yruiewyrui3	0:5919ea7b3b90	74	{
jguzik	3:1a391dd91a35	75	// stm.printf("ZLY ZNAK +/- PRAWY\n");
yruiewyrui3	0:5919ea7b3b90	76	return false;
yruiewyrui3	0:5919ea7b3b90	77	}
yruiewyrui3	0:5919ea7b3b90	78	if((bufor[9]!='$')&&(bufor[9]!='&'))
yruiewyrui3	0:5919ea7b3b90	79	{
yruiewyrui3	0:5919ea7b3b90	80	// stm.printf("ZLY ZNAK KONCA\n");
jguzik	3:1a391dd91a35	81	return false;
yruiewyrui3	0:5919ea7b3b90	82	}
yruiewyrui3	0:5919ea7b3b90	83	for(int i=2; i< 5; i++){
yruiewyrui3	0:5919ea7b3b90	84	if(((int)buffor[i]) < 48 \|\| ((int)buffor[i]) > 57)
yruiewyrui3	0:5919ea7b3b90	85	{
yruiewyrui3	0:5919ea7b3b90	86	// stm.printf("NA LEWE KOLO: NIE LICZBA\n");
yruiewyrui3	0:5919ea7b3b90	87	return false;
yruiewyrui3	0:5919ea7b3b90	88	}
yruiewyrui3	0:5919ea7b3b90	89	if(((int)buffor[i+4]) < 48 \|\| ((int)buffor[i+4]) > 57)
yruiewyrui3	0:5919ea7b3b90	90	{
jguzik	3:1a391dd91a35	91	// stm.printf("NA PRAWE KOLO: NIE LICZBA\n");
yruiewyrui3	0:5919ea7b3b90	92	return false;
yruiewyrui3	0:5919ea7b3b90	93	}
yruiewyrui3	0:5919ea7b3b90	94	}
yruiewyrui3	0:5919ea7b3b90	95	//stm.printf("pakiet poprawny\n");]
yruiewyrui3	0:5919ea7b3b90	96	return true;
yruiewyrui3	0:5919ea7b3b90	97	}
yruiewyrui3	0:5919ea7b3b90	98
jguzik	3:1a391dd91a35	99	void actualize_speed()
yruiewyrui3	0:5919ea7b3b90	100	{
yruiewyrui3	0:5919ea7b3b90	101	//*****PRZYPISANIE DO TABLICY LEWEJ LICZBY*******//
yruiewyrui3	0:5919ea7b3b90	102	for(int j=0; j<=2; j++){
yruiewyrui3	0:5919ea7b3b90	103	lewa[j]=bufor[j+2];
yruiewyrui3	0:5919ea7b3b90	104	}
yruiewyrui3	0:5919ea7b3b90	105	//*****PRZYPISANIE DO TABLICY PRAWEJ LICZBY*******//
yruiewyrui3	0:5919ea7b3b90	106	for(int k=0; k<=2; k++){
yruiewyrui3	0:5919ea7b3b90	107	prawa[k]=bufor[k+6];
yruiewyrui3	0:5919ea7b3b90	108	}
yruiewyrui3	0:5919ea7b3b90	109	//*****KONWERSJA CHAROW NA INTY*******//
yruiewyrui3	0:5919ea7b3b90	110	sscanf(lewa, "%3d", &l);
yruiewyrui3	0:5919ea7b3b90	111	sscanf(prawa, "%3d", &p);
yruiewyrui3	0:5919ea7b3b90	112	//******KOREKCJA*************//
yruiewyrui3	0:5919ea7b3b90	113	if(abs(l)>100){
yruiewyrui3	0:5919ea7b3b90	114	l=100;
yruiewyrui3	0:5919ea7b3b90	115	}
yruiewyrui3	0:5919ea7b3b90	116	if(abs(p)>100){
yruiewyrui3	0:5919ea7b3b90	117	p=100;
jguzik	3:1a391dd91a35	118	}
yruiewyrui3	0:5919ea7b3b90	119	//****KOREKCJA ZNAKU*********//
yruiewyrui3	0:5919ea7b3b90	120	if(bufor[1]=='-') l=-l;
jguzik	3:1a391dd91a35	121	if(bufor[5]=='-') p=-p;
jguzik	3:1a391dd91a35	122
jguzik	3:1a391dd91a35	123	}
jguzik	3:1a391dd91a35	124
jguzik	3:1a391dd91a35	125
jguzik	3:1a391dd91a35	126	void move_wheels()
jguzik	3:1a391dd91a35	127	{
yruiewyrui3	0:5919ea7b3b90	128	eng_left.move(l);
yruiewyrui3	0:5919ea7b3b90	129	eng_right.move(p);
yruiewyrui3	0:5919ea7b3b90	130	}
jguzik	3:1a391dd91a35	131
jguzik	3:1a391dd91a35	132	void actualize_servo_values()
jguzik	3:1a391dd91a35	133	{
yruiewyrui3	0:5919ea7b3b90	134	//*****PRZYPISANIE DO KAMERY POZIOM*******//
yruiewyrui3	0:5919ea7b3b90	135	for(int m=0; m<2; m++){
jguzik	3:1a391dd91a35	136	dodaj_poziom[m]=bufor[m+3];
yruiewyrui3	0:5919ea7b3b90	137	}
yruiewyrui3	0:5919ea7b3b90	138	//*****PRZYPISANIE DO KAMERY PION*******//
yruiewyrui3	0:5919ea7b3b90	139	for(int n=0; n<2; n++){
jguzik	3:1a391dd91a35	140	dodaj_pion[n]=bufor[n+7];
yruiewyrui3	0:5919ea7b3b90	141	}
yruiewyrui3	0:5919ea7b3b90	142	//*****KONWERSJA CHAROW NA INTY*******//
jguzik	3:1a391dd91a35	143	sscanf(dodaj_poziom, "%2d", &po);
jguzik	3:1a391dd91a35	144	sscanf(dodaj_pion, "%2d", &pi);
jguzik	3:1a391dd91a35	145	//******KOREKCJA*************//
yruiewyrui3	0:5919ea7b3b90	146	if(bufor[1]=='-') po=-po;
yruiewyrui3	0:5919ea7b3b90	147	if(bufor[5]=='-') pi=-pi;
jguzik	3:1a391dd91a35	148
jguzik	3:1a391dd91a35	149	akt_poziom = akt_poziom + po;
jguzik	3:1a391dd91a35	150	akt_pion = akt_pion + pi;
jguzik	3:1a391dd91a35	151
jguzik	3:1a391dd91a35	152	if(akt_poziom>90)
jguzik	3:1a391dd91a35	153	akt_poziom=90;
jguzik	3:1a391dd91a35	154	else if (akt_poziom<-90)
jguzik	3:1a391dd91a35	155	akt_poziom=-90;
jguzik	3:1a391dd91a35	156
jguzik	3:1a391dd91a35	157	if(akt_pion>90)
jguzik	3:1a391dd91a35	158	akt_pion=90;
jguzik	3:1a391dd91a35	159	else if (akt_pion<-90)
jguzik	3:1a391dd91a35	160	akt_poziom=-90;
yruiewyrui3	0:5919ea7b3b90	161	}
yruiewyrui3	0:5919ea7b3b90	162
jguzik	3:1a391dd91a35	163
jguzik	3:1a391dd91a35	164	void move_camera()
jguzik	3:1a391dd91a35	165	{
jguzik	3:1a391dd91a35	166	cam_poziom.position(akt_poziom);
jguzik	3:1a391dd91a35	167	cam_pion.position(akt_pion);
jguzik	3:1a391dd91a35	168	}
jguzik	3:1a391dd91a35	169
jguzik	3:1a391dd91a35	170
jguzik	3:1a391dd91a35	171
yruiewyrui3	0:5919ea7b3b90	172	int main(){
yruiewyrui3	0:5919ea7b3b90	173	int i = 0;
yruiewyrui3	0:5919ea7b3b90	174	cleanBuffer(bufor);
yruiewyrui3	0:5919ea7b3b90	175	stm.baud(115200);
yruiewyrui3	2:26a9f90142a0	176	t_sonar.start();
jguzik	3:1a391dd91a35	177	distance = 0;
jguzik	3:1a391dd91a35	178	move_camera();
jguzik	3:1a391dd91a35	179
jguzik	3:1a391dd91a35	180	while(true)
jguzik	3:1a391dd91a35	181	{
jguzik	3:1a391dd91a35	182
jguzik	3:1a391dd91a35	183	if(t.read_ms()>500){ //jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
yruiewyrui3	0:5919ea7b3b90	184	eng_left.move(0);
yruiewyrui3	0:5919ea7b3b90	185	eng_right.move(0);
yruiewyrui3	0:5919ea7b3b90	186	}
yruiewyrui3	0:5919ea7b3b90	187	if(t_sonar.read_ms()>1200){
yruiewyrui3	2:26a9f90142a0	188	distance = sensor.distance();
yruiewyrui3	0:5919ea7b3b90	189	t_sonar.stop();
yruiewyrui3	0:5919ea7b3b90	190	t_sonar.reset();
yruiewyrui3	0:5919ea7b3b90	191	t_sonar.start();
yruiewyrui3	0:5919ea7b3b90	192	}
jguzik	3:1a391dd91a35	193	if(bufor[9] == NULL)
jguzik	3:1a391dd91a35	194	{
yruiewyrui3	0:5919ea7b3b90	195	bufor[i] = stm.getc();
jguzik	3:1a391dd91a35	196	if(i==0)
jguzik	3:1a391dd91a35	197	{
yruiewyrui3	0:5919ea7b3b90	198	if(bufor[i]=='@') //zaczynamy zapelniac bufor jak dostaniemy @
yruiewyrui3	0:5919ea7b3b90	199	++i;
yruiewyrui3	0:5919ea7b3b90	200	}
jguzik	3:1a391dd91a35	201	else if(bufor[i]=='@')
jguzik	3:1a391dd91a35	202	{ // i != 0 //interpretujemy jakby potencjalny poczatek ramki
yruiewyrui3	0:5919ea7b3b90	203	cleanBuffer(bufor);
yruiewyrui3	0:5919ea7b3b90	204	bufor[0]='@';
yruiewyrui3	0:5919ea7b3b90	205	i=1; //bo zerowy znak '@' juz zczytal
jguzik	3:1a391dd91a35	206	}
yruiewyrui3	0:5919ea7b3b90	207	else i++;
jguzik	3:1a391dd91a35	208
jguzik	3:1a391dd91a35	209	continue;
yruiewyrui3	0:5919ea7b3b90	210	}
jguzik	3:1a391dd91a35	211	if(isCorrectPacket(bufor))
jguzik	3:1a391dd91a35	212	{
yruiewyrui3	0:5919ea7b3b90	213	t.stop();
yruiewyrui3	0:5919ea7b3b90	214	t.reset();
jguzik	3:1a391dd91a35	215	t.start();
yruiewyrui3	0:5919ea7b3b90	216	}
yruiewyrui3	0:5919ea7b3b90	217	if(bufor[9]=='$'){
jguzik	3:1a391dd91a35	218	actualize_speed();
jguzik	3:1a391dd91a35	219	if(distance>25){
jguzik	3:1a391dd91a35	220	move_wheels();
jguzik	3:1a391dd91a35	221	}
jguzik	3:1a391dd91a35	222	else if (distance<=25){
jguzik	3:1a391dd91a35	223	if(l<=0 && p<=0 \|\| l*p<0){
jguzik	3:1a391dd91a35	224	move_wheels();
jguzik	3:1a391dd91a35	225	}
jguzik	3:1a391dd91a35	226	else{
jguzik	3:1a391dd91a35	227	eng_left.move(0);
jguzik	3:1a391dd91a35	228	eng_right.move(0);
jguzik	3:1a391dd91a35	229	}
jguzik	3:1a391dd91a35	230	}
yruiewyrui3	0:5919ea7b3b90	231	}
yruiewyrui3	0:5919ea7b3b90	232	else if(bufor[9]=='&'){
jguzik	3:1a391dd91a35	233	actualize_servo_values();
jguzik	3:1a391dd91a35	234	move_camera();
yruiewyrui3	0:5919ea7b3b90	235	}
yruiewyrui3	0:5919ea7b3b90	236	i=0;
jguzik	3:1a391dd91a35	237	//send2rpi('s', distance);
yruiewyrui3	0:5919ea7b3b90	238	cleanBuffer(bufor);
jguzik	3:1a391dd91a35	239
yruiewyrui3	0:5919ea7b3b90	240	}
jguzik	3:1a391dd91a35	241	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	24 Jun 2016
Imports:	0
Forks:	0
Commits:	5
Dependents:	0
Dependencies:	3
Followers:	1

main.cpp@3:1a391dd91a35, 2016-06-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning