/* Drive the robot with a fixed linear speed, and turning through a fixed radius.
Do not call this function for the special cases of driving straight, or clockwise
and counter-clockwise turning; this function instructs the robot to drive only nonzero radii. */
extern int32_t irobotDriveRadius(
const irobotUARTPort_t port, /* (in) UART port */
int16_t velocity, /* (in) Velocity, in mm/s */
int16_t radius /* (in) Radius, in mm */
){ /* (ret) Error / success code */
uint8_t packet[OP_DRIVE_SIZE];
velocity = coerce(ACTUATOR_WHEEL_SPEED_MIN, velocity, ACTUATOR_WHEEL_SPEED_MAX);
radius = coerce(ACTUATOR_DRIVE_RADIUS_MIN, radius, ACTUATOR_DRIVE_RADIUS_MAX);
/* Special case: radius = 1mm is interpreted as CCW rotation;
iRobot Drive CCW covers this case, so a drive radius of 1mm is
interpreted as the next smallest radius, 2mm */
if(radius == 1){
radius = 2;
}
packet[0] = OP_DRIVE;
packet[1] = HO(velocity);
packet[2] = LO(velocity);
packet[3] = HO(radius);
packet[4] = LO(radius);
return irobotUARTWriteRaw(port, packet, OP_DRIVE_SIZE);
}
/* Drives in a fixed direction */
extern int32_t irobotDriveDirection(
const irobotUARTPort_t port, /* (in) UART port */
int16_t velocity, /* (in) Velocity, in mm/s */
const irobotDirection_t direction /* (in) direction */
){ /* (ret) Error / success code */
uint8_t packet[OP_DRIVE_SIZE];
velocity = coerce(ACTUATOR_WHEEL_SPEED_MIN, velocity, ACTUATOR_WHEEL_SPEED_MAX);
packet[0] = OP_DRIVE;
packet[1] = HO(velocity);
packet[2] = LO(velocity);
packet[3] = HO((uint16_t)direction);
packet[4] = LO((uint16_t)direction);
return irobotUARTWriteRaw(port, packet, OP_DRIVE_SIZE);
}
/* Directly actuate left and right wheels. */
extern int32_t irobotDriveDirect(
const irobotUARTPort_t port, /* (in) UART port */
int16_t leftWheelSpeed, /* (in) Left wheels speed, in mm/s */
int16_t rightWheelSpeed /* (in) Right wheel speed, in mm/s */
){ /* (ret) Error / success code */
uint8_t packet[OP_DRIVE_DIRECT_SIZE];
leftWheelSpeed = coerce(ACTUATOR_WHEEL_SPEED_MIN, leftWheelSpeed, ACTUATOR_WHEEL_SPEED_MAX);
rightWheelSpeed = coerce(ACTUATOR_WHEEL_SPEED_MIN, rightWheelSpeed, ACTUATOR_WHEEL_SPEED_MAX);
packet[0] = OP_DRIVE_DIRECT;
packet[1] = HO(rightWheelSpeed);
packet[2] = LO(rightWheelSpeed);
packet[3] = HO(leftWheelSpeed);
packet[4] = LO(leftWheelSpeed);
return irobotUARTWriteRaw(port, packet, OP_DRIVE_DIRECT_SIZE);
}
static int
command_mode(int argc, char *argv[])
{
UINTN cols, rows;
unsigned int mode;
int i;
char *cp;
char rowenv[8];
EFI_STATUS status;
SIMPLE_TEXT_OUTPUT_INTERFACE *conout;
extern void HO(void);
conout = ST->ConOut;
if (argc > 1) {
mode = strtol(argv[1], &cp, 0);
if (cp[0] != '\0') {
printf("Invalid mode\n");
return (CMD_ERROR);
}
status = conout->QueryMode(conout, mode, &cols, &rows);
if (EFI_ERROR(status)) {
printf("invalid mode %d\n", mode);
return (CMD_ERROR);
}
status = conout->SetMode(conout, mode);
if (EFI_ERROR(status)) {
printf("couldn't set mode %d\n", mode);
return (CMD_ERROR);
}
sprintf(rowenv, "%u", (unsigned)rows);
setenv("LINES", rowenv, 1);
HO(); /* set cursor */
return (CMD_OK);
}
printf("Current mode: %d\n", conout->Mode->Mode);
for (i = 0; i <= conout->Mode->MaxMode; i++) {
status = conout->QueryMode(conout, i, &cols, &rows);
if (EFI_ERROR(status))
continue;
printf("Mode %d: %u columns, %u rows\n", i, (unsigned)cols,
(unsigned)rows);
}
if (i != 0)
printf("Select a mode with the command \"mode <number>\"\n");
return (CMD_OK);
}
/* Emulate basic capabilities of cons25 terminal */
void
vidc_term_emu(int c)
{
static int ansi_col[] = {
0, 4, 2, 6, 1, 5, 3, 7,
};
int t;
int i;
switch (esc) {
case 0:
switch (c) {
case '\033':
esc = c;
break;
default:
vidc_rawputchar(c);
break;
}
break;
case '\033':
switch (c) {
case '[':
esc = c;
args[0] = 0;
argc = -1;
break;
default:
bail_out(c);
break;
}
break;
case '[':
switch (c) {
case ';':
if (argc < 0) /* XXX */
argc = 0;
else if (argc + 1 >= MAXARGS)
bail_out(c);
else
args[++argc] = 0;
break;
case 'H':
if (argc < 0)
HO();
else if (argc == 1)
CM();
else
bail_out(c);
break;
case 'J':
if (argc < 0)
CD();
else
bail_out(c);
break;
case 'm':
if (argc < 0) {
fg_c = DEFAULT_FGCOLOR;
bg_c = DEFAULT_BGCOLOR;
}
for (i = 0; i <= argc; ++i) {
switch (args[i]) {
case 0: /* back to normal */
fg_c = DEFAULT_FGCOLOR;
bg_c = DEFAULT_BGCOLOR;
break;
case 1: /* bold */
fg_c |= 0x8;
break;
case 4: /* underline */
case 5: /* blink */
bg_c |= 0x8;
break;
case 7: /* reverse */
t = fg_c;
fg_c = bg_c;
bg_c = t;
break;
case 30: case 31: case 32: case 33:
case 34: case 35: case 36: case 37:
fg_c = ansi_col[args[i] - 30];
break;
case 39: /* normal */
fg_c = DEFAULT_FGCOLOR;
break;
case 40: case 41: case 42: case 43:
case 44: case 45: case 46: case 47:
bg_c = ansi_col[args[i] - 40];
break;
case 49: /* normal */
bg_c = DEFAULT_BGCOLOR;
break;
}
}
end_term();
break;
default:
if (isdigit(c))
get_arg(c);
else
bail_out(c);
break;
}
break;
default:
bail_out(c);
break;
}
}
void xqueue_push16(xqueue_t * const queue, const uint16_t value){
xqueue_push8(queue, HO(value));
xqueue_push8(queue, LO(value));
}
/* Emulate basic capabilities of cons25 terminal */
void
vidc_term_emu(int c)
{
if(!esc) {
if(c=='\033') {
esc=1;
} else {
vidc_rawputchar(c);
}
return;
}
/* Do ESC sequences processing */
switch(c) {
case '\033':
/* ESC in ESC sequence - error */
bail_out(c);
break;
case '[':
/* Check if it's first char after ESC */
if(argc<0) {
br=1;
} else {
bail_out(c);
}
break;
case 'H':
/* Emulate \E[H (cursor home) and
* \E%d;%dH (cursor absolute move) */
if(br) {
switch(argc) {
case -1:
HO();
break;
case 1:
if(fg) args[0]+=pow10(dig)*3;
if(bg) args[0]+=pow10(dig)*4;
CM();
break;
default:
bail_out(c);
}
} else bail_out(c);
break;
case 'J':
/* Emulate \EJ (clear to end of screen) */
if(br && argc<0) {
CD();
} else bail_out(c);
break;
case ';':
/* perhaps args separator */
if(br && (argc>-1)) {
argc++;
} else bail_out(c);
break;
case 'm':
/* Change char attributes */
if(br) {
switch(argc) {
case -1:
ME();
break;
case 0:
if(fg) AF();
else AB();
break;
default:
bail_out(c);
}
} else bail_out(c);
break;
default:
if(isdigit(c)) {
/* Carefully collect numeric arguments */
/* XXX this is ugly. */
if(br) {
if(argc==-1) {
argc=0;
args[argc]=0;
dig=0;
/* in case we're in error... */
if(c=='3') {
fg=1;
return;
}
if(c=='4') {
bg=1;
return;
}
args[argc]=(int)(c-'0');
dig=1;
args[argc+1]=0;
} else {
args[argc]=args[argc]*10+(int)(c-'0');
if(argc==0) dig++;
}
} else bail_out(c);
} else bail_out(c);
break;
}
}