Project showcase
Waffle Robot

Waffle Robot

Hungry for waffles but tired of making them? The waffle robot can do all the hard work for you with the press of a button.

  • 1,008 views
  • 0 comments
  • 9 respects

Components and supplies

Bella Rotating Waffle Maker
×1
OrangePI Zero
×1
A000066 iso both
Arduino UNO & Genuino UNO
×1
Sainsmart 6-Axis Robotic Arm
×1
Sg90 servo motor 180 degrees sg90 micro
SG90 Micro-servo motor
These servomotors replaced for the two servo motors after the end of the robot arm. Although there were lightly modified to fit into the arm, they performed better than the originals. After multiple runs, original ones wore down and could no longer lift the gripper.
×2
Adafruit TowerPro servo motor
×1
Arduino Servo Motor Shield
×1

Necessary tools and machines

70y8518 40
Drill / Driver, Cordless
Hy gluegun
Hot glue gun (generic)
3drag
3D Printer (generic)

About this project

Kitchens are changing, incorporating more robotic processes to make life more simple and efficient. Some restaurants have tried completely automating their dishes. Bevier Cafe, located at the University of Illinois, challenged us to create a Waffle Robot, where students could effortlessly make their morning waffles and interact with robotics in the food industry. Click the link below to watch a demo of our project.

Demo Video:

https://youtu.be/tJtyDmpLMlU

The Waffle Robot has 5 modes:

1. Waits for a user

2. Asks a user to load the batter and spray the iron

3. Pours the batter, throws away the cup, and asks the user to close the lid

4. Rotates and cooks the waffle

5. Prompts the user to remove the waffle and waves goodbye

A button allows the user to indicate when they have finished performing prompted tasks. The button indicates if users would like to make a waffle, if users have sprayed the pan, if users have closed the lid, and when users have received the waffle.

The system uses three major components:

  • MSP 430
  • OrangePI
  • Arduino

The MSP430 microcontroller controls the state of the robot and sends this information over I2C to the OrangePI. It receives data from the OrangePI indicating when the waffle has finished cooking. The MSP430 also controls the rotation of the waffle iron.

The OrangePI processes camera data, receives the waffle state from the MSP430, computes inverse kinematics and sends the angular position commands to the Arduino. The number of green pixels is collected from the camera data to indicate if the waffle has finished cooking. Due to fluctuations in lighting, the green pixel count was averaged over 20 images. This average is then compared to the previous average. If there is a large difference in the readings, this indicates the LED had turned on. If enough green pixels are counted (and the waffle has cooked for at least 90 seconds), the waffle has finished cooking and is rotated to its original position for removal.

Because the OrangePI has the most computational power, it is used to compute the inverse kinematics code and send the angular position data to the Arduino over I2C. Different commands are sent based on the waffle state received from the MSP430. The Arduino then sends these commands to the servomotors in the Sainsmart robot arm.

Most of the time spent on this project was focused on setting up the I2C communication, troubleshooting with connection issues, and calibrating the robot code. Loose wires, multiple inverse kinematics solutions, the deterioration of the servo motor gears, and accounting for servo motor torque ability were the biggest problems faced in this project. When I2C communications failed, it was usually due to loose wires on the board. Sometimes the robot arm would be programmed to move through a point that had multiple solutions, causing the arm to move in an undesirable way. This was fixed by decreasing the speed and slightly changing the path. After multiple iterations, the gears in the two micro servomotors deteriorated. The replacements had to be slightly modified (drilling screw holes) but worked much better than the originals.

Lifting the waffle iron with a servo motor required too much torque for the servo motors available for the project. Therefore the tasks of lifting the waffle iron and removing the waffle are given to the user. In addition, the robot arm lifts 2 small cups of batter in order to minimize the torque.

For future iterations of this project, it is recommended using a more powerful servomotor to automate the opening/closing of the waffle iron as well as a system that automates the placement/pouring of the cups and batter. In addition, the system could be better organized to give the user a better reach to the batter cup placement locations and push button.

Code

