コード例 #1
0
ファイル: lidarOnly.c プロジェクト: Adeange1/roboboat2015
/* Function: mdlOutputs =======================================================
 *
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    int32_T        *y0  = (int32_T *)ssGetOutputPortRealSignal(S,0);
    
      // Request Data 
      ret = urg_requestData(&urg, URG_GD, 0, 1080);
      if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
      }

      // Receive Data
      n = urg_receiveData(&urg, data, 1080);
      // Error, can't receive negative data points
        if (n < 0) {
            urg_exit(&urg, "urg_receiveData()");
	    // Disconnect Lidar 
    		urg_disconnect(&urg);
   
        } 

      // Data Received, Construct Partial Image 
        else{ 
          	for(k=0;k<1080;k++){ 
            	y0[k]=data[k];   
          	}
        }
    //lidarOnly_Outputs_wrapper(y0);
      
      printf(data);


}
コード例 #2
0
/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    //Move stepper to zero, disconnect lidar and stepper
    stepperMove(0, stepper);
    urg_disconnect(&urg);
    stepperDisconnect(stepper);
}
コード例 #3
0
ファイル: urg_ctrl.c プロジェクト: MuiLe/copter
int urg_reboot(urg_t *urg)
{
  int expected_ret[][2] = {
    { 1, -1 },
    { 0, -1 },
  };
  int send_n;
  int recv_n;
  int i;

  urg_laserOff(urg);

  /* send RB twice */
  for (i = 0; i < 2; ++i) {
    send_n = scip_send(&urg->serial_, "RB\n");
    if (send_n != 3) {
      return SerialSendFail;
    }

    recv_n = scip_recv(&urg->serial_, "RB", NULL,
                       expected_ret[i], ScipTimeout);
    if (recv_n < 0) {
      return recv_n;
    }
  }

  /* disconnect immediately */
  urg_disconnect(urg);

  return 0;
}
コード例 #4
0
void urg_exit(urg_t *urg, const char *message){
  printf("%s: %s\n", message, urg_error(urg));
  urg_disconnect(urg);

  #ifdef MSC
      getchar();
  #endif
}
コード例 #5
0
/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S) {
    //Move stepper to zero, disconnect lidar and stepper
    stepperMove(0, stepper);
    urg_disconnect(&urg);
    stepperDisconnect(stepper);
    //Disconnect From Target Camera
    cvReleaseCapture( &capture1 );
}
コード例 #6
0
int	 CSCIPRadarComm::CloseRadar(void)
{
	if(urg.is_laser_on_==UrgLaserOn)
	{
		//具体发送命令去控制雷达停止
		urg_disconnect(&urg);
	}
	return 1;
}
コード例 #7
0
ファイル: get_version_lines.c プロジェクト: ppalma/URG-GL
int main(int argc, char *argv[])
{
  enum {
    LinesMax = 5,
  };

  //const char device[] = "COM3"; /* Example for Windows */
  const char device[] = "/dev/ttyACM0"; /* Example for Linux */

  char buffer[LinesMax][UrgLineWidth];
  char *lines[LinesMax];
  int ret;
  int i;

  /* Connection */
  urg_t urg;
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  /* Get version information */
  for (i = 0; i < LinesMax; ++i) {
    lines[i] = buffer[i];
  }
  ret = urg_versionLines(&urg, lines, LinesMax);
  printf("urg_getVersion: %s\n", urg_error(&urg));
  if (ret < 0) {
    urg_disconnect(&urg);
    exit(1);
  }

  /* Display */
  for (i = 0; i < LinesMax; ++i) {
    printf("%s\n", lines[i]);
  }

  /* Disconnect */
  urg_disconnect(&urg);

  return 0;
}
コード例 #8
0
ファイル: hokuyomiddle.c プロジェクト: clacarbone/saettanetus
/*-----------------------------------------------------------*/
int read_laser_data(urg_t *urg)
{
  int n;
  /*If there was an error...*/
  /*Getting the maximum amount of data*/
  data_max = urg_dataMax(urg);
  
  if (data_max<0)
    return -1;
  pthread_mutex_lock(&mutex_laser_read);
    if(!data_laser)
      data_laser=malloc(sizeof(long)*data_max);
      //free(data);
    /*Data buffer allocation (deallocation is demanded to library user)*/
    /*Printing and signalling allocation error*/
    if (!(data_laser)) {
      fprintf(stderr, "Error in data memory allocation\n");
      return -1;
    }
  
  /*Check if capturing mode is HYBRID. If it is true, check the remaining capture times,
   clean the serial and request data again.*/
  else
  {
    if(urg_capturing_mode==HYBRID)
    {
      /*Escamotage to avoid the laser stuck bug*/
      if(urg_remainCaptureTimes(urg)<10)
      {
	urg_disconnect(urg);
	init_urg_laser(&urg,HYBRID);
      }
    }
  }
  /*Receive the data in the buffer*/
  n = urg_receiveData(urg,data_laser,data_max);
  pthread_mutex_unlock(&mutex_laser_read);
    /*When the on_demand mode is on (it is necessary to explicitly request data)*/
  if(urg_capturing_mode==ON_DEMAND)
  {
  int ret = urg_requestData(urg, URG_GD, URG_FIRST, URG_LAST);
  if (ret < 0) {
    return ret;
  }
  }
  
  /*If there was an error...*/
  if (n < 0) {
    return -1;
  }  
  return n;
}
コード例 #9
0
ファイル: get_parameters.c プロジェクト: aldebaran/liburg
//! main
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
  const char device[] = "COM3"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  urg_t urg;
  urg_parameter_t parameters;
  int ret;

  /* Connection */
  urg_initialize(&urg);
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  /* Get sensor parameter */
  ret = urg_parameters(&urg, &parameters);
  printf("urg_getParameters: %s\n", urg_error(&urg));
  if (ret < 0) {
    urg_disconnect(&urg);
    exit(1);
  }

  /* Display */
  printf("distance_min: %ld\n", parameters.distance_min_);
  printf("distance_max: %ld\n", parameters.distance_max_);
  printf("area_total: %d\n", parameters.area_total_);
  printf("area_min: %d\n", parameters.area_min_);
  printf("area_max: %d\n", parameters.area_max_);
  printf("area_front: %d\n", parameters.area_front_);
  printf("scan_rpm: %d\n", parameters.scan_rpm_);
  printf("\n");

  /* Display information from URG structure (same resource as above) */
  printf("urg_getDistanceMax(): %ld\n", urg_maxDistance(&urg));
  printf("urg_getDistanceMin(): %ld\n", urg_minDistance(&urg));
  printf("urg_getScanMsec(): %d\n", urg_scanMsec(&urg));
  printf("urg_getDataMax(): %d\n", urg_dataMax(&urg));

