Ashok R
Published © GPL3+

Color Follow Robot with Pixy2 Cam

This robot follows the color with Pixy2 Camera.

IntermediateShowcase (no instructions)20 hours748
Color Follow Robot with Pixy2 Cam

Things used in this project

Hardware components

Pixy2Cam
×1
Rapid IoT Prototyping Kit
NXP Rapid IoT Prototyping Kit
×1
Motor Driver/Controller, H-Bridge
Motor Driver/Controller, H-Bridge
×1

Software apps and online services

MCUXpressoIDE

Hand tools and fabrication machines

Soldering Station Power Supply, For Weller WX Soldering System
Soldering Station Power Supply, For Weller WX Soldering System

Story

Read more

Schematics

Schematic

Code

Main

C/C++
/*
 * The Clear BSD License
 * Copyright (c) 2013 - 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted (subject to the limitations in the disclaimer below) provided
 *  that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <raspi.h>
#include "board.h"
#include "fsl_uart.h"
#include <math.h>
#include "pin_mux.h"
#include "clock_config.h"
#include "uart.h"
#include "backlight.h"
#include "emwin_support.h"
#include "GUI.h"
#include "project.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define BOARD_SW_IRQ PORTE_IRQn
#define BOARD_SW_IRQ_HANDLER PORTE_IRQHandler
#define APP_MAX_COUNT 5
#define APP_TIMER 10
#define APP_LOCK_TIMER 2500
/*******************************************************************************
 * Prototypes
 ******************************************************************************/

/*******************************************************************************
 * Variables
 ******************************************************************************/

uint8_t txbuff[] = "version\r\n";
uint8_t ifconfig[] = "ifconfig\r\n";
uint8_t AIData[4] = {0};
/* Whether the SW button is pressed */
volatile bool g_ButtonPress = false;
volatile uint8_t g_ButtonValue = 0;
volatile uint32_t g_cloud_counter=0;
volatile uint32_t g_lock_counter=0;
volatile uint8_t g_AppIndex = 0;
volatile uint8_t g_AppLocked = 0;
volatile uint8_t g_AppLockDisable = 0;
volatile uint8_t g_AppStatus[5] = {0,0,0,0,0};
volatile uint8_t g_AppValue[5] = {0,0,0,0,0};
volatile uint32_t frame_update=500u;
extern uint8_t masterRxData[128];
extern uint8_t RxRpiBuffer[8];
extern volatile uint8_t rxRpiBlockReceived;
extern volatile uint8_t rxRpiDataCounter;

uint32_t color_array[] = {GUI_WHITE,GUI_MAGENTA, GUI_BLUE, GUI_GREEN, GUI_YELLOW};
pixy_colormode_t *pixy_cmode=masterRxData;
volatile uint16_t dx=0;
volatile uint16_t dy=0;
volatile uint16_t pdx=0;
volatile uint16_t pdy=0;
volatile uint8_t track_index=0;
volatile int8_t track_value=0;
volatile int8_t track_last_value=0;
/* Functions required for emwin / Display */
uint32_t DSPI2_GetFreq(void)
{
    return CLOCK_GetBusClkFreq();
}


void Init_Display()
{
    Display_Connect(); /* triggers GUI_Init() */
    Backlight_SetLevel(BLIGHT_LEVEL_HIGH);

    GUI_SetBkColor(GUI_BLACK);
    GUI_SetColor(GUI_WHITE);
}



#define GUI_FONT_TITLE  GUI_Font8x18
#define GUI_FONT_NORMAL GUI_Font8x16

uint8_t Init_Backlight(void)
{
	backlight_fct_t BACKLIGHT_fct;

    BACKLIGHT_fct.connect_hw = Backlight_Connect;
    BACKLIGHT_fct.disconnect_hw = Backlight_Disconnect;
    BACKLIGHT_fct.set_level = Backlight_Set_Level;

    Backlight_Init_Driver(&BACKLIGHT_fct);

    if (Backlight_Init_Hw())
        return 1;

    return 0;
}


/*******************************************************************************
 * Code
 ******************************************************************************/
/*!
 * @brief Main function
 */
