Waffle Robot

#include <stdio.h>
#include <opencv/cv.h>
#include <highgui.h>
#include <opencv2/videoio/videoio_c.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>

float incrementation(float intial, float desired, float delta);

#define BILLION 1000000000L
#define I2C

#ifdef I2C
#include "i2c.h"
#endif

#define ADDR1  0x25
#define ADDR2  0x11

unsigned char state = 'w';

char cmd1=0;
char cmd2=0;		
char cmd3=0;
char cmd4=0;	
char cmd5=0;
char cmd6=0;	
char cmd7=0;
char cmd8=0;

// Kinematic Parameters
float l1 = 102.6;
float l2 = 31;
float l3 = 118;
float l4 = 20;
float l5 = 133.51;
float lt = 110;

// Default Position
float x = 164.51;//l2+l5;
float y = 0.0;	
float z = 240.6;//l1 + l3 + l4;
float roll = 0.0;
float grip = 70.0;
float pitch = 0.0;
float yaw = 0.0;


//Incrementing Kinematic Parameters
float x_increment = 0.0;
float y_increment = 0.0;
float z_increment = 0.0;
float roll_increment = 0.0;
float pitch_increment = 0.0;
float yaw_increment = 0.0;
float cmd6_increment = 0.0;
float grip_increment = 0.0;
char state_b = 'a';
char test_flag = 0;


float x_desired = 164.51; //x
float y_desired = 0.0; //y 	 
float z_desired = 240.6; //z
float roll_desired = 0.0;
float pitch_desired = 0.0;
float yaw_desired = 0.0;
float cmd6_desired = 0.0;
float grip_desired = 70.0;
int cmd6_temp = 0;

float increment = 0.0;

float green_average = 0;
float green_check_cnt = 1;

// 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)

#define NUMFRAME_ROWS 800
#define NUMFRAME_COLS 480
#define FRAMEOFFSET_ROWS 150
#define FRAMEOFFSET_COLS 30

#define NUM_ROWS 120
#define NUM_COLS 160

screen_t open_fb(void);
void close_fb(screen_t *screen);
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(void);


int main(void) { 
#ifdef I2C
	int bus;
	// add any other I2C variables here
	char a = 1;
    //Computer Vision I2C variables
    unsigned char CV_tx[8]={0,0,0,0,0,0,0,0};
    unsigned char CV_rx[8]={0,0,0,0,0,0,0,0};
    long CV_rxlong1 = 0;
	long CV_rxlong2 = 0;
	long CV_txlong1 = 0;
	long CV_txlong2 = 0;
	char bytosend = 0;

    //Robot Arm I2C variables
    unsigned char RA_tx[8]={0,0,0,0,0,0,0,0};
    unsigned char RA_rx[8]={0,0,0,0,0,0,0,0};
    long RA_rxlong1 = 0;
	long RA_rxlong2 = 0;
	long RA_txlong1 = 0;
	long RA_txlong2 = 0;


#endif

	uint64_t diff;
	struct timespec current_time,previous_time;

	int key=0;
	CvCapture* capture;

	IplImage *frame, *frame_hsv;

	capture= cvCaptureFromCAM(-1);

	cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH, NUM_COLS-40);  // Number of columns
	cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT, NUM_ROWS-40); // Number of rows

	if(!capture) printf("No camera detected \n");

	frame=cvQueryFrame(capture);  
	frame_hsv=cvCreateImage(cvGetSize(frame),8,3);

#ifdef I2C
	bus = i2c_start_bus(1);
#endif
  
	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 nr;
	int nc;
	int celements;
	nr=frame_hsv->height; //number of rows in image should be 120 
	nc=frame_hsv->width; //number of collumns
	celements = nc*3;  // b,g,r elements in each column

	// int offR_vh=255, offR_vl=156, offR_sh=245, offR_sl=176, offR_hh=255, offR_hl=240; //Off red
	// int onR_vh=255, onR_vl=131, onR_sh=255, onR_sl=19, onR_hh=255, onR_hl=243; //On red
	
	// int offG_vh=255, offG_vl=134, offG_sh=255, offG_sl=244, offG_hh=137, offG_hl=123; //Off green
	// int onG_vh=255, onG_vl=126, onG_sh=255, onG_sl=152, onG_hh=130, onG_hl=101; //On green
	



	//we care if the green light is on or off
	//int onG_vh=255, onG_vl=126, onG_sh=255, onG_sl=152, onG_hh=130, onG_hl=101; //On green
	//AND about if the red light is on or off
	//int vh=255, vl=131, sh=255, sl=19, hh=255, hl=243;