#ifdef MSC
  getchar();
#endif

  return 0;
}
コード例 #10
0
/* Function: mdlOutputs =======================================================
 *
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    const int32_T   *potVal  = (const int32_T*) ssGetInputPortSignal(S,0);
    const real_T   *Speed  = (const real_T*) ssGetInputPortSignal(S,1);
    const real_T   *Upos  = (const real_T*) ssGetInputPortSignal(S,2);
    const real_T   *Dpos  = (const real_T*) ssGetInputPortSignal(S,3);
    real_T        *lidarData  = (real_T *)ssGetOutputPortRealSignal(S,0);
    
    //Request data
    urg_setCaptureTimes(&urg, 65);
    ret = urg_requestData(&urg, URG_MD, 0, 1080);
    if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
    }
    
    //Tell stepper to move to new position
    CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos);
    
    for (j=0;j<65;j++) {       

      // Receive Data
      n = urg_receiveData(&urg, data, 1080);
      // Error, can't receive negative data points
        if (n < 0) {
            urg_exit(&urg, "urg_receiveData()");
	    // Disconnect Lidar 
    		urg_disconnect(&urg);
   
        } 

      // Data Received, Construct Partial Image 
        else{ 
          	for(k=0;k<1080;k++){ 
            	lidarData[(j*1080)+k] = data[k];  
          	}
        }
    }
    
    stepperMove(*Upos, stepper);

    //lidar_zero_test_Outputs_wrapper(potVal, Speed, Upos, Dpos, lidarData);
    
}
コード例 #11
0
ファイル: LaserLib.cpp プロジェクト: sunacc/Basketball_Robot
/*-----------激光初始化------------------
入口参数:
     port:激光所用串口   0-6表示COM1-COM7
	 baudrate:串口通讯速率选择  0-2 表示 19200,57600,115200
出口参数:
	 urg_state:激光状态读取地址
     return:激光数据最大量
-----------------------------------------*/
int  Laser_Init(int port,int baudrate,urg_state_t *urg_state)
{
	int i=0;
	const char* com_port[7] ={ "COM1", "COM2", "COM3","COM4","COM5","COM6","COM7" };
	const long com_baudrate[3] ={ 19200, 57600, 115200 };

	int max_size=0,recv_n=0;									//串口大小

	// Connect with URG 
	int ret = urg_connect(urg_state, com_port[port], com_baudrate[baudrate]);
	if (ret < 0) {
		// Show error message and end
		AfxMessageBox("连接激光失败");
		urg_disconnect(); //断开连接
		return 0;
	}else{
		max_size = urg_state->max_size;
		urg_sendMessage("BM", Timeout, &recv_n); //开始接收激光数据

	//	SetTimer(1,100,NULL);
	}

	return max_size;
}
コード例 #12
0
ファイル: Hokuyo.cpp プロジェクト: ankit5311/mavwork
void Hokuyo::close() {
    if (started) {
        urg_disconnect(&urg);
        started = false;
    }
}
コード例 #13
0
ファイル: capture_sample.c プロジェクト: daviddoria/vtkHokuyo
/*! main */
int main(int argc, char *argv[])
{
  enum {
    CaptureTimes = 10,
  };

#ifdef WINDOWS_OS
  const char device[] = "COM3"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  int data_max;
  long* data;
  urg_parameter_t parameter;
  int ret;
  int n;
  int i;

  /* Connection */
  urg_t urg;
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  /* Request for Data using GD */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    perror("data buffer");
    exit(1);
  }
  urg_parameters(&urg, &parameter);

  /* Request for Data using GD */
  printf("GD capture\n");
  for (i = 0; i < CaptureTimes; ++i) {
    /* Request for data */
    ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
    if (ret < 0) {
      urg_exit(&urg, "urg_requestData()");
    }

    /* Reception */
    n = urg_receiveData(&urg, data, data_max);
    if (n < 0) {
      urg_exit(&urg, "urg_receiveData()");
    }

    /* Display */
    printData(&urg, &parameter, data);
  }
  printf("\n");


  /* Request for Data using MD */
  printf("MD capture\n");

  /* set data acquisition frequency equal to infinity, to get the data more
     than 100 times */
  /* urg_setCaptureTimes(&urg, UrgInfinityTimes); */
  assert(CaptureTimes < 100);
  urg_setCaptureTimes(&urg, CaptureTimes);

  /* Request for data */
  ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
  if (ret < 0) {
    urg_exit(&urg, "urg_requestData()");
  }

  for (i = 0; i < CaptureTimes; ++i) {
    /* Reception */
    n = urg_receiveData(&urg, data, data_max);
    if (n < 0) {
      urg_exit(&urg, "urg_receiveData()");
    }

    /* Display */
    printData(&urg, &parameter, data);
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
コード例 #14
0
ファイル: multi_urg.c プロジェクト: aldebaran/liburg
/*! main */
int main(int argc, char *argv[])
{
  enum {
    Times = 10,
    Urgs = 2,
  };

  urg_t urg[Urgs];
  long *data[Urgs];
  int data_max[Urgs];
  int timestamp;
  int ret;
  int n;
  int i;
  int k;

#ifdef WINDOWS_OS
  const char *devices[] = { "COM3", "COM4" }; /* For Windows */
#else
  const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */
#endif

  /* Connection */
  for (i = 0; i < Urgs; ++i) {
    urg_initialize(&urg[i]);
    ret = urg_connect(&urg[i], devices[i], 115200);
    if (ret < 0) {
      urg_exit(&urg[i], "urg_connect()");
    }
    /* To clear existing MD command*/
    urg_laserOff(&urg[i]);

    /* It will become easy if some frames are skipped. */
    /* If specified skip is 2, then transferred data becomes half. */
    /* urg_setSkipLines(&urg[i], 2); */

    /* Reserve for receive buffer */
    data_max[i] = urg_dataMax(&urg[i]);
    data[i] = (long*)malloc(sizeof(long) * data_max[i]);
    if (data[i] == NULL) {
      perror("data buffer");
      exit(1);
    }
  }

  /* Request for MD data */
  for (i = 0; i < Urgs; ++i) {
    urg_setCaptureTimes(&urg[i], Times);

    /* Request for data */
    ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST);
    if (ret < 0) {
      urg_exit(&urg[i], "urg_requestData()");
    }
  }

  for (k = 0; k < Times; ++k) {
    for (i = 0; i < Urgs; ++i) {
      /* Ends when data reception is completed */
      int remain_times = urg_remainCaptureTimes(&urg[i]);
      printf("    %d: ", i);
      printf("(%03d/%03d): ", remain_times, Times);
      if (remain_times <= 0) {
        printf("\n");
        continue;
      }

      /* Reception */
      n = urg_receiveData(&urg[i], data[i], data_max[i]);
      if (n < 0) {
        /* Continue processing, because there is chances of receiving the
           data next time. */
        printf("%s: %s\n", "urg_receiveData()", urg_error(urg));

      } else {

        /* Display */
        timestamp = urg_recentTimestamp(&urg[i]);
        printf("timestamp: %d, ", timestamp);
#if 0
        {
          int j;
          for (j = 0; j < n; ++j) {
            /* Neglect if distance data is less than urg_minDistance() */
            printf("%d:%ld, ", j, data[i][j]);
          }
          printf("\n");
        }
#endif
        printf("\n");
      }
    }
  }

  /* Disconnect */
  for (i = 0; i < Urgs; ++i) {
    urg_disconnect(&urg[i]);
    free(data[i]);
  }

#ifdef MSC
  getchar();
#endif

  return 0;
}
コード例 #15
0
ファイル: md_scan.c プロジェクト: aldebaran/liburg
//! main
int main(int argc, char *argv[])
{
  enum {
    CaptureTimes = 10,
  };

#ifdef WINDOWS_OS
  const char device[] = "COM3"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  int data_max;
  long* data;
  int timestamp = -1;
  int previous_timestamp;
  int remain_times;
  //int scan_msec;
  urg_parameter_t parameter;
  int ret;
  int n;
  int i;
  urg_t urg;

  /* Connection */
  urg_initialize(&urg);
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
    exit(1);
  }

  /* Reserve for receive buffer */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    fprintf(stderr, "data_max: %d\n", data_max);
    perror("data buffer");
    exit(1);
  }
  urg_parameters(&urg, &parameter);
  //scan_msec = urg_scanMsec(&urg);

  /* Request for MD data */
  /* To get data continuously for more than 100 times, set capture times equal
     to infinity times(UrgInfinityTimes) */
  /* urg_setCaptureTimes(&urg, UrgInfinityTimes); */
  assert(CaptureTimes < 100);
  urg_setCaptureTimes(&urg, CaptureTimes);

  /* Request for data */
  ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
  if (ret < 0) {
    urg_exit(&urg, "urg_requestData()");
  }

  for (i = 0; i < CaptureTimes; ++i) {
    /* Reception */
    n = urg_receiveData(&urg, data, data_max);
    printf("n = %d\n", n);
    if (n < 0) {
      urg_exit(&urg, "urg_receiveData()");
    } else if (n == 0) {
      printf("n == 0\n");
      --i;
      continue;
    }

    /* Display the front data with timestamp */
    /* Delay in reception of data at PC causes URG to discard the data which
       cannot be transmitted. This may  results in remain_times to become
       discontinuous */
    previous_timestamp = timestamp;
    timestamp = urg_recentTimestamp(&urg);
    remain_times = urg_remainCaptureTimes(&urg);

    /* Neglect the distance data if it is less than urg_minDistance() */
    printf("%d/%d: %ld [mm], %d [msec], (%d)\n",
           remain_times, CaptureTimes, data[parameter.area_front_], timestamp,
           timestamp - previous_timestamp);

    printf("%d, %d\n", i, remain_times);

    if (remain_times <= 0) {
      break;
    }
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
コード例 #16
0
/* Function: mdlOutputs =======================================================
 *
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    const int32_T   *potVal  = (const int32_T*) ssGetInputPortSignal(S,0);
    const real_T   *Speed  = (const real_T*) ssGetInputPortSignal(S,1);
    const real_T   *Upos  = (const real_T*) ssGetInputPortSignal(S,2);
    const real_T   *Dpos  = (const real_T*) ssGetInputPortSignal(S,3);
    const real_T   *ScanNumber  = (const real_T*) ssGetInputPortSignal(S,4);
    real_T        *lidarData  = (real_T *)ssGetOutputPortRealSignal(S,0);
    real_T        *udOut  = (real_T *)ssGetOutputPortRealSignal(S,1);
    
    //Read number of scans
    int ScanNum = *ScanNumber;
    
    CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed);
    
    //Variables required for zeroing
    //Calibrated for three turn pot
    int zeroPos = 517;
    int dpot;
    int dstep;
    
    //Pot reader may take a minute to initialize and will send zero
    //If this is the case do nothing
    if (*potVal == 0) {}
    // Otherwise scan
    else {
        
        //Zero stepper on first run
        //Account for differnces coming from different directions
        if (counter == 0) {
            
            if (zeroPos > *potVal) {
                zeroPos = 515;
            }
            if (zeroPos < *potVal) {
                zeroPos = 519;
            }
            
            //Find required steps and move to zero position
            printf("potVal:%d\n", *potVal);
            dpot = zeroPos - *potVal;
            printf("Dpot:%d\n", dpot);
            dstep = -1036/125*dpot;
            //If error occurs dstep may be very large
            //Limit dstep value to prevent damage to system
            if (dstep > 1600) {
                dstep = 1600;
            }
            if (dstep < -1600) {
                dstep = -1600;
            }
            printf("Dstep:%d\n", dstep);
            stepperMove(dstep, stepper);
            
            //Set current position to zero
            CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
            //Move to Up position to begin scanning process
            stepperMove(*Upos, stepper);
            counter = 1;
        }
        
        //Request data
        urg_setCaptureTimes(&urg, ScanNum);
        ret = urg_requestData(&urg, URG_MD, 0, 1080);
        if (ret < 0) {
            urg_exit(&urg, "urg_requestData()");
        }
        
        //Tell stepper to move to new position
        if (counter == 1) {
            CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos);
            counter = 2;
            *udOut = 0;
        }
        else if (counter == 2) {
            CPhidgetStepper_setTargetPosition(stepper, 0, *Upos);
            counter = 1;
            *udOut = 1;
        }
        
        for (j=0;j<ScanNum;j++) {
            
            // Receive Data
            n = urg_receiveData(&urg, data, 1080);
            // Error, can't receive negative data points
            if (n < 0) {
                urg_exit(&urg, "urg_receiveData()");
                // Disconnect Lidar
                urg_disconnect(&urg);
                
            }
            
            // Data Received, Construct Partial Image
            else{
                for(k=0;k<1080;k++){
                    lidarData[(j*1080)+k] = data[k];
                }
            }
        }
        
        //stepperMove(*Upos, stepper);
        
    }

    //lidar_zero_test_v4_Outputs_wrapper(potVal, Speed, Upos, Dpos, ScanNumber, lidarData, udOut);
}
コード例 #17
0
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
    const char device[] = "COM3"; /* For Windows */
#else
    const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

    int data_max;
    long *data;
    int timestamp;
    int ret;
    int n;
    int i;

    /* Connection */
    urg_t urg;
    urg_initialize(&urg);
    ret = urg_connect(&urg, device, 115200);
    if (ret < 0) {
        urg_exit(&urg, "urg_connect()");
    }

    /* Reserve for reception data */
    data_max = urg_dataMax(&urg);
    data = (long*)malloc(sizeof(long) * data_max);
    if (data == NULL) {
        perror("malloc");
        exit(1);
    }

    /* Request for GD data */
    ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
    if (ret < 0) {
        urg_exit(&urg, "urg_requestData()");
    }

    /* Reception */
    n = urg_receiveData(&urg, data, data_max);
    printf("# n = %d\n", n);
    if (n < 0) {
        urg_exit(&urg, "urg_receiveData()");
    }

    /* Display */
    timestamp = urg_recentTimestamp(&urg);
    printf("# timestamp: %d\n", timestamp);
    for (i = 0; i < n; ++i) {
        /*Neglect the distance less than  urg_minDistance()  */
        printf("%d %ld, ", i, data[i]);
    }
    printf("\n");

    urg_disconnect(&urg);
    free(data);

#ifdef MSC
    getchar();
#endif

    return 0;
}
コード例 #18
0
ファイル: hokuyomiddle.c プロジェクト: clacarbone/saettanetus
/*-----------------------------------------------------------*/
void close_urg_laser(urg_t *urg)
{
  pthread_mutex_destroy(&mutex_laser_read);
  urg_disconnect(urg);
}
コード例 #19
0
ファイル: convert_2d.c プロジェクト: aldebaran/liburg
/*! main */
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
  const char device[] = "COM3"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  long *data = NULL;
  int data_max;
  int min_length = 0;
  int max_length = 0;
  int ret;
  int n;
  int i;

  /* Connection */
  urg_t urg;
  urg_initialize(&urg);
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
  }

  /* Reserve for Reception data */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    perror("data buffer");
    exit(1);
  }

  /* Request for GD data */
  ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
  if (ret < 0) {
    urg_exit(&urg, "urg_requestData()");
  }

  /* Reception */
  n = urg_receiveData(&urg, data, data_max);
  if (n < 0) {
    urg_exit(&urg, "urg_receiveData()");
  }

  /* Output as 2 dimensional data */
  /* Consider front of URG as positive direction of X axis */
  min_length = urg_minDistance(&urg);
  max_length = urg_maxDistance(&urg);
  for (i = 0; i < n; ++i) {
    int x, y;
    long length = data[i];

    /* Neglect the out of range values */
    if ((length <= min_length) || (length >= max_length)) {
      continue;
    }

    x = (int)(length * cos(urg_index2rad(&urg, i)));
    y = (int)(length * sin(urg_index2rad(&urg, i)));

    printf("%d\t%d\t# %d, %ld\n", x, y, i, length);
  }

  urg_disconnect(&urg);
  free(data);