WaffleMaker.cC/C++
This code goes onto the OrangePI. It sets up communication between the MSP430 and the Arduino. It computes the inverse kinematics and computer vision calculations. It then sends this information accordingly. Because I2C communication transmits and receives, all the devices must be set up in order for the system to run.
#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;
}
I2CC/C++
Helps to set up I2C communication
#include "i2c.h"
#define ADDR  0x25

int i2c_start_bus(unsigned char bus) {
	char filename[11];
	int fd;
	
	sprintf(filename, "/dev/i2c-%d", bus);
	
	fd = open(filename, O_RDWR);
	
	if (fd < 0)
		fprintf(stderr, "Failed to communicate with I2C bus #%d.\n", bus);
		
	return fd;
}

void i2c_close_bus(int fd) {
	close(fd);
}

int i2c_write_byte(int file, unsigned char addr, unsigned char value) {
	int i;
	unsigned char outbuf;
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[1];

	messages[0].addr  = addr;
	messages[0].flags = 0;
	messages[0].len   = 1;
	messages[0].buf   = &outbuf;

	outbuf = value;

	/* Transfer the i2c packets to the kernel and verify it worked */
	packets.msgs  = messages;
	packets.nmsgs = 1;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}

int i2c_write_bytes(int file, unsigned char addr, unsigned char *value, unsigned char num) {
	int i;
	unsigned char outbuf[256];
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[1];

	messages[0].addr  = addr;
	messages[0].flags = 0;
	messages[0].len   = num;
	messages[0].buf   = outbuf;

	if (num > 255) {
		perror("Maximum Send num is 255");
		return 1;
	}
	for (i = 0; i <= num; i++)
		outbuf[i] = value[i];

	/* Transfer the i2c packets to the kernel and verify it worked */
	packets.msgs  = messages;
	packets.nmsgs = 1;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}
	
int i2c_write(int file, unsigned char addr, unsigned char reg, unsigned char *value, unsigned char num) {
	int i;
	unsigned char outbuf[256];
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[1];

	messages[0].addr  = addr;
	messages[0].flags = 0;
	messages[0].len   = num + 1;
	messages[0].buf   = outbuf;

	/* The first byte indicates which register we'll write */
	outbuf[0] = reg;

	/* 
	 * The remaining bytes indicate the values that will be written.
	 * The maximum number of data bytes allowed is 255.
	 */
	for (i = 1; i <= num; i++)
		outbuf[i] = value[i - 1];

	/* Transfer the i2c packets to the kernel and verify it worked */
	packets.msgs  = messages;
	packets.nmsgs = 1;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}

int i2c_read(int file, unsigned char addr, unsigned char reg, unsigned char *value, unsigned char num) {
	unsigned char outbuf;
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[2];

	/*
	 * In order to read a register, we first do a "dummy write" by writing
	 * 0 bytes to the register we want to read from.  This is similar to
	 * the packet in set_i2c_register, except it's 1 byte rather than 2.
	 */
	outbuf = reg;
	messages[0].addr  = addr;
	messages[0].flags = 0;
	messages[0].len   = sizeof(outbuf);
	messages[0].buf   = &outbuf;

	/* The data will get returned in this structure */
	messages[1].addr  = addr;
	messages[1].flags = I2C_M_RD/* | I2C_M_NOSTART*/;
	messages[1].len   = num;
	messages[1].buf   = value;

	/* Send the request to the kernel and get the result back */
	packets.msgs	  = messages;
	packets.nmsgs	  = 2;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}


int i2c_read_byte(int file, unsigned char addr,unsigned char *value) {
	unsigned char outbuf;
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[1];

	/* The data will get returned in this structure */
	messages[0].addr  = addr;
	messages[0].flags = I2C_M_RD/* | I2C_M_NOSTART*/;
	messages[0].len   = 1;
	messages[0].buf   = value;

	/* Send the request to the kernel and get the result back */
	packets.msgs	  = messages;
	packets.nmsgs	  = 1;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}


