CZOSNEK_BY_COKIDS -

Users » jguzik » Code » CZOSNEK_BY_COKIDS

Justyna Guzik / Mbed 2 deprecated CZOSNEK_BY_COKIDS

Dependencies: czosnekengine_17_06 Servo_fullrange mbed

Fork of CZOSN_ostateczny_29_06 by Klaudia Różycka

main.cpp@5:87151d829108, 2016-06-29 (annotated)

Committer:: jguzik
Date:: Wed Jun 29 13:13:32 2016 +0000
Revision:: 5:87151d829108
Parent:: 4:5e3f03e75645

Who changed what in which revision?

User	Revision	Line number	New contents of line
yruiewyrui3	0:2e73d45d090e	1	#include "mbed.h"
yruiewyrui3	0:2e73d45d090e	2	#include "Engine.h"
yruiewyrui3	0:2e73d45d090e	3	#include "Servo.h"
jguzik	3:d78e7f6477d8	4	#include "hcsr04.h"
p683	4:5e3f03e75645	5
yruiewyrui3	0:2e73d45d090e	6	Serial stm(PA_2, PA_3);
p683	4:5e3f03e75645	7	Servo cam_horizontal(PB_6);
p683	4:5e3f03e75645	8	Servo cam_vertical(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);
jguzik	5:87151d829108	11	HCSR04 sensor(PB_9, PB_8, 11770);
yruiewyrui3	0:2e73d45d090e	12
yruiewyrui3	0:2e73d45d090e	13	const int BufferSize=10;
p683	4:5e3f03e75645	14	char buffer[BufferSize];
p683	4:5e3f03e75645	15	char right[3];
p683	4:5e3f03e75645	16	char left[3];
p683	4:5e3f03e75645	17	char add_horizontal[2];
p683	4:5e3f03e75645	18	char add_vertical[2];
jguzik	5:87151d829108	19	int act_vertical=-20, act_horizontal=-5;
p683	4:5e3f03e75645	20	int l=0, r=0, h=0, v=0;
jguzik	3:d78e7f6477d8	21	Timer t, t_sonar;
p683	4:5e3f03e75645	22	long distance;
p683	4:5e3f03e75645	23
p683	4:5e3f03e75645	24
p683	4:5e3f03e75645	25	void cleanBuffer()
p683	4:5e3f03e75645	26	{
p683	4:5e3f03e75645	27	for(int i=0; i<=BufferSize; i++)
p683	4:5e3f03e75645	28	buffer[i]=NULL;
yruiewyrui3	0:2e73d45d090e	29	}
yruiewyrui3	0:2e73d45d090e	30
p683	4:5e3f03e75645	31	char* intToChar(int value, char tab[3])
p683	4:5e3f03e75645	32	{
p683	4:5e3f03e75645	33	tab[0]= (char)(value/100 + 48);
p683	4:5e3f03e75645	34	tab[1]= (char)((value-(100*(value/100)))/10 + 48);
p683	4:5e3f03e75645	35	tab[2]= (char)(value-(100(value/100))-(10((value-(100*(value/100)))/10)) + 48);
p683	4:5e3f03e75645	36	return tab;
p683	4:5e3f03e75645	37	}
yruiewyrui3	0:2e73d45d090e	38
p683	4:5e3f03e75645	39	//************************OPIS FUNKCJI isCorrectPacket****************************************************//
p683	4:5e3f03e75645	40	// funkcja sprawdza czy ramka spelnia wymagania protokolu: @ZCCCZCCC$ lub &ZCCCZCCC$ gdzie Z={+/-} C={0,1,...9}
p683	4:5e3f03e75645	41
p683	4:5e3f03e75645	42	bool isCorrectPacket(){
p683	4:5e3f03e75645	43	if (buffer[0]!='@')
yruiewyrui3	0:2e73d45d090e	44	return false;
p683	4:5e3f03e75645	45
p683	4:5e3f03e75645	46	if((buffer[1] != '+') && (buffer[1] != '-'))
yruiewyrui3	0:2e73d45d090e	47	return false;
p683	4:5e3f03e75645	48
p683	4:5e3f03e75645	49	if((buffer[5] != '+') && (buffer[5] != '-'))
p683	4:5e3f03e75645	50	return false;
p683	4:5e3f03e75645	51
p683	4:5e3f03e75645	52	if((buffer[9]!='$')&&(buffer[9]!='&'))
p683	4:5e3f03e75645	53	return false;
p683	4:5e3f03e75645	54
yruiewyrui3	0:2e73d45d090e	55	for(int i=2; i< 5; i++){
p683	4:5e3f03e75645	56	if(((int)buffer[i]) < 48 \|\| ((int)buffer[i]) > 57)
p683	4:5e3f03e75645	57	return false;
p683	4:5e3f03e75645	58	if(((int)buffer[i+4]) < 48 \|\| ((int)buffer[i+4]) > 57)
p683	4:5e3f03e75645	59	return false;
jguzik	5:87151d829108	60	}
yruiewyrui3	0:2e73d45d090e	61	return true;
yruiewyrui3	0:2e73d45d090e	62	}
jguzik	3:d78e7f6477d8	63
p683	4:5e3f03e75645	64
p683	4:5e3f03e75645	65	void actualize_speed()
yruiewyrui3	0:2e73d45d090e	66	{
yruiewyrui3	0:2e73d45d090e	67	//*****PRZYPISANIE DO TABLICY LEWEJ LICZBY*******//
yruiewyrui3	0:2e73d45d090e	68	for(int j=0; j<=2; j++){
p683	4:5e3f03e75645	69	left[j]=buffer[j+2];
yruiewyrui3	0:2e73d45d090e	70	}
p683	4:5e3f03e75645	71	//*****PRZYPISANIE DO TABLICY PRAWEJ LICZBY******//
p683	4:5e3f03e75645	72	for(int k=0; k<=2; k++){
p683	4:5e3f03e75645	73	right[k]=buffer[k+6];
p683	4:5e3f03e75645	74	}
p683	4:5e3f03e75645	75	//*****KONWERSJA CHAROW NA INTY******************//
p683	4:5e3f03e75645	76	sscanf(left, "%3d", &l);
p683	4:5e3f03e75645	77	sscanf(right, "%3d", &r);
p683	4:5e3f03e75645	78	if(abs(l)>100)
yruiewyrui3	0:2e73d45d090e	79	l=100;
p683	4:5e3f03e75645	80
p683	4:5e3f03e75645	81	if(abs(r)>100)
p683	4:5e3f03e75645	82	r=100;
p683	4:5e3f03e75645	83
p683	4:5e3f03e75645	84	if(buffer[1]=='-') l=-l;
p683	4:5e3f03e75645	85	if(buffer[5]=='-') r=-r;
p683	4:5e3f03e75645	86	}
p683	4:5e3f03e75645	87
p683	4:5e3f03e75645	88	void move_wheels()
p683	4:5e3f03e75645	89	{
p683	4:5e3f03e75645	90	eng_left.move(l);
p683	4:5e3f03e75645	91	eng_right.move(r);
jguzik	3:d78e7f6477d8	92	}
jguzik	3:d78e7f6477d8	93
p683	4:5e3f03e75645	94	void actualize_servo_val()
jguzik	3:d78e7f6477d8	95	{
p683	4:5e3f03e75645	96	//*****PRZYPISANIE DO KAMERY POZIOM*******//
p683	4:5e3f03e75645	97	for(int m=0; m<2; m++){
p683	4:5e3f03e75645	98	add_horizontal[m]=buffer[m+3];
p683	4:5e3f03e75645	99	}
p683	4:5e3f03e75645	100	//*****PRZYPISANIE DO KAMERY PION********//
p683	4:5e3f03e75645	101	for(int n=0; n<2; n++){
p683	4:5e3f03e75645	102	add_vertical[n]=buffer[n+7];
p683	4:5e3f03e75645	103	}
p683	4:5e3f03e75645	104	//*****KONWERSJA CHAROW NA INTY**********//
p683	4:5e3f03e75645	105	sscanf(add_horizontal, "%2d", &h);
p683	4:5e3f03e75645	106	sscanf(add_vertical, "%2d", &v);
p683	4:5e3f03e75645	107
p683	4:5e3f03e75645	108	if(buffer[1]=='-') h=-h;
p683	4:5e3f03e75645	109	if(buffer[5]=='-') v=-v;
p683	4:5e3f03e75645	110
p683	4:5e3f03e75645	111	act_horizontal = act_horizontal + h;
p683	4:5e3f03e75645	112	act_vertical = act_vertical + v;
p683	4:5e3f03e75645	113
p683	4:5e3f03e75645	114	if(act_horizontal>90)
p683	4:5e3f03e75645	115	act_horizontal=90;
p683	4:5e3f03e75645	116	else if (act_horizontal<-90)
p683	4:5e3f03e75645	117	act_horizontal=-90;
p683	4:5e3f03e75645	118
p683	4:5e3f03e75645	119	if(act_vertical>90)
p683	4:5e3f03e75645	120	act_vertical=90;
p683	4:5e3f03e75645	121	else if (act_vertical<-90)
p683	4:5e3f03e75645	122	act_vertical=-90;
yruiewyrui3	0:2e73d45d090e	123	}
jguzik	3:d78e7f6477d8	124
yruiewyrui3	0:2e73d45d090e	125	void move_camera()
yruiewyrui3	0:2e73d45d090e	126	{
p683	4:5e3f03e75645	127	cam_horizontal.position(act_horizontal);
jguzik	5:87151d829108	128	cam_vertical.position(act_vertical);
yruiewyrui3	0:2e73d45d090e	129	}
yruiewyrui3	2:325a730cd438	130
p683	4:5e3f03e75645	131
yruiewyrui3	0:2e73d45d090e	132	int main(){
yruiewyrui3	0:2e73d45d090e	133	int i = 0;
yruiewyrui3	0:2e73d45d090e	134	stm.baud(115200);
jguzik	5:87151d829108	135	cleanBuffer();
jguzik	5:87151d829108	136	move_camera();
p683	4:5e3f03e75645	137	t_sonar.start();
jguzik	5:87151d829108	138
yruiewyrui3	0:2e73d45d090e	139	while(true){
p683	4:5e3f03e75645	140	if(t.read_ms()>500)//jesli przez ponad 200ms nie ma nowej ramki, zatrzymujemy robota
p683	4:5e3f03e75645	141	{
yruiewyrui3	0:2e73d45d090e	142	eng_left.move(0);
yruiewyrui3	0:2e73d45d090e	143	eng_right.move(0);
yruiewyrui3	0:2e73d45d090e	144	}
jguzik	5:87151d829108	145	if(t_sonar.read_ms()>1000)//dczyt odleglosci co 1,2s
p683	4:5e3f03e75645	146	{
p683	4:5e3f03e75645	147	distance = sensor.distance();
p683	4:5e3f03e75645	148	stm.printf("%d\n",distance);
p683	4:5e3f03e75645	149	/*intToChar(distance, tablicaf);
p683	4:5e3f03e75645	150	for (int f=0; f<3; f++)
p683	4:5e3f03e75645	151	{
p683	4:5e3f03e75645	152	stm.putc(tablicaf[f]);
p683	4:5e3f03e75645	153	}*/
jguzik	3:d78e7f6477d8	154	t_sonar.stop();
jguzik	3:d78e7f6477d8	155	t_sonar.reset();
p683	4:5e3f03e75645	156	t_sonar.start();
p683	4:5e3f03e75645	157	}
p683	4:5e3f03e75645	158	if(buffer[9] == NULL){
p683	4:5e3f03e75645	159	buffer[i] = stm.getc();
yruiewyrui3	2:325a730cd438	160	if(i==0){
jguzik	5:87151d829108	161	if(buffer[i]=='@')
p683	4:5e3f03e75645	162	++i;
yruiewyrui3	2:325a730cd438	163	}
jguzik	5:87151d829108	164	else if(buffer[i]=='@'){
p683	4:5e3f03e75645	165	cleanBuffer();
p683	4:5e3f03e75645	166	buffer[0]='@';
jguzik	5:87151d829108	167	i=1;
yruiewyrui3	2:325a730cd438	168	}
yruiewyrui3	2:325a730cd438	169	else i++;
p683	4:5e3f03e75645	170	continue;
p683	4:5e3f03e75645	171	}
p683	4:5e3f03e75645	172	if(isCorrectPacket()){
yruiewyrui3	0:2e73d45d090e	173	t.stop();
yruiewyrui3	0:2e73d45d090e	174	t.reset();
p683	4:5e3f03e75645	175	t.start();
yruiewyrui3	0:2e73d45d090e	176	}
p683	4:5e3f03e75645	177	if(buffer[9]=='$'){
p683	4:5e3f03e75645	178	actualize_speed();
p683	4:5e3f03e75645	179	if(distance>25)
jguzik	3:d78e7f6477d8	180	move_wheels();
jguzik	3:d78e7f6477d8	181	else if (distance<=25){
jguzik	5:87151d829108	182	if(l<0 && r<0 \|\| l*r<0){
jguzik	3:d78e7f6477d8	183	move_wheels();
jguzik	3:d78e7f6477d8	184	}
jguzik	3:d78e7f6477d8	185	else{
p683	4:5e3f03e75645	186	eng_left.move(0);
p683	4:5e3f03e75645	187	eng_right.move(0);
jguzik	3:d78e7f6477d8	188	}
jguzik	3:d78e7f6477d8	189	}
yruiewyrui3	0:2e73d45d090e	190	}
p683	4:5e3f03e75645	191	else if(buffer[9]=='&'){
p683	4:5e3f03e75645	192	actualize_servo_val();
p683	4:5e3f03e75645	193	move_camera();
yruiewyrui3	0:2e73d45d090e	194	}
yruiewyrui3	0:2e73d45d090e	195	i=0;
p683	4:5e3f03e75645	196	cleanBuffer();
p683	4:5e3f03e75645	197	}
p683	4:5e3f03e75645	198	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

main.cpp@5:87151d829108, 2016-06-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning