コード例 #1
0
ファイル: SensorAPastRead.c プロジェクト: anjinkristou/SSM
int main(int argc, char *argv[]) {
	SensorA sens_data;
	SSM_sid sensA_sid;
	double measured_time;
	int tid;
	double now_time;

	initSSM();
	sensA_sid = openSSM("sensor_A", 0, 0);

	while (1) {
		now_time = gettimeSSM(); //現在時刻を取得

		//現在時刻の1秒前のデータを取得
		tid = readSSM_time(sensA_sid, (char*) &sens_data, now_time - 1,
				&measured_time);
		printf("nowtime=%f\n   time=%f tid=%d a=%f b=%f c=%d\n", gettimeSSM()
				- 1, measured_time, tid, sens_data.a, sens_data.b, sens_data.c);
		//その一つ後のデータを取得
		//tid++;
		tid = readSSM(sensA_sid, (char*) &sens_data, &measured_time, tid + 1);
		printf("   time=%f tid=%d a=%f b=%f c=%d\n\n", measured_time, tid,
				sens_data.a, sens_data.b, sens_data.c);

		sleep(1);
	}
}
コード例 #2
0
	/// @brief timeを指定して書き込み
	/// @param time[in] 時間。指定しないときは現在時刻を書き込み
	/// @return 正しく書き込めたときtrueを返す
	bool write( ssmTimeT time = gettimeSSM() )
	{
		if( !isOpen(  ) )
			return false;
		int tid;
		tid = writeSSMP( ssmId, &data, time, ssm::SOKUIKIData3D::_ssmWrite, NULL );
		if(tid >= 0){timeId = tid; this->time = time; return true;}
		return false;
	}
コード例 #3
0
ファイル: SensorBHandler.c プロジェクト: anjinkristou/SSM
void receive_sensorB(SensorB *data, double *time) {
	static double a;
	static int b;
	a += 0.1;
	b++;
	data->a = a;
	data->b = b;
	*time = gettimeSSM();
	usleep(50000);
}
コード例 #4
0
ファイル: connect.c プロジェクト: dnsd/idoor2
int main ()
{
    struct sockaddr_in dstAddr;
    int dstSock;
    char c;
    char raw_start_buf[26];
    char raw_line_buf[20][1113];
    char raw_end_buf[33];
    char *line_buf[22];
    char str_buf[5];
    char det;
    long dist[20][137];
    int n;
    int numrcv;
    char *toSendText1 = "#TD2GET\n";
    char *toSendText2 = "#TD2STOP\n";

    FILE *fp; //ファイルポインタ

    int i,j,k;

    //-角度用-//
    long U_x_beta[20][136];
    long U_y_beta[20][136];
    long D_x_beta[20][136];
    long D_y_beta[20][136];
    double U_angle_x[20][136];
    double U_angle_y[20][136];
    double D_angle_x[20][136];
    double D_angle_y[20][136];

    //-時間計測用-//
    double time_1; //時間計測用
    double time_2; //時間計測用
    double time_3; //時間計測用

    //-座標値-//
    double x;

    double angle_convert(long angle); //プロトタイプ宣言
    void calc_xyz(double angle_x, double angle_y, long dist, double *x, double *y, double *z); //プロトタイプ宣言
    double get_time(void); //プロトタイプ宣言

    //-SSM-//
    LS3D SCAN_DATA;
    initSSM();
    SSM_sid LS3D_sid;

    LS3D_sid = createSSM_time("LS3D", 0, sizeof(LS3D), 1.0, 0.025);

    //-SCAN_DATAの初期化-//
    // for(i=0; i<2880; i++)
    for(i=0; i<STEP_NUM; i++)
    {
        SCAN_DATA.det = 'U';
        SCAN_DATA.dist[i] = 0.0;
        SCAN_DATA.x[i] = 0.0;
        SCAN_DATA.y[i] = 0.0;
        SCAN_DATA.z[i] = 0.0;
    }

    //-----------角度データ読み込み-----------//
    //-ファイルオープン-//
    fp = fopen("beta_data", "r");
    for(j=0; j<20; j++)
    {
        for(i=0; i<136; i++)
        {
            fscanf(fp, "%ld", &U_x_beta[j][i]);
            U_angle_x[j][i] = angle_convert(U_x_beta[j][i]);
        }
    }
    for(j=0; j<20; j++)
    {
        for(i=0; i<136; i++)
        {
            fscanf(fp, "%ld", &U_y_beta[j][i]);
            U_angle_y[j][i] = angle_convert(U_y_beta[j][i]);
        }
    }
    for(j=0; j<20; j++)
    {
        for(i=0; i<136; i++)
        {
            fscanf(fp, "%ld", &D_x_beta[j][i]);
            D_angle_x[j][i] = angle_convert(D_x_beta[j][i]);
        }
    }
    for(j=0; j<20; j++)
    {
        for(i=0; i<136; i++)
        {
            fscanf(fp, "%ld", &D_y_beta[j][i]);
            D_angle_y[j][i] = angle_convert(D_y_beta[j][i]);
        }
    }

    //-----------距離データ取得ループ-----------//
    //-ソケットの作成-//
    dstSock = socket(AF_INET, SOCK_STREAM, 0);

    //-接続先指定用構造体の準備-//
    dstAddr.sin_family = AF_INET;
    dstAddr.sin_port = htons(10940);
    dstAddr.sin_addr.s_addr = inet_addr("192.168.0.10");

    //-コネクト-//
    connect(dstSock, (struct sockaddr *)&dstAddr, sizeof(dstAddr));

    //-サーバにメッセージを送信-//
    memset(&dstAddr, 0, sizeof(dstAddr));
    send(dstSock, toSendText1, strlen(toSendText1)+1, 0);
    sleep(1);
    printf("sending...\n");

    //-サーバからデータを受信-//
    memset(raw_start_buf, 0, sizeof(raw_start_buf));
    memset(raw_line_buf, 0, sizeof(raw_line_buf));
    memset(raw_end_buf, 0, sizeof(raw_end_buf));

    while(1){ //距離データ取得ループ
        //time_1 = get_time();

        for(i=0; i<26; i++)
        {
            numrcv = recv(dstSock, &c, 1, 0);
            if(c=='\n')
            {
                raw_start_buf[i] = '\0';
                break;
            }
            raw_start_buf[i] = c;
            det = raw_start_buf[19]; //ループの外だとなぜか文字が消える
        }

        for(j=0; j<20; j++)
        {
            for(i=0; i<1177; i++)
            {
                numrcv = recv(dstSock, &c, 1, 0);
                if(c=='\n'){
                    raw_line_buf[j][i] = '\0';
                    break;
                }
                raw_line_buf[j][i] = c;
            }
        }

        for(i=0; i<33; i++)
        {
            numrcv = recv(dstSock, &c, 1, 0);
            if(c=='\n')
            {
                raw_end_buf[i] = '\0';
                break;
            }
            raw_end_buf[i] = c;
        }

        //-距離データを抽出-//
        for(i=0; i<20; i++)
        {
            char *tp;
            tp = strtok(&raw_line_buf[i][0], "#:");
            for(j=0; j<3; j++)
            {
                tp = strtok(NULL, "#:");
            }
            line_buf[i] = tp;
        }


        for(i=0; i<20; i++)
        {
            if(line_buf[i] != NULL)
            {
                for(j=0; j<136; j++)
                {
                    str_buf[0] = *(line_buf[i] + 8*j);
                    str_buf[1] = *(line_buf[i] + 8*j + 1);
                    str_buf[2] = *(line_buf[i] + 8*j + 2);
                    str_buf[3] = *(line_buf[i] + 8*j + 3);
                    str_buf[4] = '\0';

                    dist[i][j] = strtol(str_buf, NULL, 16);
                }
            }
        }

        //-xyz座標値に変換-//
        if(det == 'U')
        {
            SCAN_DATA.det = 'U';
            for(i=0; i<20; i++)
            {
                for(j=0; j<136; j++)
                {
                    calc_xyz(U_angle_x[i][j], U_angle_y[i][j], dist[i][j], &SCAN_DATA.x[i*136 + j], &SCAN_DATA.y[i*136 + j], &SCAN_DATA.z[i * 136 + j]);
                }
            }
        }else if(det == 'D'){
            SCAN_DATA.det = 'D';
            for(i=0; i<20; i++)
            {
                for(j=0; j<136; j++)
                {
                    calc_xyz(D_angle_x[i][j], D_angle_y[i][j], dist[i][j], &SCAN_DATA.x[i*136 + j], &SCAN_DATA.y[i*136 + j], &SCAN_DATA.z[i * 136 + j]);
                }
            }
        }

        //-SSM書き込み-//
        for(i=0; i<20; i++)
        {
            for(j=0; j<136; j++)
            {
                SCAN_DATA.dist[136*i + j] = dist[i][j];
            }
        }

        printf("%c\n", SCAN_DATA.det);

        writeSSM(LS3D_sid, (char*)&SCAN_DATA, gettimeSSM());

        usleep(1000);

        //time_2 = get_time();
        //usleep(freq*1000000 - (time_2 - time_1)*1000000);
        //time_3 = get_time();

    } //距離データ取得ループ

    //-計測停止要求-//
    send(dstSock, toSendText2, strlen(toSendText2)+1, 0);

    //-socketの終了-//
    close(dstSock);

    return 0;
}