#ifdef MSC
  getchar();
#endif

  return 0;
}
コード例 #20
0
/* Function: mdlOutputs =======================================================
 *
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    const int32_T   *potVal  = (const int32_T*) ssGetInputPortSignal(S,0);
    const real_T   *Speed  = (const real_T*) ssGetInputPortSignal(S,1);
    const real_T   *Upos  = (const real_T*) ssGetInputPortSignal(S,2);
    const real_T   *Dpos  = (const real_T*) ssGetInputPortSignal(S,3);
    const real_T   *ScanNumber  = (const real_T*) ssGetInputPortSignal(S,4);
    real_T        *lidarData  = (real_T *)ssGetOutputPortRealSignal(S,0);
    uint8_T        *Red  = (uint8_T *)ssGetOutputPortRealSignal(S,1);
    uint8_T        *Green  = (uint8_T *)ssGetOutputPortRealSignal(S,2);
    uint8_T        *Blue  = (uint8_T *)ssGetOutputPortRealSignal(S,3);
    real_T        *udOut  = (real_T *)ssGetOutputPortRealSignal(S,4);

      //Read number of scans
    int ScanNum = *ScanNumber;
    
    CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed);
    
    //Parameters required for zeroing
    //Calibrated for three turn pot
    int zeroPos = 534; //Potentiometer reading at level position
    int stepRes = 3950; //Number of steps to move the stepper 180 degrees
    int potRes = 323; //Number of potValues that represent the rotation of the gimbal 180 degrees
    int potGive = 2; //Accounts for slack in gearing for pot
    
    //Variables required for zeroing
    int dpot; //difference to desired pot value from current
    int dstep; //number of steps the stepper must move to get to desired position
    
    //Pot reader may take a minute to initialize and will send zero
    //If this is the case do nothing
    if (*potVal != 0){
        //Zero stepper on first run
        //Account for differnces coming from different directions
        if (counter == 0) {
            
            //write pot value
            printf("potVal:%d\n", *potVal);
            
            //Find pot value difference to calculate steps needed to level
            if (zeroPos > *potVal) {
                dpot = zeroPos - potGive - *potVal;
            }
            if (zeroPos < *potVal) {
                dpot = zeroPos + potGive -*potVal;
            }
            //write difference
            printf("Dpot:%d\n", dpot);
            
            //Find required steps and move to level position
            dstep = -stepRes/potRes*dpot;
            
            //If error occurs dstep may be very large
            //Limit dstep value to prevent damage to system
            if (dstep > 1600) {
                dstep = 1600;
            }
            if (dstep < -1600) {
                dstep = -1600;
            }
            //write number of steps to zero
            printf("Dstep:%d\n", dstep);
            stepperMove(dstep, stepper);
            
            //Set current position to zero
            CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
            //Move to Up position to begin scanning process
            stepperMove(*Upos, stepper);
            counter = 1;
        }
        
        //Request data
        urg_setCaptureTimes(&urg, ScanNum);
        ret = urg_requestData(&urg, URG_MD, 0, 1080);
        if (ret < 0) {
            urg_exit(&urg, "urg_requestData()");
        }
        
        //Tell stepper to move to new position
        if (counter == 1) {
            CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos);
            counter = 2;
            *udOut = 0;
        }
        else if (counter == 2) {
            CPhidgetStepper_setTargetPosition(stepper, 0, *Upos);
            counter = 1;
            *udOut = 1;
        }
        
        for (j=0;j<ScanNum;j++) {
            
            // Receive Data
            n = urg_receiveData(&urg, data, 1080);
            // Error, can't receive negative data points
            if (n < 0) {
                urg_exit(&urg, "urg_receiveData()");
                // Disconnect Lidar
                urg_disconnect(&urg);
                
            }
            
            // Data Received, Construct Partial Image
            else{
                for(k=0;k<1080;k++){
                    lidarData[(j*1080)+k] = data[k];
                }
            }
        }
    }// END OF LIDAR DATA ACQ
    // START CAM ACQ
    // Grab a number of frames in order to flush the data buffer
    int flush1 = cvGrabFrame( capture1 );
    int flush2 = cvGrabFrame( capture1 );
    int flush3 = cvGrabFrame( capture1 );
    int flush4 = cvGrabFrame( capture1 );

    IplImage *frame1 = cvQueryFrame( capture1 ); //Gets Frame from Camera

    unsigned char *data1 = frame1->imageData;
    long iNew, jNew = 0, kNew = 0; 
    for(iNew = 0; iNew < 480*640*3; iNew+=3){
        //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]);
        Blue[jNew*480+kNew] = data1[iNew];
        Green[jNew*480+kNew] = data1[iNew+1];
        Red[jNew*480+kNew] = data1[iNew+2];
        jNew++;
        if(jNew==640){kNew++; jNew = 0;}
    }
}
コード例 #21
0
void urg_exit(urg_t *urg, const char *message){
  printf("%s: %s\n", message, urg_error(urg));
  urg_disconnect(urg);
}
コード例 #22
0
/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    stepperMove(0, stepper);
    urg_disconnect(&urg);
    stepperDisconnect(stepper);
}
コード例 #23
0
LaserRangeFinder::~LaserRangeFinder()
{
   urg_disconnect(& m_handle) ;
   delete[] m_distances ;
   delete[] m_buffer ;
}
コード例 #24
0
ファイル: lidarOnly3.c プロジェクト: Adeange1/roboboat2015
/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    urg_laserOff(&urg);
     urg_disconnect(&urg);

}
コード例 #25
0
/* Function: mdlOutputs =======================================================
 *
 */