int main(void)
{


	BOARD_InitPins();
    BOARD_Boot_Clock_RUN();

    BOARD_INIT_GPIOS();
    //BOARD_Init_SPI_2();


    BOARD_Init_RGB_BLIGHT();
    PORT_IRQ_EnablePortAIrq();
    PORT_IRQ_EnablePortEIrq();

    //THR_UART_Init();

    //RPI_UART_Init();
    PIXY_Init();
    Init_Backlight();

    Init_Display();
    GUI_Clear();
    GUI_SetColor(GUI_BLACK);
    GUI_SetBkColor(GUI_WHITE);
    GUI_SetFont(&GUI_Font20B_1);
    GUI_SetPenSize(8);
	GUI_Clear();

	//GUI_BMP_Draw(bglogo_176x176, 0, 0);
    //App_WaitMsec(3000);
	GUI_BMP_Draw(bg_176x176, 0, 0);
    //App_WaitMsec(100);
	draw_apppage(g_AppIndex,0,0);

	//PIXY_Send_Data(PIXY_GET_VERSION);
	//App_WaitMsec(1);
	//PIXY_Get_Data();
	//	PIXY_SetLED(0,0,255);
	 GPIO_PinWrite(GPIOA, 11, 0);
	 GPIO_PinWrite(GPIOB, 8, 0);
	 GPIO_PinWrite(GPIOA, 13, 0);
	 GPIO_PinWrite(GPIOA, 14, 0);
	while(1){


		 if(g_ButtonPress){

			switch(g_ButtonValue){

			case 0:

				break;

			case 1:
				g_AppIndex++;
				if(g_AppIndex>1){
					g_AppIndex=0;
				}
				draw_apppage(g_AppIndex,1,0);
				break;

			case 2:
				frame_update=frame_update-100;
				if(frame_update<=100){
					frame_update=500;
				}
				break;

			}

			g_ButtonValue = 0;
			g_ButtonPress = false;

		   }

		 getCCBlocks();
		 App_WaitMsec(1);
		 PIXY_Get_Data();


		 if(g_AppIndex==0){

			 switch(pixy_cmode->sign){

			 case 1:
				GUI_SetColor(GUI_MAGENTA);
				GUI_FillRect(0, 144, 176, 176);
				dx= ((315 - pixy_cmode->xpos)*176)/315;
				GUI_SetColor(GUI_WHITE);
				GUI_FillCircle(dx, 160, 10);
				track_index=1;
				 break;
			 case 2:

				GUI_SetColor(GUI_BLUE);
				GUI_FillRect(0, 144, 176, 176);
				dx= ((315 - pixy_cmode->xpos)*176)/315;
				GUI_SetColor(GUI_WHITE);
				GUI_FillCircle(dx, 160, 10);
				track_index=2;

				 break;

			 case 3:
				GUI_SetColor(GUI_GREEN);
				GUI_FillRect(0, 144, 176, 176);
				dx= ((315 - pixy_cmode->xpos)*176)/315;
				GUI_SetColor(GUI_WHITE);
				GUI_FillCircle(dx, 160, 10);
				track_index=3;

				 break;
			 case 4:
				GUI_SetColor(GUI_YELLOW);
				GUI_FillRect(0, 144, 176, 176);
				dx= ((315 - pixy_cmode->xpos)*176)/315;
				GUI_SetColor(GUI_WHITE);
				GUI_FillCircle(dx, 160, 10);
				track_index=4;
				 break;

			 }
		 }
		 else{


			 if(pixy_cmode->sign==track_index){
			 dx= ((315 - pixy_cmode->xpos)*176)/315;
			 //dy= (pixy_cmode->ypos*176)/207;
			 if((abs(pdx-dx) >4)){

				 if(dx<56){

					GPIO_PinWrite(GPIOA, 13, 0);
					GPIO_PinWrite(GPIOA, 14, 1);
					track_value =-1*(dx/2);
					if(track_value<-17)track_value=-17;
					GUI_DispStringAt(" TRACK ", 54,144);
				 }
				 else if(dx>120){

					 GPIO_PinWrite(GPIOA, 11, 1);
					 GPIO_PinWrite(GPIOB, 8, 0);
					 track_value=1*((dx-120)/2);
					 if(track_value>17)track_value=17;
					 GUI_DispStringAt(" TRACK ", 54,144);
				 }
				 else{
					 track_value=0;
					GPIO_PinWrite(GPIOA, 11, 0);
					GPIO_PinWrite(GPIOB, 8, 0);
					GPIO_PinWrite(GPIOA, 13, 0);
					GPIO_PinWrite(GPIOA, 14, 0);
					GUI_DispStringAt("   IDLE   ", 54,144);
				 }

			 }
			 pdx=dx;
			 pdy=dy;
			 track_last_value = track_value;
			 }

		 }

		App_WaitMsec(frame_update);
	}


}

Credits

Ashok R

Ashok R

24 projects • 76 followers
Hobbyist/Engineer/Director/Animatior

Comments