Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
 |
| × | 2 | |||
 |
| × | 1 | |||
 |
| × | 1 | |||
 |
| × | 2 | |||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
 |
| |||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
For this project, I wanted to create a little robot friend as a tool for expanding upon several skills I have learned throughout the Mechatronics course I have been taking this past semester. Specifically, I wanted to incorporate image processing into my project which I accomplishing using a camera, Orange Pi and blob searching. In addition to this, I also decided to have my robot play different songs commanded by playing different frequencies from a tone generator.
To efficiently complete my goals for this project, I decided to utilize Texas Instruments' F28379D Launchpad board along with TI's IDE Code Composer Studio which allows for real time debugging using expressions which was an incredibly useful tool for me throughout the entirety of this project as it allowed me to test each new addition to my project without needing to reupload code to the microcontroller to make small changes.
Project Overview Video:https://drive.google.com/file/d/1aMsTTLEB7sey428hgXGUNu4AoKYspoJ3/view?usp=sharing
I.Implementing Blob Search on Camera:
For the first portion of this project, I wanted to implement image processing and blob search in order to inform my drive controller how to follow me around. To accomplish this following behavior, I selected a pink slip of paper and decided to utilize blob search detecting to make my robot aware of where I was within its range of vision by identifying me as the pink slip.
- Image Processing with Orange Pi
In order to handle real time image processing from the camera, I decided to use an Orange Pi as an inexpensive alternative to a Raspberry Pi.
Raspberry Pi
- HSV Tuning for Blob Search
Next, I plugged in a camera and uploaded code to the Orange Pi in order to let me take pictures. I took a few pictures of the pink slip in different light settings and then used these images in a MATLAB tool provided by my professor in order to determine good ranges for HSV. Finally, these HSV values were plugged into a blob search program to filter out all pixels except those of my pink slip from the robots vision.
MATLAB HSV Tuning for Used for Color Thresholding
After filtering the image, the blob search algorithm would find all non-filtered pixels and interpret clumps of unfiltered pixels as centroids. From the largest centroid, its center position in terms of rows and columns was calculated along with the diameter of the centroid. Using these three values, the position of the pink slip as well as a rough estimate of its distance can be calculated.
- UART Communication Between Orange Pi and F28379D Launchpad
Finally for this portion of the project, I needed the Orange Pi to actually communicate with the F28379D Launchpad. To accomplish this, I had the Orange Pi transmit the centroid row, column, and diameter data over a serial port using UART communication. Through this method, the F28379D Launchpad could then retrieve data from the Orange Pi by reading the characters sent over UART.
Function on F28379D Launchpad for Reading UART Data Sent from Orange Pi
II.Adding Additional Hardwareto assist Drive Controller:
From part 1, the robot knows where it is supposed to go; however, it has no way of getting there or avoiding obstacles. To add this functionality, I added two wheeled motors to the robot in addition to a couple limit switches and an ultrasonic sensor to assist with obstacle avoidance.
- Reading Motor Angles Using Quadrature Encoders
The mounts for the motors were modeled in CAD and 3D printed. After assembling the mounts and motors to the bottom of the launchpad breakout board, the encoders were added to the motors in order to retrieve positional data about the motors to inform the controller in future steps.
Generic Encoder
The motors I added utilize quadrature encoders which utilize two beams 90 degrees out of phase in order to allow for angular position and speed determination as well as direction.
Benefit of Quadrature (2-phase) Encoder for Direction Knowledge
To convert data from the encoders to meaningful angular data, the distance between cutouts were converted to radians. Then this data was retrieved by the F28379D Launchpad using functions I wrote:
Encoder Reading and Conversion to Radians
- Implementing Limit Switches
Next, I added limit switches in front of each wheel in order to alert my robot if it impacts an obstacle.
Generic Limit Switch
Code for Reading GPIO Pins for Input
- Implementing Ultrasonic Sensor
Finally, I added an ultrasonic sensor so that my robot was able to do some planning about its environment without needing to bump into obstacles. This sensor sends out a signal and measures the time it takes for the signal to return to the sensor after bouncing off the environment. This information then gets converted into an estimated distance.
HC-SR04 Ultrasonic Sensor
For an in-depth tutorial about using this sensor, I suggest using the HC-SRO4 tutorial found on instructables' website:
https://www.instructables.com/Simple-Arduino-and-HC-SR04-Example/
Video of Initial Obstacle Avoidance Implementation:
https://drive.google.com/file/d/1aFjk6UuIGNXzT2MhwEjIJccKjYjdLtZJ/view?usp=sharing
III.Developing a Drive Controller:
Finally its time to actually drive the robot. Using information of the Orange Pi, limit switches, ultrasonic sensor, and encoders, I was able to design a Proportional-Integral controller using the following block diagram:
Coupled PI Controller
The turn command is determined from the Orange Pi's centroid data. Specifically, I used the centroid's column data and set the controller to reduce error between the center of the entire image and center of the centroid.
The reference velocity for both motors was also altered by my controller by scaling it by the robot's distance from the pink slip using the centroid's diameter. I felt it was good to have the robot follow slowly when close to me and run towards me when I was far away so I designed my controller with an inverse relationship between the centroid's diameter and reference speed.
Coupled PI Controller Implemented in Code
Videos of Robot Following BehaviorWorking:
https://drive.google.com/file/d/1aDTGMrc-h1IEkQjHYVdGkX9pZpP92cWT/view?usp=sharing
https://drive.google.com/file/d/1a8ONbIVv-uabojeBSi92GwO9rn_G3Gf8/view?usp=sharing
IV.Adding Music:
Now that the robot is driving around properly and following me around, it was time to add more fun, cosmetic elements to the project. For a while, I have been wanting to add music to the project which I could trigger to play by whistling a specific tone.
The first step was to add a microphone and buzzer to the breakout board in order to hear my whistles and respond with a specific song.
Generic Buzzer
Adafruit Microphone
Next, I needed to add sheet music to my robot so it knew what to play. As of now, I have added three songs all from Star Wars: the Main Theme, Cantina Song, and Imperial March.
Cantina Band Song:
https://github.com/robsoncouto/arduino-songs/blob/master/cantinaband/cantinaband.ino
Imperial March:
https://github.com/robsoncouto/arduino-songs/blob/master/imperialmarch/imperialmarch.ino
Main Theme:
https://github.com/robsoncouto/arduino-songs/tree/master/starwars
Example of Cantina Band Sheet Music Written in Code
After finding Arduino sheet music for each of these songs, I converted each manually into a format that I could play using a timer interrupt and a PWM. Using this method, I devoted a timer interrupt to handling the tempo of the song; the tempo would determine how long each note in the song array was played for. The value in the song array corresponds to the frequency of the note which gets assigned to the period of a PWM connected to the buzzer allowing the buzzer to play a wide variety of notes.
Timer Interrupt Which Assigns Period of PWM to Match Frequency of Each Note's Tone
V.Adding FFT Processing of Microphone:
As a final step to this project, I incorporated the Fast Fourier Transform (FFT) algorithm in order to precisely decipher information from the microphone in order to command the robot to play specific songs.
The fast Fourier transform is a computationally efficient method of computing fourier transform that makes it practical to implement on real time systems like the one found in this project. Specifically within this project, the fourier transform of the signal produced by the microphone is calculated in order to produce a low signal except for specific frequencies where the signal spikes making it very easy to decipher commands from.
Resources Regarding Fast Fourier Transform:
https://en.wikipedia.org/wiki/Fast_Fourier_transform
Essentially, this allows the robot to distinguish between noise and specific tones produces by a whistle. Using this method, it becomes very easy to use my whistles as a signal to the robot in order to command it to play specific songs.
In order to practically implement this algorithm in code, the microphone firsts needed to be sampled using an ADC. These raw values are scaled intro voltage values and then appended into a 1024 element array. When this array gets processed, we don't want to lose readings from the microphone which is why a secondary array is used to store values while the first array is being processed.
Microphone ADC Readings Converted to Voltages and Appended to an Array Awaiting FFT Processing
Processing of this array can sometimes take a long time which is why FFT processing is moved into a low priority while loop located in the main function.
FFT Processing in Main While Loop
https://drive.google.com/file/d/1aIEiDKi2-9SHEqLHk6xNWh0sb6jRpSFS/view?usp=sharing
https://drive.google.com/file/d/1aJawHGvm3l1rd_psFPx718uLBfi5rR6J/view?usp=sharing
//#############################################################################
// FILE: finalproject_main.c
//
// TITLE: Robot Pet Main File
//#############################################################################
// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "f28379dSerial.h"
#include "LEDPatterns.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
//*****************************************************************************
// the defines for FFT
//*****************************************************************************
#define RFFT_STAGES 10
#define RFFT_SIZE (1 << RFFT_STAGES)
//*****************************************************************************
// the globals
//*****************************************************************************
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(pwrSpec, "FFT_buffer_2")
#endif
float pwrSpec[(RFFT_SIZE/2)+1];
float maxpwr = 0;
int16_t maxpwrindex = 0;
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(test_output, "FFT_buffer_2")
#endif
float test_output[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(ping_input, "FFT_buffer_1")
#endif
float ping_input[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_1")
#else
#pragma DATA_SECTION(pong_input, "FFT_buffer_1")
#endif
float pong_input[RFFT_SIZE];
#ifdef __cplusplus
#pragma DATA_SECTION("FFT_buffer_2")
#else
#pragma DATA_SECTION(RFFTF32Coef,"FFT_buffer_2")
#endif //__cplusplus
//! \brief Twiddle Factors
//!
float RFFTF32Coef[RFFT_SIZE];
//! \brief Object of the structure RFFT_F32_STRUCT
//!
RFFT_F32_STRUCT rfft;
//! \brief Handle to the RFFT_F32_STRUCT object
//!
RFFT_F32_STRUCT_Handle hnd_rfft = &rfft;
int ping = 0;
int pong = 0;
int16_t sample_cnt = 0;
int pingpong = 0;
//END OF FFT section
#define TIMEBASE 0.005 // 1.0/200
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void ecap1_isr(void);
//Microphone ADC Interrupt
__interrupt void ADCB_ISR (void);
void serialRXA(serial_t *s, char data);
void serialRXD(serial_t *s, char data);
int16_t int16_tReadSwitches(void);
void InitECapture(void);
void hc_sr04_trigger(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
uint32_t numRXA = 0;
uint32_t numRXD = 0;
uint16_t UARTPrint = 0;
uint16_t SendToOrange = 0;
uint16_t LEDdisplaynum = 0;
#define ORANGEPIBUFFSIZE 128
char RXBuff[ORANGEPIBUFFSIZE];
//Blob Search
int16_t recordData = 0;
int16_t centroid_row = 0;
int16_t centroid_col = 0;
int16_t centroid_diameter = 0;
//Button Count / Limit Switch Identification
int16_t button_count = 0;
//Ultrasonic Sensor
// VCC - +5V
// GND - GND
// TRIG - GPIO0
// ECHO - GPIO19
int32_t echo_count = 0;
float echo_duration = 0;
float echo_distance = 100;
uint16_t trigger_count = 0;
uint16_t trigger_state = 0;
int16_t order = 32;
float yk_uss = 0;
float xk_uss[31] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
//Buzzer Song
uint16_t song_index = 1000;
uint32_t tempo = 175000;
//ADCB / MIC setup
int16_t mic = 0;
float scaled_voltage_ADCIND4 = 0.0;
//State Machine Variable Changed by FFT
int16_t state = 0;
//Lab 6 Three-Wheel Driving Control
float LeftWheel = 0.0;
float RightWheel = 0.0;
float LeftWheelFeet = 0.0;
float RightWheelFeet = 0.0;
float uLeft = 0;
float uRight = 0;
float Old_RightWheelFeet = 0.0;
float Old_LeftWheelFeet = 0.0;
float Raw_Left_Vel = 0.0;
float Raw_Right_Vel = 0.0;
float Kp = 3.0;
float Ki = 5.0;
float Kp_turn = 3.0;
float vel_k_L = 0.0;
float vel_k_R = 0.0;
float error_L = 0.0;
float error_L_prev = 0.0;
float error_R = 0.0;
float error_R_prev = 0.0;
float Ik_L = 0.0;
float Ik_R = 0.0;
float uk_L = 0.0;
float uk_R = 0.0;
float Ik_L_prev = 0.0;
float Ik_R_prev = 0.0;
float temp_L = 0.0;
float temp_R = 0.0;
float turn = 0.0;
float error_turn = 0.0;
int16_t hardstop = 0;
float radius = 0.10625; //[ft]
float wheel_width = 0.604; //[ft]
float left_theta = 0.0;
float right_theta = 0.0;
float Raw_Left_Omega = 0.0;
float old_left_theta = 0.0;
float Raw_Right_Omega = 0.0;
float old_Right_theta = 0.0;
float pose_angle = 0.0;
float theta_avg = 0.0;
float omega_avg = 0.0;
float x_R_dot = 0.0;
float y_R_dot = 0.0;
float x_R = 0.0;
float y_R = 0.0;
float x_R_dot_prev = 0.0;
float y_R_dot_prev = 0.0;
float x_R_prev = 0.0;
float y_R_prev = 0.0;
int loop = 0;
int16_t uss_count = 0;
int16_t mic_count = 0;
int temp_state = 0;
float temp_pose_angle = 0;
int start_flag = 0;
float Vel_ref = 0.0;
int note = 0;
float Kp_centroid = 0.025;
int16_t centroid_count = 0;
int16_t temp_centroid_diam = 0;
int16_t backUp_count = 0;
float angle = 3;
int16_t backUp_dis = 250;
int16_t centroid_max = 250;
int16_t song_lockout = 250;
int16_t song_max = 500;
uint32_t tempo1 = 62500;
uint32_t tempo2 = 125000;
uint32_t tempo3 = 125000;
//This function sets DACA to the voltage between 0V and 3V passed to this function.
//If outside 0V to 3V the output is saturated at 0V to 3V
//Example code
//float myu = 2.25;
//setDACA(myu); // DACA will now output 2.25 Volts
void setDACA(float dacouta0) {
if (dacouta0 > 3.0) dacouta0 = 3.0;
if (dacouta0 < 0.0) dacouta0 = 0.0;
DacaRegs.DACVALS.bit.DACVALS = 4095*(dacouta0/3.0); // perform scaling of 0-3 to 0-4095
}
void setDACB(float dacouta1) {
if (dacouta1 > 3.0) dacouta1 = 3.0;
if (dacouta1 < 0.0) dacouta1 = 0.0;
DacbRegs.DACVALS.bit.DACVALS = 4095*(dacouta1/3.0); // perform scaling of 0-3 to 0-4095
}
void main(void)
{
// PLL, WatchDog, enable Peripheral Clocks
// This example function is found in the F2837xD_SysCtrl.c file.
InitSysCtrl();
InitGpio();
//Ultrasonic Sensor
// Trigger pin for HC-SR04
GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO0 = 1;
// Echo pin for HC-SR04
EALLOW;
InputXbarRegs.INPUT7SELECT = 19; // Set eCAP1 source to GPIO-pin
EDIS;
GPIO_SetupPinOptions(19, GPIO_INPUT, GPIO_ASYNC);
InitECapture();
//---------------------------------------------------------
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
// Blue LED on LaunchPad
GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO31 = 1;
// Red LED on LaunchPad
GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO34 = 1;
// LED1 and PWM Pin
GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;
// LED2
GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;
// LED3
GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;
// LED4
GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;
// LED5
GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;
// LED6
GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;
// LED7
GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;
// LED8
GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;
// LED9
GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;
// LED10
GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;
// LED11
GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;
// LED12
GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;
// LED13
GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;
// LED14
GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;
// LED15
GPIO_SetupPinMux(159, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(159, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;
// LED16
GPIO_SetupPinMux(160, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(160, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;
// //WIZNET Reset
// GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
// GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
// GpioDataRegs.GPASET.bit.GPIO0 = 1;
//ESP8266 Reset
GPIO_SetupPinMux(1, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(1, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO1 = 1;
// //SPIRAM CS Chip Select
// GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
// GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
// GpioDataRegs.GPASET.bit.GPIO19 = 1;
//DRV8874 #1 DIR Direction
GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO29 = 1;
//DRV8874 #2 DIR Direction
GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO32 = 1;
//DAN28027 CS Chip Select
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO9 = 1;
//MPU9250 CS Chip Select
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
//WIZNET CS Chip Select
GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPDSET.bit.GPIO125 = 1;
//PushButton 1
GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(4, GPIO_INPUT, GPIO_PULLUP);
//PushButton 2
GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(5, GPIO_INPUT, GPIO_PULLUP);
//PushButton 3
GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(6, GPIO_INPUT, GPIO_PULLUP);
//PushButton 4
GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(7, GPIO_INPUT, GPIO_PULLUP);
//Joy Stick Pushbutton
GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);
//Set GPIO52 (usually unsed) for Exercise 4 to measure pulse width of ADCB interrupt
//GPIO52
GPIO_SetupPinMux(52, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
// Clear all interrupts and initialize PIE vector table:
// Disable CPU interrupts
DINT;
// Initialize the PIE control registers to their default state.
// The default state is all PIE interrupts disabled and flags
// are cleared.
// This function is found in the F2837xD_PieCtrl.c file.
InitPieCtrl();
// Disable CPU interrupts and clear all CPU interrupt flags:
IER = 0x0000;
IFR = 0x0000;
// Initialize the PIE vector table with pointers to the shell Interrupt
// Service Routines (ISR).
// This will populate the entire table, even if the interrupt
// is not used in this example. This is useful for debug purposes.
// The shell ISR routines are found in F2837xD_DefaultIsr.c.
// This function is found in F2837xD_PieVect.c.
InitPieVectTable();
// Interrupts that are used in this example are re-mapped to
// ISR functions found within this project
EALLOW; // This is needed to write to EALLOW protected registers
PieVectTable.TIMER0_INT = &cpu_timer0_isr;
PieVectTable.TIMER1_INT = &cpu_timer1_isr;
PieVectTable.TIMER2_INT = &cpu_timer2_isr;
PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
PieVectTable.ECAP1_INT = &ecap1_isr;
PieVectTable.ADCB1_INT = &ADCB_ISR;
EDIS; // This is needed to disable write to EALLOW protected registers
// Initialize the CpuTimers Device Peripheral. This function can be
// found in F2837xD_CpuTimers.c
InitCpuTimers();
// Configure CPU-Timer 0, 1, and 2 to interrupt every second:
// 200MHz CPU Freq, 1 second Period (in uSeconds)
//Timer 0: 1 ms code for UltraSonicSensor & Limit Switches & Mic
ConfigCpuTimer(&CpuTimer0, 200, 1000);
//Timer 1 Plays Songs from Buzzer
ConfigCpuTimer(&CpuTimer1, 200, tempo);
//Timer 2 Driving Control = State Machine
ConfigCpuTimer(&CpuTimer2, 200, 4000);
// Enable CpuTimer Interrupt bit TIE
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serial(&SerialA,115200,serialRXA);
// init_serial(&SerialC,115200,serialRXC);
init_serial(&SerialD,1500000,serialRXD);
init_eQEPs();
//////////////////////////////////////////////////////////////////////////////////////
//EPWM2A: Wheel Motors
//TBCTL
EPwm2Regs.TBCTL.bit.CTRMODE = 0;
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm2Regs.TBCTL.bit.PHSEN = 0;
EPwm2Regs.TBCTL.bit.CLKDIV = 0;
//TBCTR
EPwm2Regs.TBCTR = 0;
//TBPRD
EPwm2Regs.TBPRD = 2500;
//CMPA (records duty cycle of right motor)
EPwm2Regs.CMPA.bit.CMPA = 0;
//CMPB (records duty cycle of left motor)
EPwm2Regs.CMPB.bit.CMPB = 0;
//AQCTLA (right motor)
EPwm2Regs.AQCTLA.bit.CAU = 1;
EPwm2Regs.AQCTLA.bit.ZRO = 2;
//AQCTLB (left motor)
EPwm2Regs.AQCTLB.bit.CBU = 1;
EPwm2Regs.AQCTLB.bit.ZRO = 2;
//TBPHS
EPwm2Regs.TBPHS.bit.TBPHS = 0;
// PWM Pin, initialize pins as PWM instead of GPIO
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //CHANGED TO EPWM2A
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //CHANGED TO EPWM2B
EALLOW; // Below are protected registers
GpioCtrlRegs.GPAPUD.bit.GPIO2 = 1; // For EPWM2A
GpioCtrlRegs.GPAPUD.bit.GPIO3 = 1; // For EPWM2B
EDIS;
EALLOW;
EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (“pulse” is the same as “trigger”)
EPwm5Regs.TBCTR = 0x0; // Clear counter
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
//Period of EPWM5 sets sampling rate of interrupt functions
EPwm5Regs.TBPRD = 5000; // Set Period to 0.25ms sample. Input clock is 50MHz.
// Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
EDIS;
//LAB 4 ADC Configuration
EALLOW;
//write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
//Set pulse positions to late
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
//power up the ADCs
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
//delay for 1ms to allow ADC time to power up
DELAY_US(1000);
//Select the channels to convert and end of conversion flag
//Many statements commented out, To be used when using ADCA or ADCB
//ADCB
//set channel selects for SOC0 such that it corresponds to ADCINB4
AdcbRegs.ADCSOC0CTL.bit.CHSEL = 4; //SOC0 will convert Channel you choose Does not have to be B0
AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
//INT1SEL set to 0 because only SOC0 is being used for ADCB interrupt
AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //set to last SOC that is converted and it will set INT1 flag ADCB1
AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
EDIS;
// Enable DACA and DACB outputs
EALLOW;
DacaRegs.DACOUTEN.bit.DACOUTEN = 1;//enable dacA output-->uses ADCINA0
DacaRegs.DACCTL.bit.LOADMODE = 0;//load on next sysclk
DacaRegs.DACCTL.bit.DACREFSEL = 1;//use ADC VREF as reference voltage
DacbRegs.DACOUTEN.bit.DACOUTEN = 1;//enable dacB output-->uses ADCINA1
DacbRegs.DACCTL.bit.LOADMODE = 0;//load on next sysclk
DacbRegs.DACCTL.bit.DACREFSEL = 1;//use ADC VREF as reference voltage
EDIS;
//---------------------------------------------------------
//EPWM9: Buzzer
//TBCTL
EPwm9Regs.TBCTL.bit.CTRMODE = 0;
EPwm9Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm9Regs.TBCTL.bit.PHSEN = 0;
EPwm9Regs.TBCTL.bit.CLKDIV = 1;
//TBCTR
EPwm9Regs.TBCTR = 0;
//TBPRD
EPwm9Regs.TBPRD = 0;
//CAU = 0, do nothing when CPMA is 0;
//ZRO = 3, toggle EPWM9A
//AQCTLA
EPwm9Regs.AQCTLA.bit.CAU = 0;
EPwm9Regs.AQCTLA.bit.ZRO = 3;
//TBPHS
EPwm9Regs.TBPHS.bit.TBPHS = 0;
//-------------------------------------------------------------
// PWM Pin, initialize pins as PWM instead of GPIO
GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5); //CHANGED TO EPWM9A
EALLOW; // Below are protected registers
GpioCtrlRegs.GPAPUD.bit.GPIO16 = 1; // For EPWM9A
EDIS;
// Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
// which is connected to CPU-Timer 1, and CPU int 14, which is connected
// to CPU-Timer 2: int 12 is for the SWI.
IER |= M_INT1;
IER |= M_INT4; // Enable CPU INT4 which is connected to ECAP1-4 INT
IER |= M_INT8; // SCIC SCID
IER |= M_INT9; // SCIA
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
IER |= M_INT6;
// Enable eCAP INT1 in the PIE: Group 4 interrupt 1
PieCtrlRegs.PIEIER4.bit.INTx1 = 1;
// Enable TINT0 in the PIE: Group 1 interrupt 7
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
// Enable SWI in the PIE: Group 12 interrupt 9
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
//Adcb
PieCtrlRegs.PIEIER1.bit.INTx2 = 1;
//Lab 6, EXERCISE 2:
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
//Start of FFT Section
int16_t i = 0;
//float samplePeriod = 0.0002;
// Clear input buffers:
for(i=0; i < RFFT_SIZE; i++){
ping_input[i] = 0.0f;
}
for (i=0;i<RFFT_SIZE;i++) {
ping_input[i] = 0;
}
hnd_rfft->FFTSize = RFFT_SIZE;
hnd_rfft->FFTStages = RFFT_STAGES;
hnd_rfft->InBuf = &ping_input[0]; //Input buffer
hnd_rfft->OutBuf = &test_output[0]; //Output buffer
hnd_rfft->MagBuf = &pwrSpec[0]; //Magnitude buffer
hnd_rfft->CosSinBuf = &RFFTF32Coef[0]; //Twiddle factor buffer
RFFT_f32_sincostable(hnd_rfft); //Calculate twiddle factor
for (i=0; i < RFFT_SIZE; i++){
test_output[i] = 0; //Clean up output buffer
}
for (i=0; i <= RFFT_SIZE/2; i++){
pwrSpec[i] = 0; //Clean up magnitude buffer
}
//END of FFT Section
// Enable global Interrupts and higher priority real-time debug events
EINT; // Enable Global interrupt INTM
ERTM; // Enable Global realtime interrupt DBGM
// IDLE loop. Just sit and loop forever (optional):
while(1)
{
//Print to Tera Term
if (UARTPrint == 1 ) {
//serial_printf(&SerialA,"%s\r",RXBuff);
serial_printf(&SerialA,"maxpwr: %0.3f maxpwrindex: %d\n\r",maxpwr, maxpwrindex);
UARTPrint = 0;
}
//Print to Orange Pi
if (SendToOrange == 1 ) {
//serial_printf(&SerialD,"Testing:%ld\n",CpuTimer2.InterruptCount);
SendToOrange = 0;
}
//FFT Processing
if (ping == 1) {
hnd_rfft->InBuf = &ping_input[0]; //Input buffer
}
if (pong == 1) {
hnd_rfft->InBuf = &pong_input[0]; //Input buffer
}
if (ping == 1 || pong == 1) {
ping = 0;
pong = 0;
RFFT_f32(hnd_rfft); //Calculate real FFT
#ifdef __TMS320C28XX_TMU__ //defined when --tmu_support=tmu0 in the project
// properties
RFFT_f32_mag_TMU0(hnd_rfft); //Calculate magnitude
#else
RFFT_f32_mag(hnd_rfft); //Calculate magnitude
#endif
maxpwr = 0;
maxpwrindex = 0;
for (i=3;i<(RFFT_SIZE/2);i++) {
if (pwrSpec[i]>maxpwr) {
maxpwr = pwrSpec[i];
maxpwrindex = i;
}
}
//microphone records specific frequencies as commands to play certain songs
//1000Hz ==> 102 index
if (maxpwr >= 100 && maxpwrindex == 102 && song_lockout >= song_max) {
note = 1;
start_flag = 0;
//1050 Hz ==> 108 index
} else if(maxpwr >= 100 && maxpwrindex == 108 && song_lockout >= song_max) {
note = 2;
start_flag = 0;
//1100 Hz ==> 113 index
} else if(maxpwr >= 100 && maxpwrindex == 113 && song_lockout >= song_max) {
note = 3;
start_flag = 0;
}
if (song_lockout < song_max) {
song_lockout = song_lockout + 1;
}
UARTPrint = 1;
}
}
}
// SWI_isr, Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {
// These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
// making it lower priority than all other Hardware interrupts.
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP"); // Wait one cycle
EINT; // Clear INTM to enable interrupts
numSWIcalls++;
DINT;
}
// cpu_timer0_isr - CPU Timer0 ISR
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
numTimer0calls++;
//Identify Button Press
button_count = int16_tReadSwitches();
if (button_count == 0x4) {
state = 0x1;
start_flag = 0;
} else if(button_count == 0x8) {
state = 0x2;
start_flag = 0;
}
// Trigger HC-SR04
hc_sr04_trigger();
if (echo_distance <= 10) {
start_flag = 0;
state = 0x3;
}
// Acknowledge this interrupt to receive more interrupts from group 1
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
// cpu_timer1_isr - CPU Timer1 ISR
__interrupt void cpu_timer1_isr(void)
{
//songarray located in song.h,
if (song_index < sizeof(songarray) ) {
//sets period of buzzer pin's register to
//correspond to frequency of the note being played
EPwm9Regs.TBPRD = songarray[song_index];
//increment to play the next note in the song on next timer call
song_index = song_index + 1;
} else {
EPwm9Regs.TBPRD = 0;
//convert pin back to GPIO from PWM and clear the bit to remove noise
// GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0);
// GPIO_SetupPinOptions(16, GPIO_OUTPUT, GPIO_PUSHPULL);
// GpioDataRegs.GPACLEAR.bit.GPIO16 = 1;
}
CpuTimer1.InterruptCount++;
}
// cpu_timer2_isr CPU Timer2 ISR
__interrupt void cpu_timer2_isr(void)
{
//state machine
switch (state) {
//follow blob centroid
case 0x0:
if (centroid_diameter >= 15) {
Vel_ref = 50/centroid_diameter;
turn = Kp_centroid*(80-centroid_col);
} else {
Vel_ref = 0;
turn = 0;
}
//saturate reference velocity
if (Vel_ref > 1){
Vel_ref = 1;
} else if (Vel_ref < -1) {
Vel_ref = -1;
}
if (centroid_diameter >= 65) {
Vel_ref = 0;
}
if (centroid_diameter == temp_centroid_diam) {
centroid_count = centroid_count + 1;
}
if (centroid_count >= centroid_max) {
centroid_diameter = 0;
centroid_count = 0;
}
if (centroid_diameter == 0) {
turn = 0;
Vel_ref = 0;
}
temp_centroid_diam = centroid_diameter;
//play song if correct note is played
if (note == 1) {
state = 0xA;
start_flag = 0;
note = 0;
} else if (note == 2) {
start_flag = 0;
state = 0xB;
note = 0;
} else if (note == 3) {
start_flag = 0;
state = 0xC;
note = 0;
}
break;
//turn 45 degrees left
case 0x1:
if (start_flag == 0) {
start_flag = 1;
temp_pose_angle = pose_angle;
}
Vel_ref = 0;
turn = 2.5;
if (pose_angle >= temp_pose_angle + angle) {
temp_pose_angle = 0;
turn = 0;
state = 0x0;
start_flag = 0;
}
break;
//turn 45 degrees right
case 0x2:
if (start_flag == 0) {
start_flag = 1;
temp_pose_angle = pose_angle;
}
Vel_ref = 0;
turn = -2.5;
if ((pose_angle <= temp_pose_angle - angle)) {
temp_pose_angle = 0;
turn = 0;
state = 0x0;
start_flag = 0;
}
break;
//back up
case 0x3:
Vel_ref = -1;
backUp_count = backUp_count + 1;
if (backUp_count == backUp_dis) {
state = 0x2;
Vel_ref = 0;
backUp_count = 0;
}
//note = 1, song 1 plays
case 0xA:
if(start_flag == 0) {
CpuTimer1Regs.PRD.all = ((long)(tempo1*200) - 1);
song_index = 0;
start_flag = 1;
}
if(song_index >= 284) {
start_flag = 0;
song_index = 1000;
state = 0x0;
song_lockout = 0;
}
break;
//note = 2, song 2 plays
case 0xB:
if(start_flag == 0) {
CpuTimer1Regs.PRD.all = ((long)(tempo2*200) - 1);
song_index = 285;
start_flag = 1;
}
if(song_index >= 488) {
start_flag = 0;
song_index = 1000;
state = 0x0;
song_lockout = 0;
}
break;
//note = 3, song 3 plays
case 0xC:
if(start_flag == 0) {
CpuTimer1Regs.PRD.all = ((long)(tempo3*200) - 1);
song_index = 489;
start_flag = 1;
}
if(song_index >= 765) {
start_flag = 0;
song_index = 1000;
state = 0x0;
song_lockout = 0;
}
break;
default:
break;
}
...
This file has been truncated, please download it to see its full contents.
Robot Pet .ccsproject
C/C++Includes All Code Used to Run the Robot including Code Composer Project Files and Source Code
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<configurations XML_version="1.2" id="configurations_0">
<configuration XML_version="1.2" id="Texas Instruments XDS100v2 USB Debug Probe_0">
<instance XML_version="1.2" desc="Texas Instruments XDS100v2 USB Debug Probe_0" href="connections/TIXDS100v2_Connection.xml" id="Texas Instruments XDS100v2 USB Debug Probe_0" xml="TIXDS100v2_Connection.xml" xmlpath="connections"/>
<connection XML_version="1.2" id="Texas Instruments XDS100v2 USB Debug Probe_0">
<instance XML_version="1.2" href="drivers/tixds100v2icepick_c.xml" id="drivers" xml="tixds100v2icepick_c.xml" xmlpath="drivers"/>
<instance XML_version="1.2" href="drivers/tixds100v2c28x.xml" id="drivers" xml="tixds100v2c28x.xml" xmlpath="drivers"/>
<instance XML_version="1.2" href="drivers/tixds100v2cla1.xml" id="drivers" xml="tixds100v2cla1.xml" xmlpath="drivers"/>
<instance XML_version="1.2" href="drivers/tixds100v2cs_child.xml" id="drivers" xml="tixds100v2cs_child.xml" xmlpath="drivers"/>
<platform XML_version="1.2" id="platform_0">
<instance XML_version="1.2" desc="TMS320F28379D_0" href="devices/f28379d.xml" id="TMS320F28379D_0" xml="f28379d.xml" xmlpath="devices"/>
<device HW_revision="1" XML_version="1.2" description="" id="TMS320F28379D_0" partnum="TMS320F28379D">
<router HW_revision="1.0" XML_version="1.2" description="ICEPick_C router" id="IcePick_C_0" isa="ICEPICK_C">
<subpath id="Subpath_1">
<property Type="numericfield" Value="0x11" desc="Port Number_0" id="Port Number"/>
</subpath>
</router>
</device>
</platform>
</connection>
</configuration>
</configurations>
MATLAB File for HSV Color Thresholding
MATLABFile for Blob Search HSV Tuning Provided to me by Daniel Block at the University of Illinois at Urbana-Champaign
function ME461_ColorThreshold(image_name)
% ColorThreshold performs custom color thresholding on the image 'image_name'.
% Arguments: 'image_name', filename of the image to threshold
%
% The user can zoom in on a region, then add/remove pixels from the thresholding
% program. When you are satisfied, print the filter and the program will
% display the RGB or HSV statistics. You can also handtune filters using slider
% bars for RGB or HSV filters. The Hue filter can optionally be split for
% Hues that wraparound zero. The ROIs can be moved around as needed.
% My bitmaps came from a print screen % of an Image Graph in Code ComposerStudio.
% send suggestions/problems to abecker5@uiuc.edu
%
% NOTE: non GE423 students, switch rgb2hsvGE423 to the standard rgb2hsv
% Matlab function.
% TODO: 1.) use standard HSV
% 2.)allow function to be called with filter points.
% 3.) as soon as I move the mouse, the first image wobbles
if( nargin > 0)
Original_img = imread(image_name);
else
%%%%%%%%%%%%% YOU CAN EDIT THIS PART WITH A DEFAULT IMAGE NAME %%%%%%
%Original_img = imread('can2RGB.bmp'); %red pop can
Original_img = imread('ExampleRGB.bmp'); %orange & blue golf balls
%Original_img = imread('lena.bmp'); %famous image
%%%%%%%%%%%%% END EDIT %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
close all; format compact %my preferences
scrsz = get(0,'ScreenSize');
h1.fig = figure(1);
set(h1.fig,'Name','Original Image', 'Position',[10 scrsz(4)*.05 scrsz(3)*1/3 scrsz(4)*.9]);
set(h1.fig,'MenuBar','none'); %hide menu bars
subplot(2,1,1)
image(Original_img)
axis image
title('select your{\it region of interest} (ROI) by clicking twice','FontWeight','bold')
% Select corner points of a rectangular
% region by pointing and clicking the mouse twice
BOX = [];
% Select location of the rectangles 1st pt.
%set(gcf, 'Pointer', 'fullcross') was this
set(gcf, 'Pointer', 'cross')
waitforbuttonpress;
[h1.x0,h1.y0] = selectOnScreenPt();
% Select second point
set(gcf, 'Pointer', 'fleur')
set(gcf,'WindowButtonMotionFcn', @select_p0)
waitforbuttonpress;
set(gcf, 'Pointer', 'arrow')
set(gcf,'WindowButtonMotionFcn', '')
set(gcf,'WindowButtonDownFcn', @h1butdown)
title('Original Image')
% Index into the original image to create the new image
% by getting the x and y corner coordinates as integers
ROI = Original_img(floor(min(h1.y0,h1.y1)):ceil(max(h1.y0,h1.y1)), floor(min(h1.x0,h1.x1)):ceil(max(h1.x0,h1.x1)),:);
h2.numValidPX = 0;
% Display the subsetted image with appropriate axis ratio
% this image can be clicked on to add pixels to the thresholding list
h2.fig = figure(2); %start horz start vert width height
set(h2.fig,'Name','ROI', 'Position',[scrsz(3)*2/5 -30+scrsz(4)*.5 scrsz(3)*3/5 scrsz(4)*.5]);
set(h2.fig,'MenuBar','none');
image(ROI); axis image
title({'{\it left click} to select pixels, deselect with{\it right click}';'{\it drag} to select multiple pixels'},'FontWeight','bold')
Original_imgHSV = []; IsHSplit = 0; IsRGB = 1;%initialize variables for thresholding
h3 = initfig3();
set(0,'CurrentFigure',h1.fig); %now that the other 2 images are initialized, add thresholded image to fig1
th_imag_axis = subplot(2,1,2); select_thesh('', '');
figure(h2.fig);
% set(h2.fig, 'Pointer', 'fullcross') was this
set(h2.fig, 'Pointer', 'cross')
h2.ValidPX = zeros(numel(ROI)/3,2); h2.Patches = zeros(numel(ROI)/3,1); %variables to show selected pixels
set(h2.fig,'WindowButtonDownFcn', @h2buttondown);
set(h2.fig,'WindowButtonUpFcn', @h2buttonup);
set(h2.fig,'CloseRequestFcn', @threshCloseFig);
set(h3.fig,'CloseRequestFcn', @threshCloseFig);
%%% END OF PROGRAM
%%%%%%%%% Start of functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function str = strThreshedVals( IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHsplit)
if IsRGB
str = ['R [',num2str(rHmin),':',num2str(rHmax),'], G [',num2str(gSmin),':',num2str(gSmax),'], B [',num2str(bVmin),':',num2str(bVmax),']'];
else
if IsHsplit
str = ['H [0:',num2str(rHmin),',',num2str(rHmax),':255]'];
else
str = ['H [',num2str(rHmin),':',num2str(rHmax),']'];
end
str = [str,', S [',num2str(gSmin),':',num2str(gSmax),'], V [',num2str(bVmin),':',num2str(bVmax),']'];
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [h3] = initfig3()
IsRGB = 1;
h3.boolHandTune = false;
h3.fig = figure(3); %start horz start vert width height
set(h3.fig,'Name','Range Values', 'Position',[scrsz(3)*2/5 40 scrsz(3)*3/5 scrsz(4)*.4]);
set(h3.fig,'MenuBar','none');
h3.rgbplot = subplot(1,3,1);
axis([0 4 0 255]);
xVals = [0.8,0.8,1.8,1.8,2.8,2.8];
yVals = [100,200,100,200,100,200];
for n=1:6
h3.h(n)=plot(xVals(n),yVals(n),'sk','LineWidth',2,'markerEdgecolor','k','markersize',8,'Marker','none','MarkerFaceColor',[.8 .8 .8]);
hold on
end
h3.R = plot(1*ones(size(0)),0,'.r', 'HitTest','off');
h3.G = plot(2*ones(size(0)),0,'.g', 'HitTest','off');
h3.B = plot(3*ones(size(0)),0,'.b', 'HitTest','off');
set(h3.R, 'Xdata', [], 'Ydata', []);
set(h3.G, 'Xdata', [], 'Ydata', []);
set(h3.B, 'Xdata', [], 'Ydata', []);
axis([0 4 0 255]);
set(gca,'XTick',[1,2,3]);
set(gca,'XTickLabel',{'R','G','B'});
title({'RGB values selected';' '});
h3.hsvplot = subplot(1,3,2);
set(h3.fig,'CurrentAxes',h3.hsvplot);
for n=1:6
h3.h(n+6)=plot(xVals(n),yVals(n),'sk','LineWidth',2,'markerEdgecolor','k','markersize',8,'Marker','none','MarkerFaceColor',[.8 .8 .8]);
hold on
end
h3.H = plot(1*ones(size(0)),0,'.m', 'HitTest','off');
h3.S = plot(2*ones(size(0)),0,'.c', 'HitTest','off');
h3.V = plot(3*ones(size(0)),0,'.k', 'HitTest','off');
set(h3.H,'Xdata', [], 'Ydata', []);
set(h3.S,'Xdata', [], 'Ydata', []);
set(h3.V,'Xdata', [], 'Ydata', []);
axis([0 4 0 255]);
set(gca,'XTick',[1,2,3]);
set(gca,'XTickLabel',{'H','S','V'});
title({'HSV values selected';'\itnote: there may be wraparounds'});
h3.buttonArea = init_RGBbut; %setup the buttons and their callbacks
set(gcf, 'WindowButtonDownFcn', {@select_thesh})
title({'Thresholding';' Options'});
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit)
R = zeros(h2.numValidPX,1);
G = zeros(h2.numValidPX,1);
B = zeros(h2.numValidPX,1);
H = zeros(h2.numValidPX,1);
S = zeros(h2.numValidPX,1);
V = zeros(h2.numValidPX,1);
for in = 1:h2.numValidPX
R(in) = (ROI(h2.ValidPX(in,2),h2.ValidPX(in,1),1));
G(in) = (ROI(h2.ValidPX(in,2),h2.ValidPX(in,1),2));
B(in) = (ROI(h2.ValidPX(in,2),h2.ValidPX(in,1),3));
% [H(in) S(in) V(in)] = rgb2hsv(R(in)/255,G(in)/255,B(in)/255); %Matlab rgb2hsv is different than on robots
% H(in)=255*H(in);S(in)=255*S(in);V(in)=255*V(in);
[H(in) S(in) V(in)] = rgb2hsvME461(R(in),G(in),B(in)); %Matlab rgb2hsv is different than on robots
end
if h2.numValidPX > 0
if(IsRGB)
rHmax = max(R);
rHmin = min(R);
gSmax = max(G);
gSmin = min(G);
bVmax = max(B);
bVmin = min(B);
else
rHmax = max(H);
rHmin = min(H);
gSmax = max(S);
gSmin = min(S);
bVmax = max(V);
bVmin = min(V);
if(IsHSplit)
H = sort(H); %arranges all hues from smallest to largest
bigSplit = 0; % we want the largest possible split
for splitpt = 1:255
ind1 = find(H <splitpt,1, 'last' );
ind2 = find(H>=splitpt,1, 'first');
if(~isempty(ind1))
bot = H(ind1);
else
bot = 0;
end
if(~isempty(ind2))
top = H(ind2);
else
top = 255;
end
if top-bot-1 > bigSplit
bigSplit = top-bot-1;
rHmax = top;
rHmin = bot;
end
end % end for
end %end if hsplit
end
else %end if Valid PX
rHmax = 0;
rHmin = 255;
gSmax = 0;
gSmin = 255;
bVmax = 0;
bVmin = 255;
end
try %update the figure 3 if it exists
set(h3.R,'Xdata', 1*ones(size(R)), 'Ydata', R);
set(h3.G,'Xdata', 2*ones(size(G)), 'Ydata', G);
set(h3.B,'Xdata', 3*ones(size(B)), 'Ydata', B);
set(h3.H,'Xdata', 1*ones(size(H)), 'Ydata', H);
set(h3.S,'Xdata', 2*ones(size(S)), 'Ydata', S);
set(h3.V,'Xdata', 3*ones(size(V)), 'Ydata', V);
if h3.boolHandTune
if IsRGB
rHmin = min( get(h3.h(1),'ydata'),get(h3.h(2),'ydata'));
rHmax = max( get(h3.h(1),'ydata'),get(h3.h(2),'ydata'));
gSmin = min( get(h3.h(3),'ydata'),get(h3.h(4),'ydata'));
gSmax = max( get(h3.h(3),'ydata'),get(h3.h(4),'ydata'));
bVmin = min( get(h3.h(5),'ydata'),get(h3.h(6),'ydata'));
bVmax = max( get(h3.h(5),'ydata'),get(h3.h(6),'ydata'));
else
rHmin = min( get(h3.h(7),'ydata'),get(h3.h(8),'ydata'));
rHmax = max( get(h3.h(7),'ydata'),get(h3.h(8),'ydata'));
gSmin = min( get(h3.h(9),'ydata'),get(h3.h(10),'ydata'));
gSmax = max( get(h3.h(9),'ydata'),get(h3.h(10),'ydata'));
bVmin = min( get(h3.h(11),'ydata'),get(h3.h(12),'ydata'));
bVmax = max( get(h3.h(11),'ydata'),get(h3.h(12),'ydata'));
end
end
catch
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%thresh takes a color filter and applies it to the given image
function thresh(Original_img, IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit, figure_handle, axes_handle)
if(IsRGB)
Thresh_img = Original_img(:,:,1) >= rHmin & Original_img(:,:,1) <= rHmax ...
& Original_img(:,:,2) >= gSmin & Original_img(:,:,2) <= gSmax ...
& Original_img(:,:,3) >= bVmin & Original_img(:,:,3) <= bVmax;
threshed = cast(repmat(Thresh_img,[1,1,3]),'uint8').*Original_img; % threshold the original image
else
if ~exist('Original_imgHSV','var') || isempty(Original_imgHSV)
Original_imgHSV = Original_img;
for in = 1:numel(Original_img(:,1,1))
for ij = 1:numel(Original_img(1,:,1))
R = Original_img(in,ij,1);
G = Original_img(in,ij,2);
B = Original_img(in,ij,3);
% [H, S, V] = rgb2hsv(R/255,G/255,B/255);
% H=255*H;S=255*S;V=255*V;
[H, S, V] = rgb2hsvME461(R,G,B);
Original_imgHSV(in,ij,1) = H;
Original_imgHSV(in,ij,2) = S;
Original_imgHSV(in,ij,3) = V;
end
end
end
if IsHSplit
Thresh_img = (Original_imgHSV(:,:,1) <= rHmin | Original_imgHSV(:,:,1) >= rHmax) ...
& Original_imgHSV(:,:,2) >= gSmin & Original_imgHSV(:,:,2) <= gSmax ...
& Original_imgHSV(:,:,3) >= bVmin & Original_imgHSV(:,:,3) <= bVmax;
else
Thresh_img = Original_imgHSV(:,:,1) >= rHmin & Original_imgHSV(:,:,1) <= rHmax ...
& Original_imgHSV(:,:,2) >= gSmin & Original_imgHSV(:,:,2) <= gSmax ...
& Original_imgHSV(:,:,3) >= bVmin & Original_imgHSV(:,:,3) <= bVmax;
end
threshed = cast(repmat(Thresh_img,[1,1,3]),'uint8').*Original_imgHSV; % threshold the HSV image
end %IsRGB
currfig = get(0,'CurrentFigure'); %save the currently highlighted figure
if( nargin >= 11)
set(0,'CurrentFigure',figure_handle) %adjust the figure without setting it as selected
set(figure_handle,'CurrentAxes',axes_handle);
end
image(threshed)
axis image
threshedVals = strThreshedVals( IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit);
title({strcat('Threshed Image, ',num2str(sum(sum(Thresh_img))), ' px detected');threshedVals});
set(0,'CurrentFigure',currfig);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[H S V] = rgb2hsvGE423(R,G,B)
red = (double(R)-16)*255/224;
green = (double(G)-16)*255/224;
blue = (min(double(B)*2,240)-16)*255/224;
minV = min(red,min(green,blue));
value = max(red,max(green,blue));
delta = value - minV;
if(value~=0)
sat = (delta*255) / value;% s
if (delta ~= 0)
if( red == value )
hue = 60*( green - blue ) / delta; % between yellow & magenta
elseif( green == value )
hue = 120 + 60*( blue - red ) / delta; % between cyan & yellow
else
hue = 240 + 60*( red - green ) / delta; % between magenta & cyan
end
if( hue < 0 )
hue = hue + 360;
end
else
hue = 0;
sat = 0;
end
else
% r = g = b = 0 // s = 0, v is undefined
sat = 0;
hue = 0;
end
H = max(min(floor(((hue*255)/360)),255),0);
S = max(min(floor(sat),255),0);
V = max(min(floor(value),255),0);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[H S V] = rgb2hsvME461(R,G,B)
red = double(R);
green = double(G);
blue = double(B);
minV = min(red,min(green,blue));
value = max(red,max(green,blue));
delta = value - minV;
if(value~=0)
sat = (delta*255) / value;% s
if (delta ~= 0)
if( red == value )
hue = 60*( green - blue ) / delta; % between yellow & magenta
elseif( green == value )
hue = 120 + 60*( blue - red ) / delta; % between cyan & yellow
else
hue = 240 + 60*( red - green ) / delta; % between magenta & cyan
end
if( hue < 0 )
hue = hue + 360;
end
else
hue = 0;
sat = 0;
end
else
% r = g = b = 0 // s = 0, v is undefined
sat = 0;
hue = 0;
end
H = max(min(floor(((hue*255)/360)),255),0);
S = max(min(floor(sat),255),0);
V = max(min(floor(value),255),0);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function buttonArea = init_RGBbut(src, eventdata) %#ok<*INUSD>
IsRGB = 1;
IsHSplit = 0;
buttonArea.plot = subplot(1,3,3);
set(gca,'XTick',[]); %turn off ticks
set(gca,'YTick',[]);
buttonArea.butBkgOnC = [.5 .5 .5];
buttonArea.butBkgOffC = [.8 .8 .8];
butX = [0,1,1,0,0];
butY = [0,0,0.5,0.5,0];
buttonArea.butFontOffC = [.7 .7 .7];
buttonArea.butFontOnC = [0.0 0.0 0.0];
buttonArea.RGBbut = patch(butX,butY+0.5,buttonArea.butBkgOnC);
buttonArea.RGBtext = text(0.1,0.75,{'Using RGB'},'FontSize',14);
buttonArea.HSVbut = patch(butX,butY,buttonArea.butBkgOffC);
buttonArea.HSVtext = text(0.1,0.25,{'Use HSV'},'FontSize',14, 'Color',buttonArea.butFontOffC);
buttonArea.HSVSPLITbut = patch(butX,butY-0.5,buttonArea.butBkgOffC);
buttonArea.HSVSPLITtext = text(0.1,-0.25,{'Split H'},'FontSize',14, 'Color',buttonArea.butFontOffC);
buttonArea.HANDTUNEbut = patch(butX,butY-1,buttonArea.butBkgOffC);
buttonArea.HANDTUNEtext = text(0.1,-0.75,{'Hand Tune Limits'},'FontSize',10, 'Color',buttonArea.butFontOffC);
buttonArea.PRINTbut = patch(butX,butY-1.5,buttonArea.butBkgOffC);
buttonArea.PRINTtext = text(0.1,-1.25,{'Print Filter'},'FontSize',14, 'Color',buttonArea.butFontOffC);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = select_thesh(src, eventdata)
if (h3.buttonArea.plot == gca)
cp=get(gca,'CurrentPoint');
y0 = cp(1,2);
if(y0 >0.5)
IsRGB = 1;
set(h3.buttonArea.RGBbut,'FaceColor',h3.buttonArea.butBkgOnC );
set(h3.buttonArea.HSVbut,'FaceColor',h3.buttonArea.butBkgOffC );
set(h3.buttonArea.RGBtext,'String','Using RGB', 'Color',h3.buttonArea.butFontOnC);
set(h3.buttonArea.HSVtext,'String','Use HSV', 'Color',h3.buttonArea.butFontOffC);
elseif(y0 > 0)
IsRGB = 0;
set(h3.buttonArea.HSVbut,'FaceColor',h3.buttonArea.butBkgOnC );
set(h3.buttonArea.RGBbut,'FaceColor',h3.buttonArea.butBkgOffC );
set(h3.buttonArea.HSVtext,'String','Using HSV', 'Color',h3.buttonArea.butFontOnC);
set(h3.buttonArea.RGBtext,'String','Use RGB', 'Color',h3.buttonArea.butFontOffC);
elseif(y0 > -0.5)
IsHSplit = ~IsHSplit;
if(~IsHSplit)
set(h3.buttonArea.HSVSPLITbut,'FaceColor',h3.buttonArea.butBkgOffC);
set(h3.buttonArea.HSVSPLITtext,'Color',h3.buttonArea.butFontOffC);
else
set(h3.buttonArea.HSVSPLITbut,'FaceColor',h3.buttonArea.butBkgOnC);
set(h3.buttonArea.HSVSPLITtext,'Color',h3.buttonArea.butFontOnC);
end
elseif(y0 > -1)
if h3.boolHandTune
h3.boolHandTune = false;
set(h3.buttonArea.HANDTUNEbut,'FaceColor',h3.buttonArea.butBkgOffC);
set(h3.buttonArea.HANDTUNEtext,'Color',h3.buttonArea.butFontOffC);
else
h3.boolHandTune = true;
set(h3.buttonArea.HANDTUNEbut,'FaceColor',h3.buttonArea.butBkgOnC);
set(h3.buttonArea.HANDTUNEtext,'Color',h3.buttonArea.butFontOnC);
end
elseif(y0 >-1.5) %print the filter coefficients
set(h3.buttonArea.PRINTbut,'FaceColor',h3.buttonArea.butBkgOnC);
set(h3.buttonArea.PRINTtext,'Color',h3.buttonArea.butFontOnC);
[rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
threshedStr = strThreshedVals( IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit);
display(['Selected Threshold: ', threshedStr]);
pause(0.5)
set(h3.buttonArea.PRINTbut,'FaceColor',h3.buttonArea.butBkgOffC);
set(h3.buttonArea.PRINTtext,'Color',h3.buttonArea.butFontOffC);
end %button click
for icont=1:6
if( IsRGB && h3.boolHandTune )
set(h3.h(icont),'Marker','>')
else
set(h3.h(icont),'Marker','none')
end
end
for icont=7:12
if( ~IsRGB && h3.boolHandTune )
set(h3.h(icont),'Marker','>')
else
set(h3.h(icont),'Marker','none')
end
end
end
for icont = 1:12
if gco == h3.h(icont)
set(h3.h(icont),'MarkerFaceColor',h3.buttonArea.butBkgOnC );
set(h3.fig, 'WindowButtonMotionFcn', {@select_level, h3.h(icont)})
set(h3.fig,'WindowButtonDownFcn','')
set(h3.fig,'WindowButtonUpFcn', 'uiresume')
uiwait
set(h3.fig,'WindowButtonMotionFcn','')
set(h3.fig,'WindowButtonDownFcn',{@select_thesh})
set(h3.h(icont),'MarkerFaceColor',h3.buttonArea.butBkgOffC );
end
end
%update limits
[rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
thresh(Original_img, IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit, h1.fig, th_imag_axis)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[x,y] = selectOnScreenPt()
cp=get(gca,'CurrentPoint');
x=max(1,min(cp(1,1), size(Original_img,2)));
y=max(1,min(cp(1,2), size(Original_img,1)));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = select_p0(src, eventdata) % Interactively select first point
[h1.x1,h1.y1] = selectOnScreenPt();
draw_fig(h1.x0, h1.y0, h1.x1,h1.y1)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = h1butdown(src, eventdata) % Interactively select first point
if gco == BOX(1)
%BOX(2) = patch(x,y
[h1.xs,h1.ys] = selectOnScreenPt();
%display(num2str([xs,ys]))
set(gcf,'WindowButtonMotionFcn', {@movebox})
set(gcf,'WindowButtonUpFcn', {@h1butup})
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = h1butup(src, eventdata)% Interactively select first point
set(gcf,'WindowButtonMotionFcn', '');
set(gcf,'WindowButtonUpFcn', '');
[h1.xf,h1.yf] = selectOnScreenPt();
dx =h1.xf-h1.xs;
dy =h1.yf-h1.ys;
x0n = min(h1.x0+dx, h1.x1+dx);
y0n = min(h1.y0+dy, h1.y1+dy);
x1n = max(h1.x0+dx, h1.x1+dx);
y1n = max(h1.y0+dy, h1.y1+dy);
if( x1n > size(Original_img,2) )
x1n = size(Original_img,2); x0n = size(Original_img,2) - (h1.x1-h1.x0);
end
if( x0n < 1)
x0n = 1; x1n = 1+(h1.x1-h1.x0);
end
if( y1n > size(Original_img,1) )
y1n = size(Original_img,1); y0n = size(Original_img,1) - (h1.y1-h1.y0);
end
if( y0n < 1)
y0n = 1; y1n = 1+(h1.y1-h1.y0);
end
h1.x0 = x0n; h1.x1 = x1n; h1.y0 = y0n; h1.y1 = y1n;
draw_fig(h1.x0, h1.y0, h1.x1,h1.y1)
ROI = Original_img(floor(min(h1.y0,h1.y1)):ceil(max(h1.y0,h1.y1)), floor(min(h1.x0,h1.x1)):ceil(max(h1.x0,h1.x1)),:);
currfig = get(0,'CurrentFigure'); %save the currently highlighted figure
set(0,'CurrentFigure',h2.fig)
for c=1:h2.numValidPX
delete(h2.Patches(c))
end
h2.numValidPX = 0;
image(ROI)
axis image
title({'{\it left click} to select pixels, deselect with{\it right click}';'{\it drag} to select multiple pixels'},'FontWeight','bold')
% learn about the hue wrapparound by making another plot
[rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
% threshold original image using min and max values
thresh(Original_img, IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit, h1.fig, th_imag_axis);
set(0,'CurrentFigure',currfig )
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = movebox(src, eventdata)% % Ensure the box never leaves the region
[xf,yf] = selectOnScreenPt();
dx =xf-h1.xs;
dy =yf-h1.ys;
x0n = min(h1.x0+dx, h1.x1+dx);
y0n = min(h1.y0+dy, h1.y1+dy);
x1n = max(h1.x0+dx, h1.x1+dx);
y1n = max(h1.y0+dy, h1.y1+dy);
if( x1n > size(Original_img,2) )
x1n = size(Original_img,2); x0n = size(Original_img,2) - (h1.x1-h1.x0);
end
if( x0n < 1)
x0n = 1; x1n = 1+(h1.x1-h1.x0);
end
if( y1n > size(Original_img,1) )
y1n = size(Original_img,1); y0n = size(Original_img,1) - (h1.y1-h1.y0);
end
if( y0n < 1)
y0n = 1; y1n = 1+(h1.y1-h1.y0);
end
draw_fig(x0n, y0n, x1n,y1n)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = select_level(src, eventdata, pt)%#ok<INUSL>
cp=get(gca,'CurrentPoint'); %sets a color hand-tuned limit
y = cp(1,2);
x = cp(1,1);
if( x<-1 || x > 4 ||y<-40 || y>290 )
uiresume
end
y = max(min(255,round(y)),0);
set(pt,'ydata',y);
[rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
thresh(Original_img, IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit, h1.fig, th_imag_axis)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = h2buttondown(src, eventdata)
cp=get(gca,'CurrentPoint');
h2.x1 = round(cp(1,1));
h2.y1 = round(cp(1,2));
if strcmp(get(gcf,'selectiontype'), 'normal')
h2.but = 1;
elseif strcmp(get(gcf,'selectiontype'), 'alt')
h2.but = 3;
else
h2.but = [];
end
rbbox;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = threshCloseFig(src, eventdata)
% [rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
% threshedStr = strThreshedVals( IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit);
% display(['Selected Threshold: ', threshedStr]);
delete(get(0,'CurrentFigure'));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = h2buttonup(src, eventdata) % select/deselect pixels
set(gcf,'Pointer','watch'); drawnow expose
point2 = get(gca,'CurrentPoint'); % button up detected
%limit the points
h2.x1 = min(max(1, h2.x1), size(ROI,2));
h2.y1 = min(max(1, h2.y1), size(ROI,1));
x2 = min(max(1, round(point2(1,1))),size(ROI,2));
y2 = min(max(1, round(point2(1,2))),size(ROI,1));
xts = min(h2.x1,x2):max(h2.x1,x2);
yts = min(h2.y1,y2):max(h2.y1,y2);
for k = 1:numel(xts) %loop to step though all (de)selected pixels.
for j = 1:numel(yts)
xt = xts(k);
yt = yts(j);
newPixel = [xt,yt];
i = 1; %variable to step through ValidPX
foundpx = 0;
while foundpx == 0 && i <= h2.numValidPX %search to see if the pixel has already been selected
if h2.ValidPX(i,:) == round(newPixel);
foundpx = 1; %mark that we found it!
if(h2.but == 3) % remove this pixel
h2.numValidPX = h2.numValidPX -1;
delete(h2.Patches(i)) %remove the patch from plot window
if( i <= h2.numValidPX) %remove when not last item
h2.ValidPX(i:h2.numValidPX,:) = h2.ValidPX(i+1:h2.numValidPX+1,:);
h2.Patches(i:h2.numValidPX) = h2.Patches(i+1:h2.numValidPX+1);
end
end
end
i = i+1;
end %end while
if (foundpx == 0 && h2.but ==1) %add the patch to the screen
h2.numValidPX = h2.numValidPX +1;
h2.ValidPX(h2.numValidPX,:) = newPixel;
h2.Patches(h2.numValidPX)= patch([xt+0.5,xt+0.5,xt-0.5,xt-0.5],[yt+0.5,yt-0.5,yt-0.5,yt+0.5],'r');
end
end %column loop
end %row loop
%set(gcf,'Pointer','fullcross'); was this
set(gcf,'Pointer','cross');
% learn about the hue wrapparound by making another plot
[rHmin, rHmax, gSmin, gSmax, bVmin, bVmax] = RangeValues(IsRGB, h2, IsHSplit);
% threshold original image using min and max values
thresh(Original_img, IsRGB, rHmin, rHmax, gSmin, gSmax, bVmin, bVmax, IsHSplit, h1.fig, th_imag_axis);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[] = draw_fig(x0, y0, x1,y1) % Draws a rectangle patch
x=[x0,x1,x1,x0,x0];
y=[y0,y0,y1,y1,y0];
if isempty(BOX) % New patch
BOX(1) = patch(x,y,'k','edgecolor','k','LineStyle','-','facealpha',0,'LineWidth',2);
BOX(2) = patch(x,y,'k','edgecolor','w','LineStyle',':','facealpha',0,'HitTest','off','LineWidth',2);
else % Modify patch
set(BOX(1),'xdata',x,'ydata',y);
set(BOX(2),'xdata',x,'ydata',y);
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end %end function
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <highgui.h>
#include <math.h>
#include <sys/select.h>
#include <termios.h>
#include <stropts.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <stdint.h>
#include <sys/mman.h>
#include <linux/fb.h>
#include <time.h>
#define BILLION 1000000000L
#include "serial_dev.h"
char SERFILE[11] = {'/','d','e','v','/','t','t','y','S','2','\0'};
#define SERIALBUFFSIZE 1024
char DanTXBuff[SERIALBUFFSIZE];
/*
* setup_serial()
* sets the serial port up at 115200 baud
*/
int setup_serial()
{
sd_setup(SERFILE);//starts non-blocking
sd_ioflush();
}
// Simple structure to hold details about the framebuffer device after it has
// been opened.
typedef struct {
// Pointer to the pixels. Try to not write off the end of it.
uint32_t * buffer;
// The file descriptor for the device.
int fd;
// Number of bytes in the buffer. To work out how many elements are in the buffer, divide this by 4 (i.e. sizeof(uint32_t))
size_t screen_byte_size;
// Structs providing further information about the buffer configuration. See https://www.kernel.org/doc/Documentation/fb/api.txt
struct fb_fix_screeninfo fix_info;
struct fb_var_screeninfo var_info;
} screen_t;
// Because you can't have too many variants of NULL
static const screen_t NULL_SCREEN = {0};
// Indicates if the passed screen_t struct is valid.
#define valid_screen(s) ((s).buffer != NULL)
/**
* Opens the first frame buffer device and returns a screen_t struct
* for accessing it. If the framebuffer isn't in the expected format
* (32 bits per pixel), NULL_SCREEN will be returned.
*/
screen_t open_fb() {
const char * const SCREEN_DEVICE = "/dev/fb0";
int screen_fd = open(SCREEN_DEVICE, O_RDWR);
if (screen_fd == -1) {
printf("ERROR: Failed to open %s\n", SCREEN_DEVICE);
return NULL_SCREEN;
}
struct fb_var_screeninfo var_info = {0};
if (ioctl(screen_fd, FBIOGET_VSCREENINFO, &var_info) == -1) {
printf("ERROR: Failed to get variable screen info\n");
return NULL_SCREEN;
}
struct fb_fix_screeninfo fix_info = {0};
if (ioctl(screen_fd, FBIOGET_FSCREENINFO, &fix_info) == -1) {
printf("ERROR: Failed to get fixed screen info\n");
return NULL_SCREEN;
}
if (var_info.bits_per_pixel != 32) {
printf("ERROR: Only support 32 bits per pixel. Detected bits per pixel: %d\n", var_info.bits_per_pixel);
return NULL_SCREEN;
}
const size_t screen_byte_size = var_info.xres * var_info.yres * var_info.bits_per_pixel / 8;
uint32_t * const buffer = (uint32_t *)mmap(NULL, screen_byte_size, PROT_READ | PROT_WRITE, MAP_SHARED, screen_fd, 0 /* offset */);
screen_t screen = {
.buffer = buffer,
.fd = screen_fd,
.screen_byte_size = screen_byte_size,
.fix_info = fix_info,
.var_info = var_info
};
return screen;
}
/**
* Closes the framebuffer when you are finished with it. Don't try
* to access things in the struct after calling this or else a
* kitten will die.
*/
void close_fb(screen_t *screen) {
munmap(screen->buffer, screen->screen_byte_size);
close(screen->fd);
*screen = NULL_SCREEN;
}
#define NUMFRAME_ROWS 576
#define NUMFRAME_COLS 720
//#define FRAMEOFFSET1_ROWS 120
//#define FRAMEOFFSET1_COLS 40
//#define FRAMEOFFSET2_ROWS 120
//#define FRAMEOFFSET2_COLS 380
#define FRAMEOFFSET1_ROWS 20
#define FRAMEOFFSET1_COLS 40
#define FRAMEOFFSET2_ROWS 280
#define FRAMEOFFSET2_COLS 40
int my_min(int a, int b, int c);
int my_max(int a, int b, int c);
void rgb2hsv(int red,int green,int blue,int *hue,int *sat,int *value);
int _kbhit() {
static const int STDIN = 0;
static int initialized = 0;
if (! initialized) {
// Use termios to turn off line buffering
struct termios term;
tcgetattr(STDIN, &term);
term.c_lflag &= ~ICANON;
tcsetattr(STDIN, TCSANOW, &term);
setbuf(stdin, NULL);
initialized = 1;
}
int bytesWaiting;
ioctl(STDIN, FIONREAD, &bytesWaiting);
return bytesWaiting;
}
#define NUM_ROWS 240
#define NUM_COLS 320
#define MARKER_SIZE 10
//#define NUM_ROWS 120
//#define NUM_COLS 160
//#define MARKER_SIZE 5
int main(int argc, char** argv) {
uint64_t diff;
struct timespec current_time,previous_time;
int key=0;
CvCapture* capture1;
IplImage *frame1, *frame_hsv1;
cv::Mat DanMat(NUM_ROWS,NUM_COLS, CV_8UC1, cv::Scalar(0));
unsigned char *imagedata = (unsigned char*)(DanMat.data);
capture1= cvCaptureFromCAM(0);
cvSetCaptureProperty(capture1,CV_CAP_PROP_FRAME_WIDTH, NUM_COLS); // Number of columns
cvSetCaptureProperty(capture1,CV_CAP_PROP_FRAME_HEIGHT, NUM_ROWS); // Number of rows
// cvSetCaptureProperty(capture1,CV_CAP_PROP_FPS,30); // Doesn't seem to do anything
if(!capture1) printf("No camera1 detected \n");
frame1=cvQueryFrame(capture1);
frame_hsv1=cvCreateImage(cvGetSize(frame1),8,3);
screen_t screen = open_fb();
if (!valid_screen(screen)) {
printf("ERROR: Failed to open screen\n");
return 1;
}
float period=0,Hz=0;
unsigned char *data_hsv, *data_bgr, *current_pixel;
int r=0;
int c=0;
int area = 0;
int maxarea = 0;
int maxindex = 0;
int nr;
int nc;
int celements;
int numpixels1 = 0;
int rowsum = 0;
int colsum = 0;
int rbar1 = 0;
int cbar1 = 0;
nr=frame_hsv1->height; //number of rows in image should be 120
nc=frame_hsv1->width; //number of collumns
celements = nc*3; // b,g,r elements in each column
//int vh=255, vl=20, sh=184, sl=63, hh=165, hl=149;
int vh=255, vl=207, sh=255, sl=207;
int h=0, s=0, v=0;
int blue=0, green=0, red=0;
// set up the parameters (check the defaults in opencv's code in blobdetector.cpp)
cv::SimpleBlobDetector::Params params;
params.minDistBetweenBlobs = 40.0f;
params.filterByInertia = false;
params.filterByConvexity = false;
params.filterByColor = false;
params.filterByCircularity = false;
params.filterByArea = true;
params.minArea = 100.0f;
params.maxArea = 20000.0f;
// ... any other params you don't want default value
// set up and create the detector using the parameters
cv::SimpleBlobDetector blob_detector(params);
// or cv::Ptr<cv::SimpleBlobDetector> detector = cv::SimpleBlobDetector::create(params)
std::vector<cv::KeyPoint> keypoints;
printf("Initializing serial port driver %s...\n",SERFILE);
setup_serial();
printf("...OK\n");
//sd_set_nonblocking();
sd_set_blocking();
printf(".\n");
sd_ioflush();
clock_gettime(CLOCK_MONOTONIC, &previous_time);
while(! _kbhit()) { //27 is the code corresponding to escape key
frame1=cvQueryFrame(capture1);
if(!frame1) break;
// -----loop through image pixels-----
data_bgr=(unsigned char *)(frame1->imageData);
numpixels1 = 0;
rowsum = 0;
colsum = 0;
for(r=0; r<nr; r++) {
for(c=0; c<nc;c++) {
blue = (int)data_bgr[r*celements+c*3];
green = (int)data_bgr[r*celements+c*3+1];
red = (int)data_bgr[r*celements+c*3+2];
rgb2hsv(red,green,blue,&h,&s,&v);
//--------------process each thresholded pixel here------------------
if(((h>=0 && h<=20) || (h>=250 && h <= 255)) && (s>=sl && s<=sh) && (v>=vl && v<=vh)) {
//set pixel to green in the bgr image
green = 255;
blue = 0;
red = 0;
numpixels1++;
rowsum += r;
colsum += c;
imagedata[DanMat.cols * r + c ] = 255;
} else {
imagedata[DanMat.cols * r + c ] = 0;
}
screen.buffer[(r+FRAMEOFFSET1_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET1_COLS + c)] = (int)((red<<16) | (green<<8) | blue);
}
} //--------------end pixel processing---------------------
if (numpixels1 > 0) {
rbar1 = rowsum/numpixels1 - NUM_ROWS/2;
cbar1 = colsum/numpixels1 - NUM_COLS/2;
} else {
rbar1 = 0;
cbar1 = 0;
}
blob_detector.detect(DanMat, keypoints);
// extract the x y coordinates of the keypoints:
printf("Num Keypoints = %d\n",keypoints.size());
for(r=0; r<nr; r++) {
for(c=0; c<nc;c++) {
if (imagedata[DanMat.cols * r + c ] == 255) {
red = 0;
green = 255;
blue = 0;
} else {
red = 0;
green = 0;
blue = 0;
}
screen.buffer[(r+FRAMEOFFSET2_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET2_COLS + c)] = (int)((red<<16) | (green<<8) | blue);
}
}
red = 255;
blue = 0;
green = 0;
maxarea = 0;
maxindex = -1;
for (int i=0; i<keypoints.size(); i++){
c = (int)keypoints[i].pt.x;
r = (int)keypoints[i].pt.y;
area = (int)keypoints[i].size;
if (area > maxarea) {
maxarea = area;
maxindex = i;
}
printf("r=%d,c=%d,a=%d ",r,c,area);
if ( (r > MARKER_SIZE) && (r < (NUM_ROWS-MARKER_SIZE)) && (c > MARKER_SIZE) && (c < (NUM_COLS-MARKER_SIZE)) ) {
for (int ri=r-MARKER_SIZE/2; ri < r+MARKER_SIZE/2;ri++) {
for (int ci=c-MARKER_SIZE/2; ci < c+MARKER_SIZE/2;ci++) {
// printf("c=%d",ci);
screen.buffer[(ri+FRAMEOFFSET2_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET2_COLS + ci)] = (int)((red<<16) | (green<<8) | blue);
}
// printf("r=%d",ri);
}
// printf("\n");
}
}
if (maxindex >= 0) {
sprintf(DanTXBuff,"Max: r=%d c=%d a=%d\n",(int)keypoints[maxindex].pt.y,(int)keypoints[maxindex].pt.x,(int)keypoints[maxindex].size);
sd_write(DanTXBuff);
}
clock_gettime(CLOCK_MONOTONIC, ¤t_time);
diff = BILLION * (current_time.tv_sec - previous_time.tv_sec) + current_time.tv_nsec - previous_time.tv_nsec;
period = diff * 1e-9;
if (period > 0.0) {
Hz = 1.0/period;
} else {
Hz = 0.0;
}
//printf("Hz = %.4f, time = %llu r=%d, c=%d, num=%d,Key=%d\n",Hz,(long long unsigned int)diff,rbar,cbar,numpixels,key);
//printf("Hz = %.4f,r1=%d,c1=%d,num1=%d\n",Hz,rbar1,cbar1,numpixels1);
printf("Hz = %.4f\n",Hz);
previous_time = current_time;
} // end while
cvReleaseCapture(&capture1);
printf("All done, bye bye.\n");
close_fb(&screen);
return 0;
}
int my_min(int a, int b, int c) {
int min;
min = a;
if (b < min) {
min = b;
}
if (c < min) {
min = c;
}
return (min);
}
int my_max(int a, int b, int c) {
int max;
max = a;
if (b > max) {
max = b;
}
if (c > max) {
max = c;
}
return (max);
}
void rgb2hsv(int red,int green,int blue,int *hue,int *sat,int *value) {
int min, delta;
min = my_min( red, green, blue );
*value = my_max( red, green, blue );
delta = *value - min;
if( *value != 0 ) {
*sat = (delta*255) / *value; // s
if (delta != 0) {
if( red == *value )
*hue = 60*( green - blue ) / delta; // between yellow & magenta
else if( green == *value )
*hue = 120 + 60*( blue - red ) / delta; // between cyan & yellow
else
*hue = 240 + 60*( red - green ) / delta; // between magenta & cyan
if( *hue < 0 )
*hue += 360;
} else {
*hue = 0;
*sat = 0;
}
} else {
// r = g = b = 0
// s = 0, v is undefined
*sat = 0;
*hue = 0;
}
*hue = (*hue*255)/360;
}
#include "serial_dev.h"
int sd_setup(char *dn)
{
// struct serial_struct serinfo;
sd_dev_fd = open(dn, O_RDWR| O_NOCTTY | O_NONBLOCK);
strcpy(sd_dev_name, dn);
if (sd_dev_fd < 0)
{
perror(dn);
exit(-1);
}
sd_ioflush();
// added to change to ASYNC_LOW_LATENCY
// Not sure if this helps at all or is doing anything
// ioctl (sd_dev_fd, TIOCGSERIAL, &serinfo);
// serinfo.flags |= ASYNC_LOW_LATENCY;
// ioctl (sd_dev_fd, TIOCSSERIAL, &serinfo);
tcgetattr(sd_dev_fd, &sd_options);
sd_options.c_cflag = B1500000| CS8 | CLOCAL | CREAD;
// 1.5Mhz fastest you can go on Orange Pi Zero.
sd_options.c_iflag = IGNPAR;
sd_options.c_oflag = 0;
sd_options.c_lflag = 0;
sd_options.c_cc[VMIN]=1;
sd_options.c_cc[VTIME]=0;
//cfmakeraw(&options);
tcsetattr(sd_dev_fd, TCSANOW ,&sd_options);
printf("set\n");
}
int sd_set_blocking()
{
long temp_flags;
temp_flags = fcntl(sd_dev_fd, F_GETFL);
if (temp_flags == -1)
perror("serial_dev::set_blocking get\n");
temp_flags &= ~O_NONBLOCK;
int i = fcntl(sd_dev_fd, F_SETFL, temp_flags);
if (i == -1)
perror("serial_dev::set_blocking set \n");
else {
return sd_flags = temp_flags;
sd_blocking = 1;
}
}
int sd_set_nonblocking()
{
long temp_flags;
temp_flags = fcntl(sd_dev_fd, F_GETFL);
if (temp_flags == -1)
perror("serial_dev::set_nonblocking get\n");
temp_flags |= O_NONBLOCK;
int i = fcntl(sd_dev_fd, F_SETFL, temp_flags);
if (i == -1) {
perror("serial_dev::set_nonblocking set \n");
return i;
}
else {
return sd_flags = temp_flags;
sd_blocking = 0;
}
}
int sd_iflush()
{
return tcflush(sd_dev_fd, TCIFLUSH);
}
int sd_oflush()
{
return tcflush(sd_dev_fd, TCOFLUSH);
}
int sd_ioflush()
{
return tcflush(sd_dev_fd, TCIOFLUSH);
}
ssize_t sd_writen(char *buf, size_t count)
{
size_t orig_count = count;
int temp;
if (sd_blocking) {
while(count > 0) {
temp = write(sd_dev_fd, buf, count);
if (temp == -1)
perror("serial_dev::write");
count -= temp;
buf += temp;
}
return orig_count;
}
else {
return write(sd_dev_fd, buf, count);
}
}
ssize_t sd_readn(char *buf, size_t count)
{
size_t orig_count = count;
int temp;
if (sd_blocking) {
while(count > 0) {
temp = read(sd_dev_fd, buf, count);
if (temp == -1)
perror("serial_dev::read");
count -= temp;
buf += temp;
}
return orig_count;
}
else {
return read(sd_dev_fd, buf, count);
}
}
ssize_t sd_write(char *stringdata)
{
size_t len = strlen(stringdata);
return sd_writen(stringdata, len);
}
ssize_t sd_read(size_t len)
{
return sd_readn(sd_data, len);
}
int sd_get_speed()
{
sd_get_options();
speed_t code = cfgetospeed(&sd_options);
switch (code) {
case B0: return(0);
case B300: return(300);
case B1200: return(1200);
case B2400: return(2400);
case B4800: return(4800);
case B9600: return(9600);
case B19200: return(19200);
case B38400: return(38400);
case B57600: return(57600);
default: return(115200);
};
}
int sd_set_speed(int speed)
{
unsigned int rate;
if (speed < 300)
rate = B0;
else if (speed < 1200)
rate = B300;
else if (speed < 2400)
rate = B1200;
else if (speed < 4800)
rate = B2400;
else if (speed < 9600)
rate = B4800;
else if (speed < 19200)
rate = B9600;
else if (speed < 38400)
rate = B19200;
else if (speed < 57600)
rate = B38400;
else if (speed < 115200)
rate = B57600;
else
rate = B115200;
sd_get_options();
if(cfsetispeed(&sd_options, rate) != 0)
perror("Failed setting input speed\n");
if(cfsetospeed(&sd_options, rate) != 0)
perror("Failed setting output speed\n");
return sd_set_options();
}
int sd_get_options()
{
return tcgetattr(sd_dev_fd, &sd_options);
}
int sd_set_options()
{
return tcsetattr(sd_dev_fd,TCSANOW, &sd_options);
}
int sd_kill()
{
close(sd_dev_fd);
}
Comments