/* 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: 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);
}
static void mdlStart(SimStruct *S)
{
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
}
//! 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: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
const real_T *Upos = (const real_T*) ssGetInputPortSignal(S,1);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
CPhidgetStepper_create(&stepper);
stepperConnect(600000, 1214,stepper);
stepperMove(*Upos, stepper);
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
//Lidar setup
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
//Stepper Setup
CPhidgetStepper_create(&stepper);
stepperConnect(600000, 1214,stepper);
//Zero prameters
counter = 0;
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
const real_T *Upos = (const real_T*) ssGetInputPortSignal(S,1);
const int32_T *potVal = (const int32_T*) ssGetInputPortSignal(S,0);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
CPhidgetStepper_create(&stepper);
stepperConnect(600000, 1214,stepper);
//Zero prameters
int zeroPos = 418;
int dpot;
int dstep;
//Zero stepper on start
if (zeroPos > *potVal) {
zeroPos = 418;
}
if (zeroPos < *potVal) {
zeroPos = 419;
}
printf("potVal:%d\n",*potVal);
dpot = zeroPos - *potVal;
printf("Dpot:%d\n",dpot);
dstep = -1036/45*dpot;
printf("Dstep:%d\n",dstep);
//stepperMove(dstep, stepper);
CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
stepperMove(*Upos, stepper);
}
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;
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
//Lidar setup
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
//Stepper Setup
CPhidgetStepper_create(&stepper);
stepperConnect(600000, 1214,stepper);
//Zero prameters
counter = 0;
// Webcam SetUp
capture1 = cvCaptureFromCAM( -1 ); //Sets up Camera Object
if ( !capture1 )
fprintf( stderr, "ERROR: capture is NULL \n" );
}
/*! 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
long timestamp = 0;
int ret = 0;
int i;
/* Connection */
urg_t urg;
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Transit to timestamp mode and displays time stamp */
urg_enableTimestampMode(&urg);
for (i = 0; i < CaptureTimes; ++i) {
timestamp = urg_currentTimestamp(&urg);
printf("%02d: timestamp: %ld [msec]\n", i, timestamp);
}
urg_disableTimestampMode(&urg);
#ifdef MSC
getchar();
#endif
return 0;
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S) {
// Lidar Setup
ret = urg_connect(&urg, device, 115200);
if(ret<0)
urg_exit(&urg, "urg_connect()");
else
urg_setCaptureTimes(&urg, UrgInfinityTimes);
// Stepper Setup
// Default 1214 Speed before Speed Input
CPhidgetStepper_create(&stepper);
stepperConnect(1000000, 1214, stepper);
// Zero Counter variables
counter = 0;
camCounter = 0;
lidarCounter = 0;
bothCounter = 0;
// Webcam Setup
capture1 = cvCaptureFromCAM( -1 ); //Sets up Camera Object
if (!capture1)
fprintf( stderr, "ERROR: capture is NULL \n" );
}
/*! 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;
}
/* 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;
}
/*! 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;}
}
}
//! 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 *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
}
}