int i2c_read_bytes(int file, unsigned char addr, unsigned char *value, unsigned char num) {
	unsigned char outbuf;
	struct i2c_rdwr_ioctl_data packets;
	struct i2c_msg messages[1];


	/* The data will get returned in this structure */
	messages[0].addr  = addr;
	messages[0].flags = I2C_M_RD/* | I2C_M_NOSTART*/;
	messages[0].len   = num;
	messages[0].buf   = value;

	/* Send the request to the kernel and get the result back */
	packets.msgs	  = messages;
	packets.nmsgs	  = 1;
	if(ioctl(file, I2C_RDWR, &packets) < 0) {
		perror("Unable to send data");
		return 1;
	}

	return 0;
}
I2C.hC/C++
Helps to set up I2C. Goes onto the OrangePI.
#include <stdio.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <string.h>

int i2c_start_bus(unsigned char bus);
void i2c_close_bus(int fd);
int i2c_write(int file, unsigned char addr, unsigned char reg, unsigned char *value, unsigned char num);
int i2c_write_byte(int file, unsigned char addr, unsigned char value);
int i2c_read(int file, unsigned char addr, unsigned char reg, unsigned char *value, unsigned char num);
int i2c_read_byte(int file, unsigned char addr,unsigned char *value);
int i2c_write_bytes(int file, unsigned char addr, unsigned char *value, unsigned char num);
int i2c_read_bytes(int file, unsigned char addr, unsigned char *value, unsigned char num);
makeit.shC/C++
Used to make an executable file of WaffleMaker.c. This goes onto the OrangePI
gcc -O2  i2c.c WaffleMaker.c -lm -o WaffleMaker `pkg-config --cflags --libs opencv` 
ArduinoArduino
This code runs on the Arduino
#include <Servo.h>
#include <Wire.h>  // Library which contains functions to have I2C Communication

#define SLAVE_ADDRESS 0x11 // Define the I2C address to Communicate to Uno

Servo servo_1;
Servo servo_2;
Servo servo_3;
Servo servo_4;
Servo servo_5;
Servo servo_6;
Servo servo_g;


long num_events=0;
int index = 0;
unsigned char newservovalues[8] = "0"; //collects cmd from orange pi
char newservoflag = 0;
int cmd1 = 0;
int cmd2 = 0;
int cmd3 = 0;
int cmd4 = 0;
int cmd5 = 0;
int cmd6 = 0;
int cmd7 = 0;
int cmd8 = 0;

void setup() {
  Wire.begin(SLAVE_ADDRESS); // this will begin I2C Connection with 0x11 address
  Wire.onReceive(receiveEvent); // sendData is funtion called when Pi requests data
  Serial.begin(9600); //baud rate for arduino COM

  servo_1.attach(11);
  servo_2.attach(10);
  servo_3.attach(9);
  servo_4.attach(8);
  servo_5.attach(7);
  servo_6.attach(6);
  servo_g.attach(5);

  servo_1.write(90);
  servo_2.write(100);
  servo_3.write(20);
  servo_4.write(90);
  servo_5.write(90);
  servo_6.write(90);
  servo_g.write(90);
}

void loop() {
      if (newservoflag == 1){
     cmd1 = newservovalues[0];
     cmd2 = newservovalues[1];
     cmd3 = newservovalues[2];
     cmd4 = newservovalues[3];
     cmd5 = newservovalues[4];
     cmd6 = newservovalues[5];
     cmd7 = newservovalues[6];
     cmd8 = newservovalues[7];
     
     newservoflag = 0;
     
     servo_1.write(cmd1);
     servo_2.write(cmd2);
     servo_3.write(cmd3);
     servo_4.write(cmd4);
     servo_5.write(cmd5);
     servo_6.write(cmd6);
     servo_g.write(cmd7);
     
     Serial.print(cmd1);
     Serial.print("\t");
     Serial.print(cmd2);
     Serial.print("\t");
     Serial.print(cmd3);
     Serial.print("\t");
     Serial.print(cmd4);
     Serial.print("\t");
     Serial.print(cmd5);
     Serial.print("\t");
     Serial.print(cmd6);
     Serial.print("\t");
     Serial.print(cmd7);
     Serial.print("\t");
     Serial.print(cmd8);
     Serial.print("\n");    
  }
}
void receiveEvent(int howMany)
{
  while(1 < Wire.available()) // loop through all but the last
  {
    char c = Wire.read(); // receive byte as a character
    newservovalues[index] = c; //save the new data in an array
    index++;
  }
  
  char x = Wire.read();    // receive byte as an integer
    newservovalues[index] = x;
    index=0;

  newservoflag = 1;

}
user_Waffle_StateCodeC/C++
This code controls the state of the robot. This should be loaded onto the MSP430
/******************************************************************************
MSP430F2272 Project Creator 4.0

ME 461 - S. R. Platt
Fall 2010

Updated for CCSv4.2 Rick Rekoske 8/10/2011

Written by: Steve Keres
College of Engineering Control Systems Lab
University of Illinois at Urbana-Champaign
 *******************************************************************************/

