ISR_Pixy-explorer - v1

Users » Bisca » Code » ISR_Pixy-explorer

Luis Osorio / Mbed 2 deprecated ISR_Pixy-explorer

Dependencies: ISR_Mini-explorer mbed ISR_Pixy_explorer

main.cpp@2:0dd04ad0ce1b, 2017-07-05 (annotated)

Committer:: Bisca
Date:: Wed Jul 05 09:20:36 2017 +0000
Revision:: 2:0dd04ad0ce1b
Parent:: 1:a88edf25d893

Who changed what in which revision?

User	Revision	Line number	New contents of line
Bisca	0:b63eda8e8952	1	#include "mbed.h"
Bisca	0:b63eda8e8952	2	#include "robot.h" // Inicializa o robô. Este include deverá ser usado em todos os main.cpp!
Bisca	0:b63eda8e8952	3	#include "Pixy.h"
Bisca	0:b63eda8e8952	4
Bisca	0:b63eda8e8952	5	Pixy pixy(Pixy::I2C,PTC11,PTC10);
Bisca	0:b63eda8e8952	6	Timer t;
Bisca	0:b63eda8e8952	7
Bisca	0:b63eda8e8952	8	int main() {
Bisca	0:b63eda8e8952	9	initRobot();
Bisca	0:b63eda8e8952	10	pixy.setAddress(0x21); //Camera Address 0x10
Bisca	0:b63eda8e8952	11
Bisca	0:b63eda8e8952	12
Bisca	0:b63eda8e8952	13	int Speed = 0;
Bisca	0:b63eda8e8952	14	int aux_Speed = 0;
Bisca	0:b63eda8e8952	15	int Coef = 0;
Bisca	0:b63eda8e8952	16	int aux_Coef = 0;
Bisca	0:b63eda8e8952	17	int left_speed = 0;
Bisca	0:b63eda8e8952	18	int left_Dir = 1;
Bisca	0:b63eda8e8952	19	int right_speed = 0;
Bisca	0:b63eda8e8952	20	int right_Dir = 1;
Bisca	0:b63eda8e8952	21
Bisca	1:a88edf25d893	22	int period = 20; //each loop takes 20ms
Bisca	0:b63eda8e8952	23	//////////width PI controller
Bisca	1:a88edf25d893	24	int w_setpoint = 150;
Bisca	0:b63eda8e8952	25	float w_Kp = 15;
Bisca	1:a88edf25d893	26	float w_Ki = 0.05;
Bisca	0:b63eda8e8952	27	int w_e = 0;
Bisca	0:b63eda8e8952	28	int w_e2 = 0;
Bisca	1:a88edf25d893	29	int w_e_threshold = 25;
Bisca	1:a88edf25d893	30	int max_speed = 1000;
Bisca	0:b63eda8e8952	31
Bisca	0:b63eda8e8952	32	//////////x PI controller
Bisca	0:b63eda8e8952	33	int x_setpoint = 160;
Bisca	1:a88edf25d893	34	float x_Kp = 3;
Bisca	1:a88edf25d893	35	float x_Ki = 0.02;
Bisca	0:b63eda8e8952	36	int x_e = 0;
Bisca	0:b63eda8e8952	37	int x_e2 = 0;
Bisca	1:a88edf25d893	38	int x_e_threshold = 30;
Bisca	1:a88edf25d893	39	int max_speed_rotation = 2000;
Bisca	1:a88edf25d893	40
Bisca	1:a88edf25d893	41	//////////servo PI controller
Bisca	1:a88edf25d893	42	int y_control=0;
Bisca	1:a88edf25d893	43	int y_setpoint = 100;
Bisca	1:a88edf25d893	44	float y_Kp = 2;
Bisca	1:a88edf25d893	45	float y_Ki = 0.02;
Bisca	1:a88edf25d893	46	int y_e = 0;
Bisca	1:a88edf25d893	47	int y_e2 = 0;
Bisca	1:a88edf25d893	48	int y_e_threshold = 10;
Bisca	1:a88edf25d893	49	int y_max = 2000;
Bisca	1:a88edf25d893	50
Bisca	0:b63eda8e8952	51
Bisca	0:b63eda8e8952	52	q_led_red_fro = 1; //Led Red Front
Bisca	0:b63eda8e8952	53
Bisca	0:b63eda8e8952	54
Bisca	1:a88edf25d893	55	uint16_t Block_width = 0, Block_x = 0, Block_y = 0;
Bisca	0:b63eda8e8952	56	uint16_t blocks;
Bisca	0:b63eda8e8952	57
Bisca	0:b63eda8e8952	58	t.start();
Bisca	0:b63eda8e8952	59	while(1) {
Bisca	0:b63eda8e8952	60	if(q_led_red_fro == 1){
Bisca	0:b63eda8e8952	61	q_led_red_fro = 0;
Bisca	0:b63eda8e8952	62	}else{
Bisca	0:b63eda8e8952	63	q_led_red_fro = 1;
Bisca	0:b63eda8e8952	64	}
Bisca	0:b63eda8e8952	65
Bisca	0:b63eda8e8952	66	blocks = 0;
Bisca	0:b63eda8e8952	67	blocks = (uint16_t) pixy.getBlocks();
Bisca	0:b63eda8e8952	68
Bisca	0:b63eda8e8952	69	if (blocks > 0){
Bisca	0:b63eda8e8952	70	Block_width = pixy.blocks[0].width;
Bisca	0:b63eda8e8952	71	Block_x = pixy.blocks[0].x;
Bisca	1:a88edf25d893	72	Block_y = pixy.blocks[0].y;
Bisca	0:b63eda8e8952	73
Bisca	1:a88edf25d893	74	//Distance/Speed Control
Bisca	1:a88edf25d893	75	w_e2 = (w_setpoint - Block_width) - w_e;
Bisca	1:a88edf25d893	76	w_e = w_setpoint - Block_width;
Bisca	1:a88edf25d893	77	Speed = Speed+ int(w_Kpw_e2) + int ((w_Kiw_e*period));
Bisca	1:a88edf25d893	78	aux_Speed = Speed;
Bisca	1:a88edf25d893	79	if (abs(Speed) >max_speed)
Bisca	1:a88edf25d893	80	{
Bisca	1:a88edf25d893	81	Speed = (int) max_speed *Speed/abs(Speed) ;
Bisca	1:a88edf25d893	82	}else if (abs(w_e) <w_e_threshold)
Bisca	1:a88edf25d893	83	{
Bisca	1:a88edf25d893	84	Speed = 0;
Bisca	1:a88edf25d893	85	}
Bisca	1:a88edf25d893	86
Bisca	1:a88edf25d893	87	//x Control
Bisca	1:a88edf25d893	88	x_e2 = (x_setpoint - Block_x) - x_e;
Bisca	1:a88edf25d893	89	x_e = x_setpoint - Block_x;
Bisca	1:a88edf25d893	90	Coef = Coef+ int(x_Kpx_e2) + int ((x_Kix_e*period));
Bisca	1:a88edf25d893	91	aux_Coef = Coef;
Bisca	1:a88edf25d893	92	if (abs(Coef) >max_speed_rotation)
Bisca	1:a88edf25d893	93	{
Bisca	1:a88edf25d893	94	Coef =(int) max_speed_rotation*Coef/abs(Coef);
Bisca	1:a88edf25d893	95	}
Bisca	1:a88edf25d893	96	else if (abs(x_e) < x_e_threshold )
Bisca	1:a88edf25d893	97	{
Bisca	1:a88edf25d893	98	Coef = 0;
Bisca	1:a88edf25d893	99	}
Bisca	1:a88edf25d893	100
Bisca	1:a88edf25d893	101	left_speed = Speed - Coef;
Bisca	1:a88edf25d893	102	if (abs(left_speed) >max_speed)
Bisca	1:a88edf25d893	103	{
Bisca	1:a88edf25d893	104	left_speed =(int) max_speed*left_speed/abs(left_speed);
Bisca	1:a88edf25d893	105	}
Bisca	1:a88edf25d893	106	if (left_speed >0)
Bisca	1:a88edf25d893	107	{
Bisca	1:a88edf25d893	108	left_Dir=1;
Bisca	1:a88edf25d893	109	left_speed = abs(left_speed);
Bisca	1:a88edf25d893	110	}else{
Bisca	1:a88edf25d893	111	left_Dir=0;
Bisca	1:a88edf25d893	112	left_speed = abs(left_speed);
Bisca	1:a88edf25d893	113	}
Bisca	1:a88edf25d893	114
Bisca	1:a88edf25d893	115	right_speed = Speed + Coef;
Bisca	1:a88edf25d893	116	if (abs(right_speed) >max_speed)
Bisca	1:a88edf25d893	117	{
Bisca	1:a88edf25d893	118	right_speed =(int) max_speed*right_speed/abs(right_speed);
Bisca	1:a88edf25d893	119	}
Bisca	1:a88edf25d893	120	if (right_speed >0)
Bisca	1:a88edf25d893	121	{
Bisca	1:a88edf25d893	122	right_Dir=1;
Bisca	1:a88edf25d893	123	right_speed = abs(right_speed);
Bisca	1:a88edf25d893	124	}else{
Bisca	1:a88edf25d893	125	right_Dir=0;
Bisca	1:a88edf25d893	126	right_speed = abs(right_speed);
Bisca	1:a88edf25d893	127	}
Bisca	1:a88edf25d893	128
Bisca	1:a88edf25d893	129
Bisca	1:a88edf25d893	130	//y Control
Bisca	1:a88edf25d893	131	y_e2 = (y_setpoint - Block_y) - y_e;
Bisca	1:a88edf25d893	132	y_e = y_setpoint - Block_y;
Bisca	1:a88edf25d893	133	y_control = y_control+ int(y_Kpy_e2) + int ((y_Kiy_e*period));
Bisca	1:a88edf25d893	134	if (abs(y_control) >y_max)
Bisca	1:a88edf25d893	135	{
Bisca	1:a88edf25d893	136	y_control =(int) y_max*y_control/abs(y_control);
Bisca	1:a88edf25d893	137	}
Bisca	1:a88edf25d893	138
Bisca	1:a88edf25d893	139	pixy.setServos(0,500+y_control);
Bisca	1:a88edf25d893	140
Bisca	0:b63eda8e8952	141
Bisca	1:a88edf25d893	142	}else{ //If no block received
Bisca	1:a88edf25d893	143	//Reset all linear speed variables
Bisca	1:a88edf25d893	144	w_e2 = 0;
Bisca	1:a88edf25d893	145	w_e = 0;
Bisca	1:a88edf25d893	146	Speed = 0;
Bisca	1:a88edf25d893	147	aux_Speed = 0;
Bisca	1:a88edf25d893	148	//Reset all angular speed variables
Bisca	1:a88edf25d893	149	x_e2 = 0;
Bisca	1:a88edf25d893	150	x_e = 0;
Bisca	1:a88edf25d893	151	Coef = 0;
Bisca	1:a88edf25d893	152	aux_Coef = 0;
Bisca	1:a88edf25d893	153	//Reset all servo speed variables
Bisca	1:a88edf25d893	154	y_e2 = 0;
Bisca	1:a88edf25d893	155	y_e = 0;
Bisca	1:a88edf25d893	156
Bisca	1:a88edf25d893	157
Bisca	1:a88edf25d893	158
Bisca	0:b63eda8e8952	159	left_Dir=0;
Bisca	1:a88edf25d893	160	left_speed = max_speed;
Bisca	1:a88edf25d893	161	right_Dir =1;
Bisca	1:a88edf25d893	162	right_speed = max_speed;
Bisca	0:b63eda8e8952	163	}
Bisca	0:b63eda8e8952	164
Bisca	0:b63eda8e8952	165	leftMotor(left_Dir,left_speed);
Bisca	1:a88edf25d893	166	rightMotor(right_Dir,right_speed);
Bisca	1:a88edf25d893	167
Bisca	0:b63eda8e8952	168	while (t.read_ms () < period )
Bisca	0:b63eda8e8952	169	{
Bisca	0:b63eda8e8952	170	//waste time
Bisca	0:b63eda8e8952	171	}
Bisca	0:b63eda8e8952	172	//pc.printf("Speed=%d , width=%d , w_e2=%d, w_e=%d \n",Speed , Block_width , w_e2 , w_e );
Bisca	0:b63eda8e8952	173	//pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d \n",Coef, Block_x , x_e2 , x_e );
Bisca	0:b63eda8e8952	174	pc.printf("Coef=%d , x=%d, x_e2=%d, x_e=%d :: Speed=%d ,leftMotor=%d , rightMotor=%d \n",Coef, Block_x , x_e2 , x_e , Speed , left_speed , right_speed);
Bisca	0:b63eda8e8952	175
Bisca	0:b63eda8e8952	176
Bisca	0:b63eda8e8952	177	t.reset();
Bisca	0:b63eda8e8952	178	}
Bisca	0:b63eda8e8952	179	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	29 Mar 2017
Imports:	11
Forks:	0
Commits:	3
Dependents:	0
Dependencies:	3
Followers:	1

main.cpp@2:0dd04ad0ce1b, 2017-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning