static void
handle_legacy_disk(struct bregs *regs, u8 extdrive)
{
if (! CONFIG_DRIVES) {
// XXX - support handle_1301 anyway?
disk_ret(regs, DISK_RET_EPARAM);
return;
}
if (extdrive < EXTSTART_HD) {
struct drive_s *drive_g = getDrive(EXTTYPE_FLOPPY, extdrive);
if (!drive_g)
goto fail;
floppy_13(regs, drive_g);
return;
}
struct drive_s *drive_g;
if (extdrive >= EXTSTART_CD)
drive_g = getDrive(EXTTYPE_CD, extdrive - EXTSTART_CD);
else
drive_g = getDrive(EXTTYPE_HD, extdrive - EXTSTART_HD);
if (!drive_g)
goto fail;
disk_13(regs, drive_g);
return;
fail:
// XXX - support 1301/1308/1315 anyway?
disk_ret(regs, DISK_RET_EPARAM);
}
bool DriveManagement::isSameDrive(const QString &file1,const QString &file2) const
{
if(mountSysPoint.size()==0)
{
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,QStringLiteral("no mount point found"));
return false;
}
const QString &drive1=getDrive(file1);
if(drive1.isEmpty())
{
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,QStringLiteral("drive for the file1 not found: %1").arg(file1));
return false;
}
const QString &drive2=getDrive(file2);
if(drive2.isEmpty())
{
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,QStringLiteral("drive for the file2 not found: %1").arg(file2));
return false;
}
ULTRACOPIER_DEBUGCONSOLE(Ultracopier::DebugLevel_Notice,QStringLiteral("%1 is egal to %2?").arg(drive1).arg(drive2));
if(drive1==drive2)
return true;
else
return false;
}
static int rf_encoder_changed(void *data) {
int pos=*(int*)data;
if(pos!=0) {
if(function || tune) {
// tune drive
double d=getTuneDrive();
d+=(double)pos;
if(d<0.0) {
d=0.0;
} else if(d>100.0) {
d=100.0;
}
set_tune(d);
} else {
// drive
double d=getDrive();
d+=(double)pos;
if(d<0.0) {
d=0.0;
} else if(d>100.0) {
d=100.0;
}
set_drive(d);
}
}
free(data);
return 0;
}
int getDriveId(u8 exttype, struct drive_s *drive_g)
{
int i;
for (i = 0; i < ARRAY_SIZE(IDMap[0]); i++)
if (getDrive(exttype, i) == drive_g)
return i;
return -1;
}
static int getPathLen(direntry_t *entry)
{
int length=0;
while(1) {
if(entry->entry == -3) /* rootDir */
return strlen(getDrive(entry->Dir)) + 1 + length + 1;
length += 1 + strlen(entry->name);
entry = getDirentry(entry->Dir);
}
}
static char *sprintPwd(direntry_t *entry, char *ptr)
{
if(entry->entry == -3) {
strcpy(ptr, getDrive(entry->Dir));
strcat(ptr, ":/");
ptr = strchr(ptr, 0);
} else {
ptr = sprintPwd(getDirentry(entry->Dir), ptr);
if(ptr[-1] != '/')
*ptr++ = '/';
strcpy(ptr, entry->name);
ptr += strlen(entry->name);
}
return ptr;
}
static void _fprintPwd(FILE *f, direntry_t *entry, int recurs, int escape)
{
if(entry->entry == -3) {
fputs(getDrive(entry->Dir), f);
putc(':', f);
if(!recurs)
putc('/', f);
} else {
_fprintPwd(f, getDirentry(entry->Dir), 1, escape);
if (escape && strpbrk(entry->name, NEED_ESCAPE)) {
char *ptr;
for(ptr = entry->name; *ptr; ptr++) {
if (strchr(NEED_ESCAPE, *ptr))
putc('\\', f);
putc(*ptr, f);
}
} else {
fprintf(f, "/%s", entry->name);
}
}
}
Stream_t *open_root_dir(char *drive, int flags)
{
Stream_t *Fs;
int i, k;
init_streamcache();
k = -1;
for(i=0; i<256; i++) {
if (fss[i] == NULL || strcmp(getDrive(fss[i]), drive) == 0) {
k = i;
break;
}
}
if(k == -1) {
fprintf(stderr, "Cannot initialize '%s:', out of table space\n",
drive);
return NULL;
}
/* open the drive */
if(fss[k])
Fs = fss[k];
else {
Fs = fs_init(drive, flags);
if (!Fs){
fprintf(stderr, "Cannot initialize '%s:'\n", drive);
return NULL;
}
fss[k] = Fs;
}
return OpenRoot(Fs);
}
static int enterDirectory(Stream_t *Dir)
{
int r;
char drive;
if(currentDir == Dir)
return 0; /* still the same directory */
leaveDirectory(0);
drive = getDrive(Dir);
r=enterDrive(Dir, drive);
if(r)
return r;
currentDir = COPY(Dir);
dynDirPath = getPwd(getDirentry(Dir));
if(!dynDirPath)
dirPath=emptyString;
else {
if(!dynDirPath[3] && concise)
dynDirPath[2]='\0';
dirPath=dynDirPath;
}
/* print directory title */
if(!concise)
printf("\nDirectory for %s\n", dirPath);
if(!wide && !concise)
printf("\n");
dirsOnDrive++;
bytesInDir = 0;
filesInDir = 0;
return 0;
}
static int enterDirectory(Stream_t *Dir)
{
int r;
char *drive;
char *slash;
if(currentDir == Dir)
return 0; /* still the same directory */
leaveDirectory(0);
drive = getDrive(Dir);
r=enterDrive(Dir, drive);
if(r)
return r;
currentDir = COPY(Dir);
dirPath = getPwd(getDirentry(Dir));
if(!dirPath)
dirPath=emptyString;
if(concise &&
(slash = strrchr(dirPath, '/')) != NULL && slash[1] == '\0')
*slash = '\0';
/* print directory title */
if(!concise)
printf("\nDirectory for %s\n", dirPath);
if(!wide && !concise)
printf("\n");
dirsOnDrive++;
bytesInDir = 0;
filesInDir = 0;
return 0;
}
static void run(void)
{
robot_console_printf("\n\n\n");
int i;
float y[2*NB_CPG];
float dy[2*NB_CPG];
float x[NB_CPG];
float delta = 0.0;
float speed = 0.0;
float drive =0.0;
char speed_char[10];
char drive_char[10];
/*for (i = 0;i<7;i++) {
old_pos[i] = current_position[i];
}*/
/*float initial_conditions[2*NB_CPG]={0.0, 0.0, 0.0, 0.0,
0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1,
0.0, 0.0, 0.0, 0.0,
0.2, 0.1};*/
//base values
//FILE* fileID = fopen("recherche_syst.m", "w");
//fprintf(fileID, "Z = [ ");
float v;
float R[NB_CPG];
for(i=0; i<2*NB_CPG; ++i)
{
y[i] = 0.2; //initial_conditions[i]; // (float)(9-(i/2))/10.0;
dy[i] = 0.0;
}
// run TIME miliseconds
for(i = 0 ; i < TIME ; i += TIME_STEP)
{
drive=getDrive(drive);
v = drive;
set_ampl(v, R);
fcn(y, dy, v, R, current_movement);
euler(y,dy,TIME_STEP);
next_step(x,y);
//fprintf(fileID," %f %f %f %f %f %f ;\n", y[2]-y[0], y[4]-y[0], y[6]-y[2], y[6]-y[4], y[6]-y[0], y[2]-y[4]);
// Calculate speed
if (!(i%(TIME_STEP*50))) {
//Every 50 TIME_STEP to avoid oscillations (= 800ms)
delta = pow(pow((current_position[0]- old_pos[0]),2) + pow((current_position[2]-old_pos[2]),2), 0.5);
old_pos[0] = current_position[0];
old_pos[2] = current_position[2];
speed = 1000*delta/((float)50*TIME_STEP);
}
//robot_console_printf("Speed : %d, ,%f, %f, %f, %f\n", i, delta, speed, old_pos[0], current_position[0]);
elapsed_time += (float) TIME_STEP / 1000;
/*if (elapsed_time > 5.0 && abs(delta) < 0.000001) {//abs(current_position[6]) > 2) {
break;
}*/
// Print informations
sprintf(drive_char, "%.2f", drive);
sprintf(speed_char, "%.2f", speed);
supervisor_set_label(3, drive_char, 0.16, 0.01, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(4, getCurrentMovementString(), 0.16, 0.045, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(5, speed_char, 0.16, 0.08, 0.04, 0xff0000); /* red speed */
// actuate front legs
servo_set_position(servos[0], x[0]);
servo_set_position(servos[2], x[2]);
//actuate front kmees
servo_set_position(servos[5], x[4]);
servo_set_position(servos[7], x[6]);
// actuate back legs
servo_set_position(servos[1], x[1]);
servo_set_position(servos[3], x[3]);
//actuate back knees
servo_set_position(servos[6], x[5]);
servo_set_position(servos[8], x[7]);
//actuate spine
servo_set_position(servos[4], x[8]);
robot_step(TIME_STEP);
}
// Reset
servo_set_position(servos[0], 0);
servo_set_position(servos[1], 0);
servo_set_position(servos[2], 0);
servo_set_position(servos[3], 0);
servo_set_position(servos[4], 0);
servo_set_position(servos[5], 0);
servo_set_position(servos[6], 0);
servo_set_position(servos[7], 0);
servo_set_position(servos[8], 0);
const float FLOAT_POS[7] = { 0, 1000, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)FLOAT_POS);
robot_step(10*TIME_STEP);
const float INITIAL_POSITION_AND_ROTATION[7] = { 0, 0.3, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)INITIAL_POSITION_AND_ROTATION);
elapsed_time = 0.0;
//fclose(fileID);
}
/**
* Makes sure the path of the filename exists. If it doesn't, create the path.
*/
void Filename::verifyAndCreatePath(void) const
{
#if defined(WIN32)
if (WindowsUnicode)
{
char *buffer = NULL;
DWORD buflen;
// get our current path
buflen = GetFullPathName(".", 0, buffer, NULL);
buffer = new char[buflen + 1];
buflen = GetFullPathName(".", buflen + 1, buffer, NULL);
Unicode::String srcPath = Unicode::narrowToWide(buffer);
delete[] buffer;
// get the destination path
std::string correctPath(getDrive() + getPath());
buflen = GetFullPathName(correctPath.c_str(), 0, buffer, NULL);
buffer = new char[buflen + 1];
buflen = GetFullPathName(correctPath.c_str(), buflen + 1, buffer, NULL);
Unicode::String destPath = Unicode::narrowToWide(buffer);
delete[] buffer;
std::vector<Unicode::String> splitDestPath;
splitPath(destPath, splitDestPath,
static_cast<Unicode::unicode_char_t>(WIN32_PATH_SEPARATOR));
Unicode::String prefix = L"\\\\?\\";
destPath = prefix + splitDestPath[0];
for (size_t i = 1; i < splitDestPath.size(); ++i)
{
destPath += WIN32_PATH_SEPARATOR;
destPath += splitDestPath[i];
if (SetCurrentDirectoryW((LPCWSTR)destPath.c_str()) == 0)
{
if (CreateDirectoryW((LPCWSTR)destPath.c_str(), NULL) == 0)
return;
if (SetCurrentDirectoryW((LPCWSTR)destPath.c_str()) == 0)
return;
}
}
// set the directory back to the working directory
SetCurrentDirectoryW(srcPath.c_str());
}
else
{
char *buffer = NULL;
DWORD buflen;
// get our current path
buflen = GetFullPathName(".", 0, buffer, NULL);
buffer = new char[buflen + 1];
buflen = GetFullPathName(".", buflen + 1, buffer, NULL);
std::string srcPath = buffer;
delete[] buffer;
// get the destination path
std::string correctPath(getDrive() + getPath());
buflen = GetFullPathName(correctPath.c_str(), 0, buffer, NULL);
buffer = new char[buflen + 1];
buflen = GetFullPathName(correctPath.c_str(), buflen + 1, buffer, NULL);
std::string destPath = buffer;
delete[] buffer;
// change to the destination path
if (destPath.size() >= MAX_PATH)
return;
std::vector<std::string> splitDestPath;
splitPath(destPath, splitDestPath, WIN32_PATH_SEPARATOR);
destPath = splitDestPath[0];
for (size_t i = 1; i < splitDestPath.size(); ++i)
{
destPath += WIN32_PATH_SEPARATOR;
destPath += splitDestPath[i];
if (SetCurrentDirectory(destPath.c_str()) == 0)
{
if (CreateDirectory(destPath.c_str(), NULL) == 0)
return;
if (SetCurrentDirectory(destPath.c_str()) == 0)
return;
}
}
// set the directory back to the working directory
SetCurrentDirectory(srcPath.c_str());
}
#elif defined(linux)
// @todo: implement for linux
#endif
} // Filename::verifyAndCreatePath
void Object_Menu_Control::eject_bubble(void) {
write_protect = false;
emit sig_eject_bubble(getDrive());
}
void Object_Menu_Control::insert_bubble(void) {
emit sig_insert_bubble(getDrive());
}