#include "msp430x22x2.h"
#include "UART.h"

char newprint = 0;
unsigned long timecnt = 0;
char TXData[8] = {0,0,0,0,0,0,0,0};
char RXData[8] = {0,0,0,0,0,0,0,0};
long rcv_1 = 0;
long rcv_2 = 0;
long snd_1 = 0;
long snd_2 = 0;
char flag = 0;
int rec_flag = 1;
char servo_flag = 0;
char servo_cnt = 0;
char greenLight = 0; //flag: 0 if not ready, 1 if ready
//char redLight = 0; //0 if see red light, 1 if no red (indicates if closed during cook state)
char greenLightLarge = 1; //1 if closed, 0 if open
//char handleInPlace = 1; //This should be set when the waffle iron is in the down position
//(probably need some sort of 3D print attachement to waffle so that an external serve will slip in when in the down position)
char button = 0; //0 if waiting, 1 if button pressed (to start waffle process)
char state = 'w';
char robot_flag = 0; //sent to Arduino to command movement, 1 send command, 0 no command
char robot_stopped = 0; //1 robot moving, 0 robot stopped
int enjoy_count = 0;
char text = 0;

//void delay(int seconds){}

void main(void) {

    WDTCTL = WDTPW + WDTHOLD;                 // Stop WDT

    if (CALBC1_16MHZ ==0xFF || CALDCO_16MHZ == 0xFF) while(1);

    DCOCTL = CALDCO_16MHZ;                      // Set uC to run at approximately 16 Mhz
    BCSCTL1 = CALBC1_16MHZ;

    //P1IN              Port 1 register used to read Port 1 pins setup as Inputs
    P1SEL &= ~0xFF; // Set all Port 1 pins to Port I/O function
    P1REN &= ~0xFF; // Disable internal resistor for all Port 1 pins
    P1DIR |= 0xFF;  // Set Port 1 Pin 0 and Pin 1 (P1.0,P1.1) as an output.  Leaves Port1 pin 2 through 7 unchanged
    P1OUT &= ~0xFF;  // Initially set all Port 1 pins set as Outputs to zero
    P1IFG &= ~0xFF;  // Clear P_ort 1 interrupt flags
    P1IES &= ~0xFF; // If port interrupts are enabled a High to Low transition on a Port pin will cause an interrupt
    P1IE &= ~0xFF; // Disable all port interrupts


    //Add your code here to initialize USCIB0 as an I2C slave

    // Timer A Config
    TACCTL0 = CCIE;                           // Enable Timer A interrupt
    TACCR0 = 16000;                           // period = 1ms
    TACTL = TASSEL_2 + MC_1;                  // source SMCLK, up mode

    //P2IN          Port 2 register used to read Port 2 pins setup as Inputs (For Button press, currently using button 4)
    P2SEL &= ~0xFF; // 0000 0000 Set all Port 2 pins to Port I/O function
    P2REN |= 0xFF; // Enable internal resistor for all Port 2 pins
    P2DIR &= ~0xFF;   // Set all pins in Port 2 as an input.
    P2OUT |= 0xC0; // setting bits 6 and 7
    P2OUT &= ~0x30; // clearing bits 4 and 5
    P2IE |= 0xF0;                             // P2.4~7 interrupt enabled
    P2IES |= 0xC0;                            // 1100 0000 -->  Hi/lo edge
    P2IES &= ~0x30;                            // 0011 0000 -->  lo/hi edge
    P2IFG &= ~0xF0;                           // P1.3 IFG cleared

    /* Uncomment this block when using the wireless modems on the robots
    // Wireless modem control pins
    P2SEL &= ~0xC0; // Digital I/O
    P2REN &= ~0xC0; // Disable internal resistor for P2.6 and P2.7
    P2OUT |= 0x80;          // CMD/Data pin high for data
    P2DIR |= 0x80;          // Output direction for CMD/data pin (P2.7)
    P2DIR &= ~0x40;         // Input direction for CTS pin (P2.6)
    P1IFG &= ~0xC0;  // Clear Port P2.6 and P2.7 interrupt flags
    P1IES &= ~0xC0; // If port interrupts are enabled a High to Low transition on a Port pin will cause an interrupt
    P1IE &= ~0xC0; // Disable P2.6 and P2.7 interrupts
     */

    UCB0CTL1 = UCSWRST + UCSSEL_2;
    P3SEL |= 0x06;
    UCB0CTL0 = UCSYNC + UCMODE_3;
    UCB0I2COA = 0x0025;
    UCB0CTL1 &= ~UCSWRST;
    IE2 |= UCB0RXIE;
    IFG2 &= ~0xFF;

    // Timer B Config
    TBCCR0 = 40000;                           // period = 1ms
    TBCCR1 = 20000;
    TBCTL = TBSSEL_2 + MC_1 + ID_3;                  // source SMCLK, up mode, divider 8

    //P4.4 to PWM
    P4SEL |= 0x02;  //From datasheet looked for TA1 setting P1.2
    P4DIR |= 0x02;
    TBCCTL1 = OUTMOD_7 + CLLD_1; //reset/set for convention in datasheet

    Init_UART(9600,1);  // Initialize UART for 9600 baud serial communication

    _BIS_SR(GIE);   // Enable global interrupt

    TBCCR1 = 1000; //start with shaft in open position
    while(1) {
        switch (state) {
        case 'w': //wait
            greenLight=0;
            // wait for customer to press button

            if((button==1) & (state=='w')){
                state='s';
                button = 0;
            }
            break;

        case 's': //spray
            // prompt customer to spray pan and place cup on x
            //print("Please spray waffle iron and load batter");
            //print("Press button when finished");
            if((button==1)&(state=='s')){
                state = 'b';
                button = 0;
            }
            break;

        case 'b': //batter
            //print("please close lid");
            //robot move
            if((button==1)){
                state = 'c';
                button = 0;
                TBCCR1 = 4500;
            }
            break;

        case 'c': //cook
            // wait until sees green light to send MSP430 flag to turn back shaft to correct position
            if((greenLight==1 && (rcv_1 > 90))){
                TBCCR1 = 1000; //rotate shaft back
                state = 'r';
                greenLight=0;
            }

            if((button==1)){
                state = 'r';
                TBCCR1 = 1000; //rotate shaft back
                button = 0;
            }
            break;

        case 'r': //remove
            // hand user fork and prompt to take out waffle
            //print("Please open and remove waffle");
            if((button==1)){
                state = 'w';
                button = 0;
            }
            break;

        default:
            // statements
            break;
        }


        if(newmsg) {
            //my_scanf(rxbuff,&var1,&var2,&var3,&var4);
            newmsg = 0;
        }

        if (newprint)  {
            //P1OUT ^= 0x1;     // Blink LED
            //UART_printf("%c, %d, gL:%d\n\r", state, greenLight, greenLightLarge);//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);
            // UART_send(1,(float)timecnt/500);


            if(state=='w'){
                UART_printf("want a waffle?\n\r");//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);

            }
            if(state=='s'){
                UART_printf("please spray, %d\n\r", rcv_2);//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);
            }
            if(state=='b'){
                UART_printf("pls cls whn fin %d\n\r", rcv_2);//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);
            }
            if(state=='c'){
                UART_printf("cooking %d %d\n\r", TBCCR1, rcv_2);//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);
            }

            if(state=='r'){
                UART_printf("enjoy! %d \n\r", rcv_2);//("%ld,%ld,%d\n\r", rcv_1, snd_1, greenLight);
            }
            newprint = 0;
        }

    }
}


