/* 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); }
/* 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); }
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; }
void urg_exit(urg_t *urg, const char *message){ printf("%s: %s\n", message, urg_error(urg)); urg_disconnect(urg); #ifdef MSC getchar(); #endif }
/* 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 ); }
int CSCIPRadarComm::CloseRadar(void) { if(urg.is_laser_on_==UrgLaserOn) { //具体发送命令去控制雷达停止 urg_disconnect(&urg); } return 1; }
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; }
/*-----------------------------------------------------------*/ 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; }
//! 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, ¶meters); 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; }
/* 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); }
/*-----------激光初始化------------------ 入口参数: 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; }
void Hokuyo::close() { if (started) { urg_disconnect(&urg); started = false; } }
/*! 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, ¶meter); /* 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, ¶meter, 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, ¶meter, data); } urg_disconnect(&urg); free(data); #ifdef MSC getchar(); #endif return 0; }
/*! 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; }
//! 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, ¶meter); //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; }
/* 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); }
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; }
/*-----------------------------------------------------------*/ void close_urg_laser(urg_t *urg) { pthread_mutex_destroy(&mutex_laser_read); urg_disconnect(urg); }
/*! 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; }
/* 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;} } }
void urg_exit(urg_t *urg, const char *message){ printf("%s: %s\n", message, urg_error(urg)); urg_disconnect(urg); }
/* 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); }
LaserRangeFinder::~LaserRangeFinder() { urg_disconnect(& m_handle) ; delete[] m_distances ; delete[] m_buffer ; }
/* 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); }
/* 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 } }