process.cpp

#include "process.h"
#include "serialport.h"
#include "pthread.h"
#include "global.h"
#include "vel_optimal.h"
#include "string.h"
#include "stack"
#include "math.h"

using namespace std; 	

// receive sensor data and fill them in sensor[]
// port 1 ===> ttyUSB0
void* sensor_data(void* argv)
{
	int cnt = 0, nread = 0;
	int state = 0;
	//int data_from = 0; 		// judge frame, 0 is sensor data and 1 is odometer data
	STATUS finish = FALSE;
	int fd;
	int timer = 0;
	unsigned char frame[128], buf[1024];
	fd = openserial((char*)"/dev/ttyUSB0", 115200, 8, 'N', 1);
	if(fd == -1)
	{
		printf("open ttyUSB0 failed!\n");
		//return;
	}
	else
	{
		//printf("open ttyUSB0 success!\n");
		while(1)
		{
			//printf("in while...\n");
		    if((nread = read(fd, buf, 1024))>1)
		    {
		    	//printf("\ndata receive! nread = %d\n", nread);
		    	for(int i=0; i<nread; i++)
		    	{
		    		//printf("-%2x-", buf[i]);
		    		
		    		switch(state)
			    	{
			    		case H1_SENSOR:
			    			//printf("H1_SENSOR\n");
			    			if( 'S' == buf[i] )
			    			{
			    				state++;
			    				//printf("S");
			    			}
			    			else
			    				state = 0;
			    			break;
			    		case H2_SENSOR:
			    			//printf("H2_SENSOR\n");
			    			if( 'J' == buf[i] )
			    			{
			    				state++;
			    				//printf("J"9);
			    			}
			    				
			    			else
			    				state = 0;
			    			break;
			    		case E1_SENSOR:
			    			if( 'T' == buf[i] )
			    			{
			    				//printf("T");
			    				state++;
			    			}
			    				
			    			else
			    				state = 0;
			    			break;
			    		case E2_SENSOR:
			    			if( 'U' == buf[i] )
			    			{
			    				//printf("\n");
			    				//state ++;
			    				finish = TRUE;
			    			}
			    			else
			    				state = 0;
			    			break;
			    		default:

			    			frame[state - 2] = buf[i];
			    			state++;
			    			break;

			    	} 
					
			    	 if ( TRUE == finish )
				    {
				    	pthread_mutex_lock(&m_sensor);
				    	for(int k=0; k<SENSOR_NUM; k++)
				    	{
				    		sensor_frame[k] = frame[2*k] * 256 + frame[2*k+1];
				    		if(13 == k || 14 == k)
				    		{
				    			sensor_frame[k] += 120;
				    			sensor_frame[k] *= 2;
				    		}
				    			
				    		//printf("%d ", sensor_frame[k]);
				    	}
				    	printf("sensor data receive....\n");
				    	finish = FALSE;
				    	state = 0;
				    	pthread_cond_signal(&sensor_update);  //send signal 
				    	pthread_mutex_unlock(&m_sensor);

				    	/*printf("sensor_frame update... timer: %d\n", timer++);
					    for(int k=0; k<15; k++)
					    {
					    	printf("%d ", sensor_frame[k]);
					    }
					    printf("\n");*/
				    }
		    	}
		    	memset(buf, 0, sizeof(buf));
		    	
		    }

		    

  		}
  			close(fd);
			printf("Process A is here!\n");
			test = cnt++;
			sleep(1);
	}
	
}