// Timer A0 interrupt service routine
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A (void)
{
    timecnt++; // Keep track of time for main while loop.

    //    if ((timecnt%50) == 0) {
    //        //        newprint = 1;  // flag main while loop that .5 seconds have gone by.
    //        if(state=='c'){
    //
    //            if(TBCCR1 <= 4000){
    //                servo_flag=1;
    //            }
    //            else if(TBCCR1 >= 4500){
    //                servo_flag=0;
    //            }
    //            if(servo_flag == 1){
    //                TBCCR1 = TBCCR1 + 25;
    //            }
    //            else if(servo_flag == 0){
    //                TBCCR1 = TBCCR1 - 25;
    //            }
    //        }
    //    }

}

// Port 2 interrupt service routine - runs if button is pressed
#pragma vector=PORT2_VECTOR
__interrupt void Port_2(void)
{
    if(P2IFG&BIT4){
        button=1; //button has been pressed
        P1OUT = P1OUT^BIT4; //toggling only bit 4 (LED CHECK)
        P2IFG &= ~BIT4; //clear P2IFG flag
    }
}

/*
// ADC 10 ISR - Called when a sequence of conversions (A7-A0) have completed
#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR(void) {

}
 */


// USCI Transmit ISR - Called when TXBUF is empty (ready to accept another character)
#pragma vector=USCIAB0TX_VECTOR
__interrupt void USCI0TX_ISR(void) {

    if((IFG2&UCA0TXIFG) && (IE2&UCA0TXIE)) {        // USCI_A0 requested TX interrupt
        if(printf_flag) {
            if (currentindex == txcount) {
                senddone = 1;
                printf_flag = 0;
                IFG2 &= ~UCA0TXIFG;
            } else {
                UCA0TXBUF = printbuff[currentindex];
                currentindex++;
            }
        } else if(UART_flag) {
            if(!donesending) {
                UCA0TXBUF = txbuff[txindex];
                if(txbuff[txindex] == 255) {
                    donesending = 1;
                    txindex = 0;
                } else {
                    txindex++;
                }
            }
        }

        IFG2 &= ~UCA0TXIFG;
    }

    if((IFG2&UCB0RXIFG) && (IE2&UCB0RXIE)) {    // USCI_B0 RX interrupt occurs here for I2C
        // put your RX code here.
        RXData[flag] = UCB0RXBUF;
        TXData[flag] = RXData[flag];
        P1OUT ^= 0x04;
        newprint = 1;
        flag = flag + 1;

        if (flag == 8){
            rcv_1 = ((long)RXData[3]<<24) + ((long)RXData[2]<<16) + ((long)RXData[1]<<8) + ((long)RXData[0]);
            rcv_2 = ((long)RXData[7]<<24) + ((long)RXData[6]<<16) + ((long)RXData[5]<<8) + ((long)RXData[4]);

            if((rcv_2&0x01)==0x01){
                greenLight = 1;
            }


            //TBCCR1 = rcv_1;
            //dont care what we send right now
            snd_1 = state;//TBCCR1;
            snd_2 = state;//TBCCR1;

            //TBCCR1 = rcv_1;
            TXData[0] = (unsigned char)(snd_1 >>0);
            TXData[1] = (unsigned char)(snd_1 >>8);
            TXData[2] = (unsigned char)(snd_1 >>16);
            TXData[3] = (unsigned char)(snd_1 >>24);
            TXData[4] = (unsigned char)(snd_2 >>0);
            TXData[5] = (unsigned char)(snd_2 >>8);
            TXData[6] = (unsigned char)(snd_2 >>16);
            TXData[7] = (unsigned char)(snd_2 >>24);
            IE2 &= ~UCB0RXIE;
            IE2 |= UCB0TXIE;
            P1OUT ^= 0x10;
            flag = 0;

        }






        //IFG2 &= ~UCB0RXIFG;

    } else if ((IFG2&UCB0TXIFG) && (IE2&UCB0TXIE)) { // USCI_B0 TX interrupt
        // put your TX code here.

        UCB0TXBUF = TXData[flag];
        P1OUT ^= 0x08;
        //IFG2 &= ~UCB0TXIFG;
        flag = flag + 1;
        if (flag == 8){
            IE2 &= ~UCB0TXIE;
            IE2 |= UCB0RXIE;
            P1OUT ^= 0x20;
            flag = 0;
        }
    }
}