static void mdlOutputs(SimStruct *S, int_T tid) {
    const int32_T   *choice  = (const int32_T*) ssGetInputPortSignal(S, 0);
    const int32_T   *potVal  = (const int32_T*) ssGetInputPortSignal(S, 1);
    const real_T   *Speed  = (const real_T*) ssGetInputPortSignal(S, 2);
    const real_T   *Upos  = (const real_T*) ssGetInputPortSignal(S, 3);
    const real_T   *Dpos  = (const real_T*) ssGetInputPortSignal(S, 4);
    const real_T   *ScanNumber  = (const real_T*) ssGetInputPortSignal(S, 5);
    real_T        *lidarData  = (real_T *)ssGetOutputPortRealSignal(S, 0);
    uint8_T        *Red  = (uint8_T *)ssGetOutputPortRealSignal(S, 1);
    uint8_T        *Green  = (uint8_T *)ssGetOutputPortRealSignal(S, 2);
    uint8_T        *Blue  = (uint8_T *)ssGetOutputPortRealSignal(S, 3);
    real_T        *udOut  = (real_T *)ssGetOutputPortRealSignal(S, 4);
    
    //Read number of scans
    int ScanNum = *ScanNumber;
    
    IplImage *frame1;
    
    CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed);
    
    //Parameters required for zeroing
    //Calibrated for three turn pot
    int zeroPos = 483; //Potentiometer reading at level position
    int stepRes = 7328; //Number of steps to move the gimbal 180 degrees
    int potRes = 741; //Number of potValues that represent the rotation of the gimbal 180 degrees
    int potGive = 14; //Accounts for slack in gearing for pot
    
    //Variables required for zeroing
    int dpot; //difference to desired pot value from current
    int dstep; //number of steps the stepper must move to get to desired position
    
    int midscan = ScanNum/2;
    
    
    // Stepper Always On
    // Option 0 - Lidar Only
    // Option 1 - Cam Only
    // Option 2 - All On
    // Default case - Do Nothing/Error Recovery
    switch(*choice){
        
        //////////////////////////////////////////////////////
        /////////////// Option 0 - Lidar Only  ///////////////
        //////////////////////////////////////////////////////
        
        case 0:{ 
            //Pot reader may take a minute to initialize and will send zero
            //If this is the case do nothing
            if (*potVal != 0) {
                //Zero stepper on first run
                //Account for differnces coming from different directions
                if (counter == 0) {
                    printf("potVal:%d\n", *potVal);
                    //Find pot value difference to calculate steps needed to level
                    if (zeroPos > *potVal) {
                        dpot = zeroPos - potGive - *potVal;
                    }
                    else if (zeroPos < *potVal) {
                        dpot = zeroPos + potGive -*potVal;
                    }
                    else {
                        dpot = 0;
                    }
                    printf("Dpot:%d\n", dpot);
                    
                    //Find required steps and move to level position
                    dstep = -stepRes/potRes*dpot;
                    
                    //If error occurs dstep may be very large
                    //Limit dstep value to prevent damage to system
                    if (dstep > 3000)
                        dstep = 3000;
                    if (dstep < -3000)
                        dstep = -3000;
                    
                    printf("Dstep:%d\n", dstep);
                    stepperMove(dstep, stepper);
                    
                    //Set current position to zero
                    CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
                    //Move to Up position to begin scanning process
                    stepperMove(*Upos, stepper);
                    counter = 1;
                }
                
                if (lidarCounter == 0) {
                    camCounter = 0;
                    bothCounter = 0;
                    lidarCounter = 1;
                }
                
                //Tell stepper to move to new position
                if (counter == 1) {
                    CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos);
                    counter = 2;
                    *udOut = 0;
                }
                else if (counter == 2) {
                    CPhidgetStepper_setTargetPosition(stepper, 0, *Upos);
                    counter = 1;
                    *udOut = 1;
                }
                //Sleep to allow stepper to start moving
                usleep(120000);
                
                //Request data
                urg_setCaptureTimes(&urg, ScanNum);
                ret = urg_requestData(&urg, URG_MD, 0, 1080);
                if (ret < 0)
                    urg_exit(&urg, "urg_requestData()");
                
                               
                for (j=0;j<ScanNum;j++) {
                    // Receive Data
                    n = urg_receiveData(&urg, data, 1080);
                    // Error, can't receive negative data points
                    if (n < 0) {
                        urg_exit(&urg, "urg_receiveData()");
                        // Disconnect Lidar
                        urg_disconnect(&urg);
                    }
                    
                    // Data Received, Construct Partial Image
                    else{
                        for(k=0;k<1080;k++){
                            lidarData[(j*1080)+k] = data[k];
                        }
                    }
                }
            }// END OF LIDAR DATA ACQ
            break;
        }
        
        ///////////////////////////////////////////////////
        /////////////// Option 1 - Cam Only ///////////////
        ///////////////////////////////////////////////////
        
        case 1:{ 
            
            if (camCounter == 0) {
                // Grab a number of frames in order to flush the data buffer
                int flush1 = cvGrabFrame( capture1 );
                int flush2 = cvGrabFrame( capture1 );
                int flush3 = cvGrabFrame( capture1 );
                int flush4 = cvGrabFrame( capture1 );
                
                lidarCounter = 0;
                bothCounter = 0;
                camCounter = 1;
            }
            
            frame1 = cvQueryFrame( capture1 ); //Gets Frame from Camera
            
            unsigned char *data1 = frame1->imageData;
            long iNew, jNew = 0, kNew = 0;
            for(iNew = 0; iNew < 480*640*3; iNew+=3){
                //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]);
                Blue[jNew*480+kNew] = data1[iNew];
                Green[jNew*480+kNew] = data1[iNew+1];
                Red[jNew*480+kNew] = data1[iNew+2];
                jNew++;
                if(jNew==640){kNew++; jNew = 0;}
            }
            break;
        }
        
        ///////////////////////////////////////////////////
        //////////////// Option 2  - All On ///////////////
        ///////////////////////////////////////////////////
        
        case 2:{ 
            //Pot reader may take a minute to initialize and will send zero
            //If this is the case do nothing
            
            
            //Zero stepper on first run
            //Account for differnces coming from different directions
            if (counter == 0) {
                printf("potVal:%d\n", *potVal);
                //Find pot value difference to calculate steps needed to level
                if (zeroPos > *potVal) {
                    dpot = zeroPos - potGive - *potVal;
                }
                else if (zeroPos < *potVal) {
                    dpot = zeroPos + potGive -*potVal;
                }
                else {
                    dpot = 0;
                }
                printf("Dpot:%d\n", dpot);
                
                //Find required steps and move to level position
                dstep = -stepRes/potRes*dpot;
                
                //If error occurs dstep may be very large
                //Limit dstep value to prevent damage to system
                if (dstep > 3000)
                    dstep = 3000;
                if (dstep < -3000)
                    dstep = -3000;
                
                printf("Dstep:%d\n", dstep);
                stepperMove(dstep, stepper);
                
                //Set current position to zero
                CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
                //Move to Up position to begin scanning process
                stepperMove(*Upos, stepper);
                counter = 1;
            }
            
            if (bothCounter == 0) {
                //Preform on first run
                lidarCounter = 0;
                camCounter = 0;
                bothCounter = 1;
            }
            
            //START OF DATA ACQUISITION
                                  
            //Tell stepper to move to new position
            if (counter == 1) {
                CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos);
                counter = 2;
                *udOut = 0;
            }
            else if (counter == 2) {
                CPhidgetStepper_setTargetPosition(stepper, 0, *Upos);
                counter = 1;
                *udOut = 1;
            }
            
            //Sleep to allow stepper to start moving
            usleep(120000);
            
            //Request data
            urg_setCaptureTimes(&urg, ScanNum);
            ret = urg_requestData(&urg, URG_MD, 0, 1080);
            if (ret < 0)
                urg_exit(&urg, "urg_requestData()");
            
            //Sleep so image will be taken in middle of lidar scan
            //usleep(100000);
            
            // Grab a number of frames in order to flush the data buffer
            int flush1 = cvGrabFrame( capture1 );
            int flush2 = cvGrabFrame( capture1 );
            int flush3 = cvGrabFrame( capture1 );
            int flush4 = cvGrabFrame( capture1 );
            
            // Acquire from from webcam
            //IplImage *frame1 = cvQueryFrame( capture1 );
            
            
            for (j=0;j<ScanNum;j++) {
                // Receive Data
                n = urg_receiveData(&urg, data, 1080);
                
                // Grab frame from webcam in middle of scan
                if (j == midscan) {
                    frame1 = cvQueryFrame( capture1 );
                }
                
                // Error, can't receive negative data points
                if (n < 0) {
                    urg_exit(&urg, "urg_receiveData()");
                    // Disconnect Lidar
                    urg_disconnect(&urg);
                }
                
                // Data Received, Construct Partial Image
                else{
                    for(k=0;k<1080;k++){
                        lidarData[(j*1080)+k] = data[k];
                    }
                }
            }
            
            
            
            unsigned char *data1 = frame1->imageData;
            long iNew, jNew = 0, kNew = 0;
            for(iNew = 0; iNew < 480*640*3; iNew+=3){
                //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]);
                Blue[jNew*480+kNew] = data1[iNew];
                Green[jNew*480+kNew] = data1[iNew+1];
                Red[jNew*480+kNew] = data1[iNew+2];
                jNew++;
                if(jNew==640){kNew++; jNew = 0;}
            }
            break;
        }
        
        ///// Default Case for all other invalid input /////
        
        default:{
            lidarCounter = 0;
            camCounter = 0;
            bothCounter = 0;
            break;
        }
        // END OF LIDAR DATA ACQ
    }
}