struct field_outline_t form_getFieldFormInfo
(struct self_pos_t *absolute_locate, const struct wheel_speed_t go_forward){
struct xy_coord_t current_bump_pos, before_bump_pos;
struct xy_coord_t start_corner_pos, first_corner_pos, second_corner_pos;
struct field_outline_t field_info;
int corner_counter = 0;
int loop_counter = 0;
int judge_bump;
int judge_corner_flag;
int approach_corner_angle[CORNER_NUM];
int draw_count = 0;
char *fname1 = "field_form_coord.txt";
FILE *fp1;
/* 外壁情報取得開始時の隅の座標の取得 */
start_corner_pos.x = absolute_locate->pos.x;
start_corner_pos.y = absolute_locate->pos.y;
/* 衝突位置座標の初期化 */
before_bump_pos.x = current_bump_pos.x = 0;
before_bump_pos.y = current_bump_pos.y = 0;
while(1){
/* createを直進させる */
directDrive(go_forward.vel_left, go_forward.vel_right);
getCurrentSelfPos(absolute_locate, go_forward);
if(loop_counter % DRAW_COUNT_RANGE == 0){
filePrintCoord(&fp1, fname1, "a", absolute_locate->pos.x, absolute_locate->pos.y);
printf("%d, %d %d\n", absolute_locate->pos.x, absolute_locate->pos.y, absolute_locate->theta);
}
/* createが衝突したかを判定する */
/* getBumpsAndWheelDropsはcreateが衝突したならば以下の値を返す */
/* 右側のbumpsensorが衝突を検知->1 左側のbumpsensor->2 両方のbumpsensorに衝突を検知->3 */
judge_bump = getBumpsAndWheelDrops();
if(judge_bump == 1 || judge_bump == 2 || judge_bump == 3){
current_bump_pos = form_getBumpPos(absolute_locate);
/*隅に到達したならば、judge_corner_flagを立てる*/
judge_corner_flag = form_judgeCorner(current_bump_pos, before_bump_pos);
if(judge_corner_flag == TRUE){
corner_counter++;
printf("corner_num = %d\n\n\n\n", corner_counter);
form_resetAbsoluteTheta(corner_counter, absolute_locate);
approach_corner_angle[corner_counter] = absolute_locate->theta;/* 隅に進入時のcreateの姿勢角度を保存 */
/*
一つ目及び２つ目の隅を見つけた際にフィールドの縦横の長さを計算する
フィールドの形は平行四辺形との規定があるため、２辺の長さしか算出して
いない。
*/
switch(corner_counter){
case 1:
first_corner_pos = form_getCornerPos(absolute_locate);
field_info.virtical_length = form_getFieldLength(start_corner_pos, first_corner_pos);
break;
case 2:
second_corner_pos = form_getCornerPos(absolute_locate);
field_info.side_length = form_getFieldLength(first_corner_pos, second_corner_pos);
break;
case CORNER_NUM:
field_info.corner_angle = form_setEachCornerAngle(approach_corner_angle);
robot_Stop_Back_Stop(1, BACK_DISTANCE, absolute_locate);
absolute_locate->theta = robot_Turn_Stop(TARGET_ANGLE_5, CORRECTION_ANGLE_5, turn_clockwise_100, absolute_locate->theta);
return field_info;
default:
break;
}
}
robot_Stop_Back_Stop(0.2, BACK_DISTANCE, absolute_locate);
absolute_locate->theta = robot_Turn_Stop(TARGET_ANGLE_5, CORRECTION_ANGLE_5, turn_clockwise_100, absolute_locate->theta);
before_bump_pos = current_bump_pos;
}
countCicleTime();
draw_count++;
}
}
int main(int argc, char** argv){
int port_number;
int client_length;
struct sockaddr_in server_address, client_address;
struct hostent *host_properties;
char buffer[BUFFER_SIZE];
char *host_address;
int message_size;
int message_byte_size;
const char delimiters[ ] = ":";
port_number = atoi( argv[ 2 ] );
//-- create the socket
printf( "creating udp socket...\n" );
socket_descriptor = socket(AF_INET, SOCK_DGRAM, 0);
if ( socket_descriptor < 0 )
perror( "Error opening the socket" );
//-- quick trick to avoid error on binding
int optval = 1;
setsockopt( socket_descriptor, SOL_SOCKET, SO_REUSEADDR, ( const void * )&optval, sizeof( int ) );
//-- set server internet address
bzero( ( char * )&server_address, sizeof( server_address ) );
server_address.sin_family = AF_INET;
server_address.sin_addr.s_addr = inet_addr( argv[ 1 ] );
server_address.sin_port = htons( ( unsigned short )port_number );
//-- bind socket to port
if( bind( socket_descriptor, ( struct sockaddr * )&server_address, sizeof( server_address ) ) < 0 )
perror( "Error binding socket to port" );
//-- if nothing has failed up to this point, let's start the connection to the create robot
printf( "connecting to irobot create on %s...\n", argv[ 3 ] );
startOI_MT( argv[ 3 ] );
//-- the main loop !
client_length = sizeof( client_address );
while( 1 ){
//-- receive the packets from the iphone app
bzero( buffer, BUFFER_SIZE );
message_byte_size = recvfrom( socket_descriptor, buffer, BUFFER_SIZE, 0, ( struct sockaddr * )&client_address, &client_length );
if( message_byte_size < 0 )
perror("Error in recvfrom");
printf("%s\n", buffer);
fflush(stdout);
//-- split the control instruction and extract left wheel velocity and right wheel velocity
char *left;
char *right;
left = strtok( buffer, delimiters ); // left = "Left";
left = strtok( NULL, delimiters ); // left = string with actual value for left wheel velocity
right = strtok( NULL, delimiters ); // right = "Right"
right = strtok( NULL, delimiters ); // right = string with actual value for right wheel velocity
int left_velocity = atoi( left );
int right_velocity = atoi( right );
//-- quick fix so that if the velocities of the wheels are too close
// together then set them to the same, this should be handled on the
// iphone code.
if( abs( left_velocity - right_velocity ) < ERROR_MARGIN )
left_velocity = right_velocity;
//-- send command to the robot
directDrive( left_velocity, right_velocity );
}
//-- stop robot
stopOI_MT();
}