// USCI Receive ISR - Called when shift register has been transferred to RXBUF
// Indicates completion of TX/RX operation
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void) {

    if((IFG2&UCA0RXIFG) && (IE2&UCA0RXIE)) {  // USCI_A0 requested RX interrupt (UCA0RXBUF is full)

        if(!started) {  // Haven't started a message yet
            if(UCA0RXBUF == 253) {
                started = 1;
                newmsg = 0;
            }
        } else {    // In process of receiving a message
            if((UCA0RXBUF != 255) && (msgindex < (MAX_NUM_FLOATS*5))) {
                rxbuff[msgindex] = UCA0RXBUF;

                msgindex++;
            } else {    // Stop char received or too much data received
                if(UCA0RXBUF == 255) {  // Message completed
                    newmsg = 1;
                    rxbuff[msgindex] = 255; // "Null"-terminate the array
                }
                started = 0;
                msgindex = 0;
            }
        }
        IFG2 &= ~UCA0RXIFG;
    }

    if((UCB0I2CIE&UCNACKIE) && (UCB0STAT&UCNACKIFG)) { // I2C NACK interrupt

        UCB0STAT &= ~UCNACKIFG;
    }
    if((UCB0I2CIE&UCSTPIE) && (UCB0STAT&UCSTPIFG)) { // I2C Stop interrupt

        UCB0STAT &= ~UCSTPIFG;
    }
    if((UCB0I2CIE&UCSTTIE) && (UCB0STAT&UCSTTIFG)) { //  I2C Start interrupt

        UCB0STAT &= ~UCSTTIFG;
    }
    if((UCB0I2CIE&UCALIE) && (UCB0STAT&UCALIFG)) {  // I2C Arbitration Lost interrupt

        UCB0STAT &= ~UCALIFG;
    }
}