// receive odomtry data and fill them in odometry[]
// port 1 ===> ttyUSB1
void* odometry(void* argv)
{
	int cnt = 0, nread = 0;
	int state = 0;
	//int data_from = 0; 		// judge frame, 0 is sensor data and 1 is odometer data
	STATUS finish = FALSE;
	int fd;
	int timer = 0;
	unsigned char frame[128], buf[512];
	fd = openserial((char*)"/dev/ttyUSB1", 19200, 8, 'N', 1);
	odometry_port = fd;			// copy id to odometry_port used by send_command process
	if(fd == -1)
	{
		printf("open ttyUSB0 failed!\n");
		//return;
	}
	else
	{
		//printf("open ttyUSB0 success!\n");
		while(1)
		{
			//printf("in while...\n");
		    if((nread = read(fd, buf, 512))>0)
		    {
		    	//printf("data receive! nread = %d\n", nread);
		    	for(int i=0; i<nread; i++)
		    	{
		    		//printf("-%x-", buf[i]);
		    		switch(state)
			    	{
			    		case H1_ODOMETRY:
			    			if( 0xee == buf[i] )
			    			{
			    				state++;
			    				//printf("S");
			    			}
			    			else
			    				state = 0;
			    			break;
			    		case H2_ODOMETRY:
			    			if( 0xaa == buf[i] )
			    			{
			    				state++;
			    				//printf("J");
			    			}
			    				
			    			else
			    				state = 0;
			    			break;
			    		case E1_ODOMETRY:
			    			if( 0x00 == buf[i] )
			    			{
			    				//printf("T");
			    				state++;
			    			}
			    				
			    			else
			    				state = 0;
			    			break;
			    		case E2_ODOMETRY:
			    			if( 0xbb == buf[i] )
			    			{
			    				//printf("U\n");
			    				//state ++;
			    				finish = TRUE;
			    			}
			    			else
			    				state = 0;
			    			break;
			    		default:

			    			frame[state - 2] = buf[i];
			    			state++;
			    			break;

			    	} 

			    	 if ( TRUE == finish )
				    {
				    	printf("odometry finish............\n");
				    	pthread_mutex_lock(&m_odometry);
				    	memcpy((void*)odometry_frame, frame, ODOMETRY_LEN);
				    	short int temp = 0;
				    	temp = frame[0];
				    	temp = temp<<8;
				    	temp |= frame[1];
				    	robot_cur_pos.x = temp;
				    	temp = frame[2];
				    	temp = temp<<8;
				    	temp |= frame[3];
				    	robot_cur_pos.y = temp;
				    	temp = frame[4];
				    	temp = temp<<8;
				    	temp |= frame[5];
				    	robot_cur_pos.theta = temp;
				    	printf("robot(%d, %d, %d)\n", (int)robot_cur_pos.x, (int)robot_cur_pos.y, (int)robot_cur_pos.theta);
				    	finish = FALSE;
				    	state = 0;
				    	//pthread_cond_signal(&odometry_update); //send signal
				    	pthread_mutex_unlock(&m_odometry);
				    }
		    	}
		    	
		    }
  		}
  			close(fd);
			printf("Process A is here!\n");
			test = cnt++;
			sleep(1);
	}
}
void* process(void* argv)
{
	stack<POSITION> robot;				 // store robot global position 
	stack<POSITION> target;
	POSITION target_cur_pos, target_global_pos;
	POSITION error_pos;
	STATUS target_state;
	int timeout = 0;
	int sensor[2] = {0};
while(1)
{
	{
		// check whether target is available
		pthread_mutex_lock(&m_sensor);			// lock sensor data
		pthread_cond_wait(&sensor_update, &m_sensor); // wait for contidion come true
		sensor[0] = (sensor[0]+sensor_frame[14])/2;
		sensor[1] = (sensor[1]+sensor_frame[13])/2;
		if( abs(sensor[0]) > TRACK_DIS_OUT || abs(sensor[1]) > TRACK_DIS_OUT )
		{
			target_state = FALSE;	// target is lost
			timeout++;				// counts timeout
			if(timeout < TIMEOUT)	// if it is not tiemout, keep following by history state
			{
				if(!robot.empty() && !target.empty())  // if there is history state, following history state
				{
					error_pos.x = target.top().x - robot.top().x;
					error_pos.y = target.top().y - robot.top().y;
					error_pos.theta = target.top().theta - robot.top().theta;
					// pridiction can be added here
					following(error_pos);
				}
				else
					stop_wheelchair();
			}
			else
				stop_wheelchair();
		}
		else						// if there is target, following target and update target_global_pos
		{
			timeout = 0;			// clear timeout indicating target is available now
			target_state = TRUE;	
			target_cur_pos = target_pos(sensor[0], sensor[1]);
			printf("target(%d,%d,%d)\n", (int)target_cur_pos.x,(int)target_cur_pos.y, (int)target_cur_pos.theta);
			pthread_mutex_lock(&m_odometry);			// lock robot_cur_pos data
			//pthread_cond_wait(&odometry_update, &m_odometry);
			target_global_pos.x = robot_cur_pos.x + target_cur_pos.x;
			target_global_pos.y = robot_cur_pos.y + target_cur_pos.y;
			target_global_pos.theta = robot_cur_pos.theta + target_cur_pos.theta;
			pthread_mutex_unlock(&m_odometry);			// lock robot_cur_pos data
			if(!target.empty())
				target.pop();
			target.push(target_global_pos);		//update target position information
			// prediction can be added here
			following(target_cur_pos);
		}
		pthread_mutex_unlock(&m_sensor);			// lock sensor data
		
	}
}
}
void lost_following()
{
	/*
	POSITION error_pos;
	if(!robot.empty())
	{
		error_pos = target.topd() - robot.top();
		// pridiction can be added here
		following(error_pos);
	}
	else
		stop_wheelchair();
	return;
	*/
}
void normal_following()
{
	/*
	POSITION target_cur_pos;
	POSITION target_global_pos;
	target_cur_pos = target_pos(sensor_frame[SENSOR_NUM - 3], sensor_frame[SENSOR_NUM - 2]);
	target_global_pos.x = robot.top().x + target_cur_pos.x;
	target_global_pos.y = robot.top().y + target_cur_pos.y;
	target_global_pos.theta = robot.top().theat + target_cur_pos.theta;
	target.pop();
	target.push(target_global_pos);		//update target position information
	// prediction can be added here
	following(target_cur_pos);
	return;
	*/
}
// caculate command according to error_pos
void following(POSITION error_pos)
{
	int 	err_dis, err_theta, i, cnt = 0;
	float   repul_force;
	float 	x_component = 0, y_component = 0;
	float   velocity_cmd, rotate_cmd, temp;

	int test;

	err_dis = sqrt(error_pos.x * error_pos.x + error_pos.y * error_pos.y);
	err_theta = error_pos.theta;
	printf("err(%d, %d)\n", err_dis, err_theta);

	if(err_dis < REFER_DIS && abs(err_theta) <REFER_ANG)
	{
		stop_wheelchair();
		return;
	}
	//pthread_mutex_lock(&m_sensor);			// sensor data sync
	
	for( i=0; i<13; i++ )		// if it is dangerous, generate repulsive force for command modification
	{
		if(sensor_frame[i] < WARNING && sensor_frame[i] > DANGER)
		{
			x_component += (1.0*(WARNING - sensor_frame[i])/(WARNING - DANGER) * (WARNING - sensor_frame[i])/(WARNING - DANGER))\
						* cos((-15*i - 90)/180.0*PI) * MAXSPEED;
			y_component += (1.0*(WARNING - sensor_frame[i])/(WARNING - DANGER) * (WARNING - sensor_frame[i])/(WARNING - DANGER))\
						* sin((-15*i - 90)/180.0*PI) * MAXSPEED;
			cnt ++;
		}
		else if (sensor_frame[i] < DANGER)
		{
			x_component += cos((-15*i -180)/180.0*PI) *NEGINF;
			y_component += sin((-15*i -180)/180.0*PI) *NEGINF;
			cnt++;
		}
		else
		{
			x_component += 0;
			y_component += 0;
		}
	}
	x_component = x_component/cnt*Kp + cos((error_pos.theta)/180.0*PI)*(1-Kp)*err_dis;
	y_component = y_component/cnt*Kp + sin((error_pos.theta)/180.0*PI)*(1-Kp)*err_dis;
	
	temp = sqrt(x_component*x_component + y_component*y_component);
	velocity_cmd = temp;
	rotate_cmd = asin( y_component/temp )*180.0/PI; 	
	//sensor_frame_update = FALSE;
	//pthread_mutex_unlock(&m_sensor);	// unlock sensor data

	pthread_mutex_lock(&m_command);//copy data to local
	command[0] = velocity_cmd;
	command[1] = rotate_cmd;
	//command_update = TRUE;
	pthread_cond_signal(&command_update);
	pthread_mutex_unlock(&m_command);
	//send_command();
	return;
}
void stop_wheelchair(void)
{
	pthread_mutex_lock(&m_command);//copy data to local
	command[0] = 0;
	command[1] = 0;
	//command_update = TRUE;
	pthread_cond_signal(&command_update);  //send signal
	pthread_mutex_unlock(&m_command);
	//pthread_mutex_unlock(&m_sensor);
	//sensor_frame_update = FALSE;
	return;
}

