void BMP2DotMatrix(_TCHAR* bmp, _TCHAR* op, _TCHAR* hex, _TCHAR* variation) {
PALLET pal;
DOTMATRIX dm, odm;
DOTMATRIX* font;
DOTMATRIXPOT start, corner, end;
FONTSIZE fs;
FILE* fp;
wchar_t* stopword;
double tolerance = wcstod(variation, &stopword);
fs.h = fs.w = 12;
if (_wfopen_s(&fp, bmp, L"rb") > 0) {
wprintf(L"Can not open file: %s\n", bmp);
return;
}
if (getPallet(&pal, fp) > 0) {
printf("Can not read pallet from bmp\n");
return;
}
thresholding(&pal, &_tchar2RGB(hex), tolerance);
dmInit(&pal);
if (getMatrix() > 0) {
printf("Can not read dot matrix from pallet\n");
return;
}
freePallet(&pal);
fclose(fp);
if (_wfopen_s(&fp, op, L"w") > 0) {
wprintf(L"Can not open file: %s\n", op);
return;
}
/*
start.r = start.c = 0;
start = matSanH(&dm, &start);
printf("start.r = %d\n", start.r);
printf("start.c = %d\n", start.c);
corner = matSanV(&dm, &start, &fs);
printf("corner.r = %d\n", corner.r);
printf("corner.c = %d\n", corner.c);
end = reMatSanV(&dm, &start, &fs);
*/
// matCarve(&corner, &end, &dm, &odm, &fs);
// dotmat2File(fp);
// matCarveByChar(&odm, font, &fs);
freeMatrix();
// freeMatrix(&odm);
fclose(fp);
printf("Program finished.\n");
}
int main (int argc, char **argv)
{
char* ntry = (char*)"";
if (argc > 1) {
ntry = argv[1];
}
double fps = FPS;
double target_dur = 1.0/fps;
double tol = 1.0e-3;
double total_dur = 0.0;
dc1394_t * d = dc1394_new();
if (!d) {
return 1;
}
dc1394camera_list_t * list;
dc1394error_t err = dc1394_camera_enumerate (d, &list);
DC1394_ERR_RTN(err,"Failed to enumerate cameras");
if (list->num == 0) { /* Verify that we have at least one camera */
dc1394_log_error("No cameras found");
return 1;
}
gCamera.init(d, list->ids[0].guid);
if (!gCamera.cam()) {
dc1394_log_error("Failed to initialize camera with guid %ld", list->ids[0].guid);
dc1394_camera_free_list (list);
return 1;
}
dc1394_camera_free_list (list);
/*-----------------------------------------------------------------------
* have the camera start sending us data
*-----------------------------------------------------------------------*/
err = gCamera.start_transmission();
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(gCamera),"Could not start camera iso transmission");
/*-----------------------------------------------------------------------
* capture one frame
*-----------------------------------------------------------------------*/
uint32_t width = 0;
uint32_t height = 0;
gCamera.get_image_size(&width, &height);
cv::Mat mapping = cv::getRotationMatrix2D(cv::Point2f(width/2.0, height/2.0), 180.0, 1.0);
#ifdef USE_SDL
static char *var = (char*)"SDL_VIDEO_WINDOW_POS=\"1280,480\"";
int ret = putenv(var);
if (SDL_Init(SDL_INIT_VIDEO) != 0) {
std::cerr << "DC1394: Unable to initialize SDL: " << SDL_GetError() << std::endl;
return 1;
}
atexit(SDL_Quit);
SDL_Surface *screen;
screen = SDL_SetVideoMode(width, height, 24, SDL_HWSURFACE);
if (screen == NULL) {
std::cerr << "DC1394: Unable to set SDL video mode:" << SDL_GetError() << std::endl;
}
SDL_Event event;
#endif
#ifndef LICKOMETER
pthread_t save_thread, acq_thread;
pthread_create( &save_thread, NULL, &thread_save_image, NULL);
#endif
pthread_t save_thread, acq_thread;
pthread_create( &acq_thread, NULL, &thread_acq_image, NULL);
timespec t_sleep, t_rem;
t_sleep.tv_sec = 0;
t_sleep.tv_nsec = 1000;
#ifndef STANDALONE
int s;
if ((s = socket(SOCKTYPE, SOCK_STREAM, 0)) < 0) {
perror("DC1394: client: socket");
cleanup_and_exit(gCamera);
return 1;
}
/*
* Create the address we will be connecting to.
*/
#ifndef INET
sockaddr_un sa;
sa.sun_family = AF_UNIX;
std::ostringstream tmpfn;
tmpfn << "fwsocket" << ntry;
std::cout << "DC1394: socket name " << tmpfn.str() << std::endl;
int nameLen = strlen(tmpfn.str().c_str());
if (nameLen >= (int) sizeof(sa.sun_path) -1) { /* too long? */
cleanup_and_exit(gCamera);
return 1;
}
sa.sun_path[0] = '\0'; /* abstract namespace */
strcpy(sa.sun_path+1, tmpfn.str().c_str());
int len = 1 + nameLen + offsetof(struct sockaddr_un, sun_path);
#else
sockaddr_in sa;
bzero((char *) &sa, sizeof(sa));
sa.sin_family = AF_INET;
hostent *server = gethostbyname("128.40.156.129");
bcopy((char *)server->h_addr,
(char *)&sa.sin_addr.s_addr,
server->h_length);
sa.sin_port = htons(35000);
int len = sizeof(sa);
#endif
/*
* Try to connect to the address. For this to
* succeed, the server must already have bound
* this address, and must have issued a listen()
* request.
*
* The third argument indicates the "length" of
* the structure, not just the length of the
* socket name.
*/
std::cout << "DC1394: Waiting for connection... " << std::flush;
while (true) {
// wait for connection:
if (connect(s, (sockaddr*)&sa, len) < 0) {
nanosleep(&t_sleep, &t_rem);
} else {
break;
}
}
std::cout << "done" << std::endl;
bool connected = false;
std::vector<char> data(BUFSIZE);
int nrec = recv(s, &data[0], data.size(), 0);
std::string datastr(data.begin(), data.end());
if (nrec<=0) {
std::cerr << "DC1394: Didn't receive start message; exiting now" << std::endl;
cleanup_and_exit(gCamera);
close(s);
return 1;
}
connected = true;
std::string ready = "ready";
while (send(s, ready.c_str(), ready.size(), 0) < 0) {
perror("DC1394: client: send");
}
int flags = 0;
if (-1 == (flags = fcntl(s, F_GETFL, 0)))
flags = 0;
if (fcntl(s, F_SETFL, flags | O_NONBLOCK)==-1) {
perror("DC1394: client: unblock");
}
#endif
/* pthread_mutex_lock( &camera_mutex );
gCamera.wait_for_trigger();
pthread_mutex_unlock( &camera_mutex );
Wait for acq_frame_buffer to fill instead
*/
int ncount = 0;
cv::Mat im(cv::Size(width, height), CV_8UC1);
cv::Mat thresh = cv::Mat::ones(cv::Size(width, height), CV_8UC1);
cv::Mat prevs(cv::Size(width, height), CV_8UC1);
cv::Mat gray(cv::Size(width, height), CV_8UC1);
// wait for image:
int nframes = get_image(im, mapping, false, -1, "", ncount);
std::cout << "DC1394: Waiting for first image to arrive... " << std::flush;
int nwait = 0;
while (!nframes) {
nanosleep(&t_sleep, &t_rem);
std::cout << "." << std::flush;
nframes = get_image(im, mapping, false, -1, "", ncount);
nwait++;
#ifdef STANDALONE
if (nwait > 1000) {
#else
if (nwait > 100000) {
#endif
std::cout << "Time out, stopping now\n";
cleanup_and_exit(gCamera);
}
}
timespec time0;
clock_gettime(CLOCK_REALTIME, &time0);
std::cout << "DC1394: image arrived: "
<< IplImage(im).depth << " bits, "
<< IplImage(im).nChannels << " channels, "
<< IplImage(im).widthStep << " step width" << std::endl;
#ifdef USE_SDL
SDL_Surface *surface =
SDL_CreateRGBSurfaceFrom((void*)im.data,
im.cols,
im.rows,
IplImage(im).depth*IplImage(im).nChannels,
IplImage(im).widthStep,
0xffffff, 0xffffff, 0xffffff, 0);
screen = SDL_GetVideoSurface();
if(SDL_BlitSurface(surface, NULL, screen, NULL) == 0)
SDL_UpdateRect(screen, 0, 0, 0, 0);
#else
cv::namedWindow("DC1394", CV_WINDOW_AUTOSIZE);
cvMoveWindow("DC1394", 1280, 480);
cv::imshow("DC1394", im);
#endif
timespec time1 = time0;
timespec time2 = time0;
timespec time3 = time0;
timespec time4 = time0;
timespec t_disconnect = time0;
timespec t_notrigger = time0;
#ifdef STANDALONE
int s = -1;
#endif
std::string fn = "";
#ifdef LICKOMETER
std::string fn_lick = "";
FILE* fp_lick = NULL;
#endif
int key = 0;
int nloop = 0;
while (true) {
clock_gettime( CLOCK_REALTIME, &time1);
#ifndef STANDALONE
std::vector<char> data(BUFSIZE);
int nrec = recv(s, &data[0], data.size(), 0);
std::string datastr(data.begin(), data.end());
#endif
nframes += get_image(im, mapping, false, s, fn, ncount);
#ifndef STANDALONE
// no update from blender in a long time, terminate process
if (datastr.find("1")==std::string::npos) {
if (connected) {
t_disconnect = time1;
connected = false;
} else {
if (tdiff(time1, t_disconnect) > TIMEOUT) {
std::cout << "DC1394: Received termination signal" << std::endl;
close(s);
pthread_cancel(acq_thread);
pthread_cancel(save_thread);
return 0;
}
}
} else {
connected = true;
}
/* Explicit termination */
if (datastr.find("quit")!=std::string::npos) {
std::cout << "DC1394: Game over signal." << std::endl;
std::string sclose = "close";
while (send(s, sclose.c_str(), sclose.size(), 0) < 0) {
perror("DC1394: client: send");
}
close(s);
pthread_cancel(acq_thread);
pthread_cancel(save_thread);
return 0;
}
// Stop recording
if (datastr.find("stop") != std::string::npos && fn != "") {
fn = "";
#ifdef LICKOMETER
fn_lick = "";
if (fp_lick) {
fclose(fp_lick);
fp_lick = NULL;
}
#endif
std::cout << "DC1394: Stopping video" << std::endl;
connected = true;
ncount = 0;
}
// Start recording
if (datastr.find("avi") != std::string::npos && datastr.find("stop") == std::string::npos && fn == "") {
std::size_t startpos = datastr.find("begin")+5;
std::size_t endpos = datastr.find("end") - datastr.find("begin") - 5;
fn = datastr.substr(startpos, endpos);
fn = std::string(trunk) + "data/" + fn;
#ifdef LICKOMETER
fn_lick = fn + "_lick";
fp_lick = fopen(fn_lick.c_str(), "wb");
std::cout << "DC1394: Recording lick detection, writing to " << fn_lick << std::endl;
#else
boost::filesystem::path path(fn);
boost::filesystem::path writepath(path);
// Test whether dir exists:
if (!boost::filesystem::exists(writepath)) {
std::cout << "DC1394: Creating directory " << writepath << std::endl;
boost::filesystem::create_directories(writepath);
}
fn += "/";
/* check save frame buffer */
std::size_t nfb = save_frame_buffer.size();
if (nfb)
std::cerr << "DC1394: Frame buffer isn't empty!" << std::endl;
std::cout << "DC1394: Starting video, writing to " << fn << std::endl;
connected = true;
ncount = 0;
#endif
}
#endif // #nstandalone
#ifdef USE_SDL
if (SDL_PollEvent(&event)) {
#ifdef STANDALONE
/* Any of these event types will end the program */
if (event.type == SDL_QUIT
|| event.type == SDL_KEYDOWN
|| event.type == SDL_KEYUP) {
std::cout << std::endl;
std::cout << std::endl << "DC1394: Total number of frames was " << nframes << std::endl;
std::cout << std::endl << "DC1394: Frame buffer: " << acq_frame_buffer.size() << " frames left" << std::endl;
close(s);
pthread_cancel(acq_thread);
pthread_cancel(save_thread);
return 0;
}
#endif // STANDALONE
}
surface->pixels = (void*)im.data;
// SDL_CreateRGBSurfaceFrom((void*)IplImage(im).imageData,
// IplImage(im).width,
// IplImage(im).height,
// IplImage(im).depth*IplImage(im).nChannels,
// IplImage(im).widthStep,
// 1, 1, 1, 0);
screen = SDL_GetVideoSurface();
if(SDL_BlitSurface(surface, NULL, screen, NULL) == 0)
SDL_UpdateRect(screen, 0, 0, 0, 0);
#else // not SDL
key = cv::waitKey(2);
cv::imshow("DC1394", im);
if (key == 1114155 || key == 65579 || key==43 /*+*/) {
uint32_t gain = 0;
err = dc1394_feature_get_value(gCamera.cam(), DC1394_FEATURE_GAIN, &gain);
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(gCamera),"Can't get gain");
if (gain < gCamera.get_maxgain()-10) {
gain += 10;
pthread_mutex_lock( &camera_mutex );
err = dc1394_feature_set_value(gCamera.cam(), DC1394_FEATURE_GAIN, gain);
pthread_mutex_unlock( &camera_mutex );
std::cout << "DC1394: New gain value: " << gain << std::endl;
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(gCamera),"Can't set gain");
}
}
if (key == 1114207 || key == 45 /*-*/) {
uint32_t gain = 0;
err = dc1394_feature_get_value(gCamera.cam(), DC1394_FEATURE_GAIN, &gain);
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(gCamera),"Can't get gain");
if (gain > gCamera.get_mingain()+10) {
gain -= 10;
pthread_mutex_lock( &camera_mutex );
err = dc1394_feature_set_value(gCamera.cam(), DC1394_FEATURE_GAIN, gain);
pthread_mutex_unlock( &camera_mutex );
DC1394_ERR_CLN_RTN(err,cleanup_and_exit(gCamera),"Can't set gain");
}
}
#endif // not SDL
#ifdef LICKOMETER
/* IS THIS ALL YOU NEED THEN?
Lick detection */
/* Not required because the captured image is already gray
cv::Mat gray = bgr2gray(im); */
gray = thresholding(im, LICK_FRAME_THRESHOLD);
if (nloop != 0) {
cv::absdiff(prevs, gray, thresh);
double pixel_sum_thresh = cv::sum(thresh)[0];
double pixel_sum_gray = cv::sum(gray)[0];
if (pixel_sum_thresh > LICK_SUM_THRESHOLD) {
std::cout << "DC1394: Lick" << std::endl;
}
if (fp_lick != NULL) {
fwrite(&pixel_sum_thresh, sizeof(pixel_sum_thresh), 1, fp_lick);
fwrite(&pixel_sum_gray, sizeof(pixel_sum_gray), 1, fp_lick);
}
}
prevs = gray.clone();
nloop++;
#endif
#ifdef STANDALONE
if (key == 1048689 || key == 113 /*q*/) {
std::cout << "DC1394: Mean frame rate was " << nframes/total_dur << " fps" << std::endl;
pthread_cancel(acq_thread);
pthread_cancel(save_thread);
return 0;
}
if (key == 1048691 /*s*/) {
fn = "";
std::cout << "DC1394: Stopping video" << std::endl;
ncount = 0;
}
if (key == 1048690 /*r*/) {
fn = trunk + std::string("tmp/");
std::cout << "DC1394: Starting video, writing to " << fn << std::endl;
ncount = 0;
}
#endif // #standalone
clock_gettime( CLOCK_REALTIME, &time2);
double loop_dur = tdiff(time2, time3);
clock_gettime( CLOCK_REALTIME, &time3);
double meanfps = 0;
total_dur = tdiff(time3, time0);
if (total_dur > 0)
meanfps = nframes / total_dur;
double currentfps = ret / loop_dur;
std::cout << "DC1394: Current fps: " << std::setprecision(7) << currentfps
<< " Average fps: " << std::setprecision(7) << meanfps << "\r" << std::flush;
#ifdef STANDALONE
// std::cout << capture_dur << "\t" << target_dur << "\t" << rem << "\t" << loop_dur << std::endl;
#endif
}
if (d) {
dc1394_free(d);
}
#ifndef STANDALONE
close(s);
#endif
return 0;
}
task main(){
startTask(nonviolenceTask);
int x1, y1, x2, y2;
//Assumes that it starts with forward pointing
//in the desired permanent definition of "forward"
//Integrator object for gyro angle and fwd, side, and rot positions
INTR gyrointr; initIntr(gyrointr);
INTR fwdintr; initIntr(fwdintr);
INTR sideintr; initIntr(sideintr);
INTR rotintr; initIntr(rotintr);
//nMotorPIDSpeedCtrl?
//neverest must have max output 78 if so, because hitechnic encoders expect more ticks.
//http://www.cougarrobot.com/index.php?option=com_content&view=article&id=331%3Aandymark-neverest-motor-notes&catid=92%3Aftc-hardware&Itemid=140
//Initiate gyro
nSchedulePriority = kHighPriority;
HTGYROstartCal(gyro);
wait1Msec(1000); //and give it a bit of time
while(1){
//ROTATION-CAPABLE AUGMENTED-TANK-DRIVE (see git history for previous versions)
//-------------GET JOYSTICK VALUES-------------//
getJoystickSettings(joystick);
x1 = thresholding(joystick.joy1_x1,3);
y1 = thresholding(joystick.joy1_y1,3);
x2 = thresholding(joystick.joy1_x2,3);
y2 = thresholding(joystick.joy1_y2,3);
//-------------CALCULATE INITIAL VALUES-------------//
//Get whatever values we need in the way we need for our particular control method.
//Takes the averages of the x and y values for the joysticks, and makes them the x/y translations, so it acts just like augmented-tank original.
//(you push both joysticks in the direction you want to translate in)
float translationX = (x1 + x2)/2.0;
float translationY = (y1 + y2)/2.0;
//Determines the difference of the vectors (actually, only the difference of the y components) to determine the rotation component.
//Because drift happens when you shove both forward, we'll threshold it heavily.
float rotation = thresholding((y1 - y2)/2.0, 20);
//-------------NORMALIZE-------------//
//If it's potentially going above our artificially imposed max-value (to help PID or something), block it from doing so.
//Otherwise, just let the speed be proportional to the joystick value.
//As a nice side effect, the division-by-zero isn't a problem anymore (infinity is apparently signed)
float JoyToWheel = min(MAX_MOTOR_VALUE / (abs(translationY) + abs(translationX) + abs(rotation)), 1.0);
translationX *= JoyToWheel;
translationY *= JoyToWheel;
rotation *= JoyToWheel;
//-------------INTEGRATE COMPONENTS-------------// (after all changes to the values are done, but before any changes to the frame of reference - because we want this to be relative to the robot's frame (right?))
//What even are the units for these? Gyro is in degrees, but fwd, side, rot are in... motorspeed*second. Whatever that is.
//AndyMark NeveRest motors supposedly do 129 RPM, but also 150RPM. The mecanums are ~10.5cm (diameter).
//We'll just have to figure out the constant factors empirically.
nxtDisplayTextLine(1,"'Encoders'");
nxtDisplayTextLine(2,"Gyro: %f", gyrointr.integral);
nxtDisplayTextLine(3,"Fwd: %f", integrate(fwdintr, translationX));
nxtDisplayTextLine(4,"Side: %f", integrate(sideintr, translationY));
nxtDisplayTextLine(5,"Rot: %f", integrate(rotintr, rotation));
//For encoding, reverse-solving the motor formulae:
//Would using these instead of using the original translationX translationY rotation values be more accurate?
//double deltaFwd = k * (deltaEncFL + deltaEncFR)/2.0;
//double deltaSide = k * (deltaEncFL - deltaEncBL)/2.0;
//double deltaRot = k * (deltaEncFR - deltaEncBL)/2.0;
//-------------ROTATION-INVARIANCE-------------// (has to be after integration of components, because this changes the frame of reference to the absolute frame)
//Gyro reading is in degrees, so the integral is too.
float currtheta = integrate(gyrointr, HTGYROreadRot(gyro));
//Translation is now invariant to rotation. Thus, when you do the integrations above it'll actually be moving the thing forward.
//rotateXYVecDeg(translationX, translationY, currtheta);
//Drivers don't like this, so it's commented out. But feel free to uncomment.
//-------------SET MOTORS-------------//
//we originally thresholded to protect motors from fluctuations; now we don't so motors don't lock up and drag, because Mecanum is sensitive.
nonviolence(motorFrontLeft, translationY + translationX + rotation, NV_MODE_LINEAR);
nonviolence(motorFrontRight, translationY - translationX - rotation, NV_MODE_LINEAR);
nonviolence(motorBackLeft, translationY - translationX + rotation, NV_MODE_LINEAR);
nonviolence(motorBackRight, translationY + translationX - rotation, NV_MODE_LINEAR);
//I have strong doubts that nonviolence will work without bigtime messing up encoders. And maybe everything. Change to NV_MODE_STEP if so.
//And wait so we don't do so much load on the NXT (?), or just overcalculation on the integral might mess it up because of time granularity
wait1Msec(10);
}
}
SEXP trainR(SEXP pm, SEXP pn, SEXP pnz, SEXP pval, SEXP pjdx, SEXP prdx,
SEXP pb, SEXP plambda, SEXP pqflag, SEXP prflag, SEXP psflag,
SEXP phflag, SEXP pvval, SEXP ptol, SEXP pktol)
#endif
{
/* problem data */
dmatrix *matX; /* feature matrix */
double *b; /* class vector */
double lambda; /* regularization parameter */
train_opts to; /* training options */
double *solution; /* entry 1 : intercept, */
/* entries 2~n+1 : coefficients */
char *ifile_x, *ifile_y, *ofile;
int rflag; /* relative lambda flag */
int hflag; /* histogram & threshold flag */
double lambda_max;
clock_t clock_pre, clock_sol, clock_wri, clock_end;
int total_nt, total_pcg, ret;
clock_pre = clock();
#ifndef Rpackage
parse_command_line_args(argc, argv, &lambda, &to, &rflag, &hflag,
&ifile_x, &ifile_y, &ofile);
/* read data file */
if (to.verbose_level>=2) fprintf(stderr,"\nReading data...\n\n");
read_mm_new_matrix(ifile_x, &matX);
read_mm_new_vector(ifile_y, &b);
#else
convert_Rdata(pm,pn,pnz,pval,pjdx,prdx,pb,plambda,pqflag,prflag,psflag,
phflag,pvval,ptol,pktol,&matX,&b,&lambda,&to,&hflag,&rflag);
#endif
lambda_max = find_lambdamax(matX, b, to.sflag);
if (rflag) lambda = lambda*lambda_max;
if (to.verbose_level>=2)
summary_prob(matX, to.sflag, lambda_max, lambda);
/* intercept(1) + coefficients(2..n+1) */
solution = malloc(sizeof(double)*(matX->n+1));
/* run solver */
if (to.verbose_level>=2) fprintf(stderr,"Running solver...\n");
clock_sol = clock();
ret = l1_logreg_train(matX, b, lambda, to, NULL, NULL,
solution, &total_nt, &total_pcg);
//dmat_profile();
/* show status */
if (matX->nz < 0) total_pcg = -1;
show_status(to.verbose_level, ret, total_nt, total_pcg);
/* write solution */
clock_wri = clock();
if (hflag == TRUE) /* manual thresholding */
{
show_histogram(matX->n, solution+1);
thresholding(matX->n, solution+1, userinput_threshold());
show_histogram(matX->n, solution+1);
}
#ifndef Rpackage
if (ofile != NULL)
{
char linebf[BUFFER_SIZE];
sprintf(linebf, comment1,ifile_x, PACKAGE_NAME, VERSION);
write_mm_vector(ofile, matX->n+1, solution, linebf, TYPE_E);
}
/* print info */
clock_end = clock();
if (to.verbose_level>=2)
summary_time(clock_pre,clock_sol,clock_wri,clock_end);
if (to.verbose_level==1)
summary_all(matX->m, matX->n, lambda, lambda_max, clock_wri-clock_sol,
total_nt, total_pcg, solution+1);
free(solution);
free(b);
dmat_free(matX);
return EXIT_SUCCESS;
#else
{
SEXP res;
res = create_Rdata_to_return(matX->n, lambda, solution);
if (matX->nz >= 0) {
free(matX->idx);
free(matX->jdx);
free(matX->rdx);
}
free(matX);
free(solution);
return res;
}
#endif
}