Custom parts and enclosures

Motor Holder
Edited Motor Holder
LED Cover
Cover for LED so readings are not as affected by environment light.
Servo Motor Attachement
Used to turn shaft
Camera Pole
Used to adjust camera height
Camera Base
Used to secure camera pole
Shaft Attachment
Glued to shaft and interlocks with servo motor attachment

Sketchfab still processing.

Motor Holder
Holds the servo motor that rotates the waffle iron

Schematics

System Diagram
Waffleschematic gc6yaqcjyk

Comments

Similar projects you might like

MeArm Robot Arm - Your Robot - V1.0

Project tutorial by Benjamin Gray

  • 29,567 views
  • 4 comments
  • 42 respects

Amazing 6WD Off-Road Robot | Arduino RC Robot

Project tutorial by Jithin Sanal

  • 9,978 views
  • 0 comments
  • 54 respects

Scriba Robot - A Printing Robot

Project in progress by RobinB

  • 2,545 views
  • 0 comments
  • 8 respects

Asi (Anansi) Robot Companion

Project tutorial by Jorvon Moss

  • 10,754 views
  • 10 comments
  • 131 respects

Joy Robot (Robô Da Alegria)

Project tutorial by Igor Fonseca Albuquerque

  • 4,648 views
  • 6 comments
  • 34 respects

Project 1: 2WD Obstacle Avoiding Robot

Project showcase by HDA Robotics

  • 35,165 views
  • 16 comments
  • 42 respects
Add projectSign up / Login