void* send_command(void* argv)
{
	int local_cmd[2] = {0};
	unsigned char buf_cmd[] ={0xee,0xaa,0x01,0x00,0x00,0x01,0x00,0x00,0x02,0x00,0x00,0xbb, '\n'};
							// 0	1	 2	  3		4	5	 6	  7	   8	9	 10	  11
	if(-1 == odometry_port)
	{
		while(1)
		printf("send command serial port error!\n");

		//return;
	}
	while(1)
	{

		//while( FALSE == command_update ) // wait command updates
		{
			// printf("[test]command update:%d\n", command_update);
			//printf("*************************\n");
		//	usleep(1000);
		}
		//printf("send command process\n");
		// usleep(150000);
		pthread_mutex_lock(&m_command);// lock command[2]
		pthread_cond_wait(&command_update, &m_command);
		//command[0] = 257;
		//command[1] = 0;
		local_cmd[0] = command[0];//(command[0] + local_cmd[0])/2;
		local_cmd[1] = command[1];//(command[1] + local_cmd[1])/2;
		
		printf("command: %d %d\n", command[0], command[1]);
		//local_cmd[0] = 500;//command[0];//command[0];
		//local_cmd[1] = command[1];
		if(local_cmd[0]<0)
		{
			buf_cmd[2] = 0x02;
			local_cmd[0] = -local_cmd[0];
			//printf("negative\n");
		}
		else
		{
			buf_cmd[2] = 0x01;
		}
		if(local_cmd[1]<0)
		{
			buf_cmd[5] = 0x02;
			local_cmd[1] = -local_cmd[1];
		}
		else
		{
			buf_cmd[5] = 0x01;
		}
		if(abs(local_cmd[1]) <10 || abs(local_cmd[1]) > MAXROTATE)
			local_cmd[1] = 0;
		if(abs(local_cmd[0]) > MAXSPEED)
			local_cmd[0] = 400;
	
		buf_cmd[4] = local_cmd[0]%256;
		buf_cmd[3] = local_cmd[0]/256;
		buf_cmd[7] = local_cmd[1]%256;
		buf_cmd[6] = local_cmd[1]/256;
		write(odometry_port, &buf_cmd, sizeof(buf_cmd)); // send command to wheelchair
		printf("(v = %d r = %d)\n", buf_cmd[3]*256+buf_cmd[4], buf_cmd[6]*256+buf_cmd[7]);	
		//printf("org --%2x--%2x--%2x--%2x--\n", buf_cmd[3],buf_cmd[4],buf_cmd[6],buf_cmd[7]);
	//	command_update = FALSE;
		pthread_mutex_unlock(&m_command);// lock command[2]
	}
}
/*
{
	int sensor[15];
	STATUS copy = FALSE;
	POSITION target_pos_current;
	POSITION target_pos_last;
	POSITION obstacle_pos;
	STATE target_state;

	initial_vset(VSET);

	while(1)
	{
		pthread_mutex_lock(&m_odometry);//copy data to local
		if(1 == frame_update)
		{
			for(int i=0; i<15; i++)
			{
				sensor[i] = sensor_frame[i];
			}
			frame_update = FALSE;
			copy = TRUE;
		}
		pthread_mutex_unlock(&m_odometry);
		if(TRUE == copy)
		{
			printf("after copy...\n");
			for(int i=0; i<15; i++)
			{
				printf("%d ", sensor[i]);
			}
			printf("\n");
			copy = FALSE;
			//target_pos_last = target_pos_current;
			//target_pos_current = target_pos(sensor[13], sensor[14]);
			//printf("target pos: x=%f y=%f theta=%f\n", target_pos_current.x, target_pos_current.y, target_pos_current.theta);
			//belows are for testing
			target_pos_last.x = 500;
			target_pos_last.y = 500;
			target_pos_current.x = 1000;
			target_pos_current.y = 1000;
			target_state = predictor(target_pos_last, target_pos_current);
			printf("target_state: vel = %f ort =%f \n", target_state.velocity, target_state.orient*180/3.14);
		}

	}
}
*/