//--------------GREEN pixel processing---------------------
	int vh=255, vl=240, sh=164, sl=0, hh=127, hl=97; //green


	int h=0, s=0, v=0;
	int blue=0, green=0, red=0;

	int g_numpix = 0, g_Xpix = 0, g_Ypix = 0, g_Xcent = 0, g_Ycent = 0;

	clock_gettime(CLOCK_MONOTONIC, &previous_time);

	while(! _kbhit()) {  //27 is the code corresponding to escape key

		frame=cvQueryFrame(capture);

		if(!frame) break;

		g_numpix = 0;
		g_Xpix = 0;
		g_Ypix = 0;
		 
		// -----loop through image pixels-----  
		data_bgr=(unsigned char *)(frame->imageData); 
		for(r=0; r<nr; r++) {  
			for(c=0; c<nc;c++) {
				blue = (int)data_bgr[r*celements+c*3];
				g
				reen = (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>=hl && h <= hh) && (s>=sl && s<=sh) && (v>=vl && v<=vh) ) {
					green = 255;
					blue = 0; 
					red = 0;
				g_Xpix = c + g_Xpix;
				g_Ypix = r + g_Ypix; 
				g_numpix ++;
				
				} 
				
				screen.buffer[(r+FRAMEOFFSET_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET_COLS + c)] = (int)((red<<16) | (green<<8) | blue);
 				
			}
		} 

	

		//--------------end pixel processing---------------------
		/*
		//Centroid Pixel Processing
		if (numpix > 100){    
		Xcent = Xpix/(numpix);
		Ycent = Ypix/(numpix);
		printf("X:%d  Y:%d\n", Xcent, Ycent);
		for (int i = -10; i < 10; i++){
			if (((Xcent+i)<160) && ((Xcent+i)>=0)){
				screen.buffer[((int)Ycent+FRAMEOFFSET_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET_COLS + (int)Xcent+i)] = ((0<<16) | (0<<8) | 0);	
			}
			for(int j = -10; j < 10; j++){
				if (((Ycent+j)<120) && ((Ycent+j)>=0)){
					screen.buffer[((int)Ycent+j+FRAMEOFFSET_ROWS)*NUMFRAME_COLS + (FRAMEOFFSET_COLS + (int)Xcent)] = ((0<<16) | (0<<8) | 0);
				}
			}	
		}
		}
		else{
			Xcent = NUM_COLS/2;
			Ycent = NUM_ROWS/2;
		}
		*/
		//I2C related code goes here(Scale Xcent, Ycent to TBCCR1)
		
		//Indicate if waffle is finished cooking
		if(green_check_cnt<20){
			if (green_check_cnt == 1){
				green_average = g_numpix;
			} else{
				green_average = (green_average+g_numpix)/2;
			}
			green_check_cnt++;
		}
		CV_txlong1 = green_average;
		
		if(green_check_cnt>=20){
			green_check_cnt=0;
			CV_txlong2 = 0x00;
			if((green_average>400)){
				CV_txlong2 |= 1; //1 green light on		
			} else{
				CV_txlong2 &= 1;
			}	
		
			CV_tx[0] = (unsigned char)(CV_txlong1 >>0);
			CV_tx[1] = (unsigned char)(CV_txlong1 >>8);
			CV_tx[2] = (unsigned char)(CV_txlong1 >>16);
			CV_tx[3] = (unsigned char)(CV_txlong1 >>24);			
			CV_tx[4] = (unsigned char)(CV_txlong2 >>0);
			CV_tx[5] = (unsigned char)(CV_txlong2 >>8);
			CV_tx[6] = (unsigned char)(CV_txlong2 >>16);
			CV_tx[7] = (unsigned char)(CV_txlong2 >>24);

       			//Computer Vision Data
			i2c_write_bytes(bus, ADDR1, CV_tx, 8);
			i2c_read_bytes(bus, ADDR1, CV_rx, 8);
		} 
		
		
		//Changing Desired Position
		//not working - not sure why?
		state = (char)CV_rxlong2;

		if(state=='s'){
			x_desired = l2+l5;
			y_desired = 0.0;		
			z_desired = l1 + l3 + l4; 
		}
		if(state=='w'){
			x_desired = l2+l5;
			y_desired = 50.0;		
			z_desired = l1 + l3 + l4; 
		} 
		if(state=='b'){ 
			test_flag = 1;
			switch(state_b){
				case 'a': //get level with cup
					x_desired = l2+l5+30.0;
					y_desired = 50.0;		
					z_desired = l1 + l3 + l4-150.0;
					grip_desired = 70.0;
					
					if((x_desired==x)&(y_desired==y)&(z_desired==z)){
						usleep(500*1000);
						state_b = 'c';
					}
				break;
				
				case 'c': //grip cup
					grip_desired = 115.0;
					
					if(abs(grip - grip_desired) < 2.0){
						usleep(500*1000);
						state_b = 'd';
					}
				break;
				
				case 'd': //move cup upright
					y_desired = 0.0;
					z_desired = l1 + l3 + l4 + 10.0;
					
					if((x_desired==x)&(y_desired==y)&(z_desired==z)){
						usleep(500*1000);
					
						state_b = 'e';
					}
				
				break;
					case 'e': //move cup above iron
					x_desired = l2+l5 - 60.0;
					y_desired = -80.0;		
					z_desired = l1 + l3 + l4 + 20.0;
					yaw_desired = -1.0;
					
					
					if((x_desired==x)&(y_desired==y)&(z_desired==z)){
						usleep(500*1000);
						cmd6_temp = (int)cmd6;
						state_b = 'f';
					}
				break;

				case 'f': //pour cup
					cmd6_desired = (float)cmd6_temp + 20.0;

						if(((int)cmd6_desired == (int)cmd6)){
						usleep(5000*1000);
						state_b = 'g';
					}
				break;
				
				case 'g': //return cup upright
					roll_desired = 0.0;
						if((roll_desired == roll)){
						usleep(500*1000);
						state_b = 'h';
					}
				break;
				
				case 'h': //move cup over trash
					x_desired = l2+l5;
					y_desired = 50.0;		
					z_desired = l1 + l3 + l4 - 50.0;
				break;
				
				case 'i': //drop cup in trash
				break;

				case 'j': //move back to start
					x_desired = l2+l5;
					y_desired = 50.0;		
					z_desired = l1 + l3 + l4 - 50.0;
				break;



			}
		}		
		if(state=='c'){ 
			x_desired = l2+l5- 50.0;
			y_desired = 50.0;		
			z_desired = l1 + l3 + l4 ; 
		}
		if(state=='r'){ 
			x_desired = l2+l5;
			y_desired = 50.0;		
			z_desired = l1 + l3 + l4; 
		}

		// x_desired = l2+l5+30;
		// y_desired = 50.0;		
		// z_desired = l1 + l3 + l4-150.0;
		// grip_desired = 100.0;
		// roll_desired = 0;//30.0*0.0174;
		

		//Kinematics Incremetnation
		x_increment = incrementation( x,  x_desired, 2);
		y_increment = incrementation( y,  y_desired, 2);
		z_increment = incrementation( z,  z_desired, 2);
		roll_increment = incrementation( roll,  roll_desired, 0.01);
		yaw_increment = incrementation(yaw, yaw_desired, 0.1);
		grip_increment = incrementation( grip, grip_desired, 1);
    	cmd6_increment = incrementation( cmd6, cmd6_desired, 1);


		//Kinematics code here
		x = x + x_increment;
		y = y + y_increment;	
		z = z + z_increment;

		roll = roll + roll_increment;
		yaw = yaw + yaw_increment;
		pitch = pitch + pitch_increment;
		grip = grip + grip_increment; 



		float T0EE[4][4] = {{   cos(pitch)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll), - sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll), cos(roll)*cos(yaw), x - lt*cos(roll)*cos(yaw)},
							{ - cos(pitch)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw),   cos(yaw)*sin(pitch) - cos(pitch)*sin(roll)*sin(yaw), cos(roll)*sin(yaw), y - lt*cos(roll)*sin(yaw)},
							{                                 -cos(roll)*sin(pitch),                                 -cos(pitch)*cos(roll),         -sin(roll),          z + lt*sin(roll)},
							{                                                     0,                                                     0,                  0,                         1}};
 

		float r2 = z-l1;
		float r1 = sqrt(x*x + y*y) - l2;
		float r3 = sqrt(r1*r1 + r2*r2);
		float r4 = 135; 
		float th1_sol = atan(y/x);
		float beta = atan(r2/r1);
		float alpha = acos((r3*r3 + l3*l3 -r4*r4)/(2*r3*l3));
		float th2_sol = alpha + beta - 3.14/2;
		float gamma = acos((r4*r4 + l3*l3 - r3*r3)/(2*r4*l3));
		float th3_sol = gamma + atan(l5/l4) - 3.14;

		float T30[4][4] = {{-sin(th2_sol + th3_sol)*cos(th1_sol), -sin(th2_sol + th3_sol)*sin(th1_sol),  cos(th2_sol + th3_sol), l2*sin(th2_sol + th3_sol) - l1*cos(th2_sol + th3_sol) - l3*cos(th3_sol)},
				{-cos(th2_sol + th3_sol)*cos(th1_sol), -cos(th2_sol + th3_sol)*sin(th1_sol), -sin(th2_sol + th3_sol), l2*cos(th2_sol + th3_sol) + l1*sin(th2_sol + th3_sol) + l3*sin(th3_sol)},
				{sin(th1_sol),                -cos(th1_sol),               0,                                                   0},
				{		0,                        0,               0,                                                   1}};

		// float T46[4][4] = T30*T0EE;
		float T46[4][4] = 
		{{ sin(th2_sol + th3_sol)*sin(th1_sol)*(cos(pitch)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)) - sin(th2_sol + th3_sol)*cos(th1_sol)*(cos(pitch)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)) - cos(th2_sol + th3_sol)*cos(roll)*sin(pitch), sin(th2_sol + th3_sol)*cos(th1_sol)*(sin(pitch)*sin(yaw) + cos(pitch)*cos(yaw)*sin(roll)) - sin(th2_sol + th3_sol)*sin(th1_sol)*(cos(yaw)*sin(pitch) - cos(pitch)*sin(roll)*sin(yaw)) - cos(th2_sol + th3_sol)*cos(pitch)*cos(roll), - cos(th2_sol + th3_sol)*sin(roll) - sin(th2_sol + th3_sol)*cos(roll)*cos(th1_sol)*cos(yaw) - sin(th2_sol + th3_sol)*cos(roll)*sin(th1_sol)*sin(yaw), l2*sin(th2_sol + th3_sol) - l1*cos(th2_sol + th3_sol) + cos(th2_sol + th3_sol)*(z + lt*sin(roll)) - l3*cos(th3_sol) - sin(th2_sol + th3_sol)*cos(th1_sol)*(x - lt*cos(roll)*cos(yaw)) - sin(th2_sol + th3_sol)*sin(th1_sol)*(y - lt*cos(roll)*sin(yaw))},
		{ cos(th2_sol + th3_sol)*sin(th1_sol)*(cos(pitch)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)) - cos(th2_sol + th3_sol)*cos(th1_sol)*(cos(pitch)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)) + sin(th2_sol + th3_sol)*cos(roll)*sin(pitch), cos(th2_sol + th3_sol)*cos(th1_sol)*(sin(pitch)*sin(yaw) + cos(pitch)*cos(yaw)*sin(roll)) - cos(th2_sol + th3_sol)*sin(th1_sol)*(cos(yaw)*sin(pitch) - cos(pitch)*sin(roll)*sin(yaw)) + sin(th2_sol + th3_sol)*cos(pitch)*cos(roll),   sin(th2_sol + th3_sol)*sin(roll) - cos(th2_sol + th3_sol)*cos(roll)*cos(th1_sol)*cos(yaw) - cos(th2_sol + th3_sol)*cos(roll)*sin(th1_sol)*sin(yaw), l2*cos(th2_sol + th3_sol) + l1*sin(th2_sol + th3_sol) - sin(th2_sol + th3_sol)*(z + lt*sin(roll)) + l3*sin(th3_sol) - cos(th2_sol + th3_sol)*cos(th1_sol)*(x - lt*cos(roll)*cos(yaw)) - cos(th2_sol + th3_sol)*sin(th1_sol)*(y - lt*cos(roll)*sin(yaw))},
		{                                                                                             cos(th1_sol)*(cos(pitch)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw)) + sin(th1_sol)*(cos(pitch)*sin(yaw) - cos(yaw)*sin(pitch)*sin(roll)),                                                                                           - cos(th1_sol)*(cos(yaw)*sin(pitch) - cos(pitch)*sin(roll)*sin(yaw)) - sin(th1_sol)*(sin(pitch)*sin(yaw) + cos(pitch)*cos(yaw)*sin(roll)),                                                                                                                         sin(th1_sol - yaw)*cos(roll),                                                                                                                                                                     sin(th1_sol)*(x - lt*cos(roll)*cos(yaw)) - cos(th1_sol)*(y - lt*cos(roll)*sin(yaw))},
		{                                                                                                                                                                                                                                   0,                                                                                                                                                                                                                                   0,                                                                                                                                                    0,                                                                                                                                                                                                                                                       1}};
 

		float th5_sol = acos( -T46[1][2]);
		float th4_sol = asin( T46[2][2] / sin(th5_sol) );
		float th6_sol = asin( -T46[1][0] / sin(th5_sol));

		// printf("%03f %03f %03f %03f %03f %03f\n", th1_sol, th2_sol, th3_sol, th4_sol, th5_sol, th6_sol);
		printf("TX1: %03ld TX2: %03ld  b:%c RX2:%c gncnt:%03ld\n", (long)CV_txlong1, (long)CV_txlong2, (char)state_b, (char)CV_rxlong2, (long)green_check_cnt);

			cmd1 = 90 + (int)(th1_sol*57.30) ;
			cmd2 = 90 + (int)(th2_sol*57.30);		
			cmd3 = 30 - (int)(th2_sol*57.30) - (int)(th3_sol*57.30);
			cmd4 = 90 + (int)(th4_sol*57.30) ;	
			cmd5 = 90 - (int)(th5_sol*57.30);
			cmd6 = 90 + (int)(th6_sol*57.30)+ (int)cmd6_increment;	
			cmd7 = (int)grip;
			cmd8 = 90;
			
			// printf("%03ld %03ld %03ld %03ld %03ld %03ld\n", (long)cmd1, (long)cmd2,(long)cmd3, (long)cmd4, (long)cmd5, (long)cmd6);

		RA_tx[0] = (unsigned char)(cmd1);  
  		RA_tx[1] = (unsigned char)(cmd2); 
		RA_tx[2] = (unsigned char)(cmd3);
		RA_tx[3] = (unsigned char)(cmd4);			
		RA_tx[4] = (unsigned char)(cmd5);
		RA_tx[5] = (unsigned char)(cmd6);
		RA_tx[6] = (unsigned char)(cmd7);
		RA_tx[7] = (unsigned char)(cmd8);

		


		//Robot Arm Data
		i2c_write_bytes(bus, ADDR2, RA_tx, 8);
		i2c_read_bytes(bus, ADDR2, RA_rx, 8);

		
		CV_rxlong1 = (CV_rx[3]<<24) + (CV_rx[2]<<16) + (CV_rx[1]<<8) + (CV_rx[0]);
		CV_rxlong2 = (CV_rx[7]<<24) + (CV_rx[6]<<16) + (CV_rx[5]<<8) + (CV_rx[4]);
		
		RA_rxlong1 = (RA_rx[3]<<24) + (RA_rx[2]<<16) + (RA_rx[1]<<8) + (RA_rx[0]);
		RA_rxlong2 = (RA_rx[7]<<24) + (RA_rx[6]<<16) + (RA_rx[5]<<8) + (RA_rx[4]);

		
        //printf("%03ld %03ld %03ld\n", (long)x, (long)y,(long)z);
 	
        // usleep(200*1000);
		

		clock_gettime(CLOCK_MONOTONIC, &current_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\n",Hz);
		previous_time = current_time;
	}  // end while

	cvReleaseCapture(&capture);

#ifdef I2C
	
	i2c_close_bus(bus);

#endif

	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;
	
}

/**
 * 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(void) {
	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;
}

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;
}


//Aids in position incrementation
float incrementation(float initial, float desired, float delta){
	//x_desired = desired position
	//delta = servo motor change
	//x = current position
	if (desired > initial){
		increment = delta;
    } else if(desired < initial){
		increment = -delta;
	}else if(desired == initial){
		increment = 0;
	}
	return increment;
}