/* 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);
}
/* 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 );
}
/**
"SM" — Stepper Move
Command: SM,duration,axis1[,axis2]<CR>
Response: OK<NL><CR>
Firmware versions: all (with changes)
Execution: Added to FIFO motion queue
Arguments:
duration is an integer in the range from 1 to 16777215, giving time in
milliseconds.
axis1 and axis2 are integers, each in the range from -16777215 to 16777215,
giving movement distance in steps.
Description:
Use this command to make the motors draw a straight line at constant velocity,
or to add a delay to the motion queue.
If both axis1 and axis2 are zero, then a delay of duration ms is executed. axis2
is an optional value, and if it is not included in the command, zero steps are
assumed for axis 2.
The sign of axis1 and axis2 represent the direction each motor should turn.
The minimum speed at which the EBB can generate steps for each motor is 1.31
steps/second. The maximum speed is 25,000 steps/second. If the SM command finds
that this speed range will be violated on either axis, it will output an error
message declaring such and it will not complete the move.
Note that internally the EBB generates an Interrupt Service Routine (ISR) at the
25 kHz rate. Each time the ISR fires, the EBB determines if a step needs to be
taken for a given axis or not. The practical result of this is that all steps
will be 'quantized' to the 25 kHz (40 μs) time intervals, and thus as the step
rate gets close to 25 kHz the 'correct' time between steps will not be
generated, but instead each step will land on a 40 μs tick in time. In almost
all cases normally used by the EBB, this doesn't make any difference because the
overall proper length for the entire move will be correct.
A value of 0 for duration is invalid and will be rejected.
The EBB firmware can sustain moves of 3ms of more continuously without any
inter-move gaps in time.
Example: SM,1000,250,-766\r Move axis1 by 250 steps and axis2 by -766 steps, in
1000 ms of duration.
*/
void EBBParser::parseSM(const char* arg1, const char* arg2, const char* arg3)
{
if (arg1 == NULL || arg2 == NULL) {
sendError();
return;
}
sendAck();
const int duration = atoi(arg1);
const int axis1 = atoi(arg2);
const int axis2 = (arg3 != NULL) ? atoi(arg3) : 0;
stepperMove(duration, axis1, axis2);
}
/* 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: 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);
}
/* 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);
}
/* 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);
}
/* 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);
}
/* 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;}
}
}
/* 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
}
}