orc.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

	request[PACKET_ID] = PACKET_ID_NO_ACK;
	packet_fill_checksum(request);

	// send the packet
	pthread_mutex_lock(&orc->writeLock);
	write_fully(orc->fd, request, request[PACKET_DATALEN] + 4);
	pthread_mutex_unlock(&orc->writeLock);
}

void orc_transaction_retry(orc_t *orc, uint8_t *request, uint8_t *response)
{
	int res;

loop:
	res = orc_transaction_once(orc, request, response);
	if (res==0)
		return;

	usleep(5000);
	goto loop;
}

#define CMDPROLOGUE uint8_t req[256]; \
	uint8_t resp[256];

void orc_lcd_clear(orc_t *orc)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 1;
	req[PACKET_DATA +0] = CMD_LCD_CLEAR;

	orc_transaction_once(orc, req, resp);
}

void orc_lcd_console_home(orc_t *orc)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 1;
	req[PACKET_DATA +0] = CMD_CONSOLE_HOME;

	orc_transaction_once(orc, req, resp);
}

#define CHUNKSIZE 20

static int min(int a, int b)
{
	return a < b ? a : b;
}

void orc_lcd_console_write(orc_t *orc, const char *fmt, ...)
{
	CMDPROLOGUE;

	VPRINTALLOC; // this sets up variables 'buf' and 'len'

	for (int i = 0; i < len; i+=CHUNKSIZE) {
		int thislen = min(len-i, CHUNKSIZE);

		req[PACKET_DATALEN] = 1+thislen;
		req[PACKET_DATA + 0] = CMD_CONSOLE_WRITECHARS;
		memcpy(&req[PACKET_DATA + 1], &buf[i], thislen);

		orc_transaction_once(orc, req, resp);
	}

	free(buf);
}

void orc_lcd_draw_string(orc_t *orc, int x, int y, int font, const char *fmt, ...)
{
	CMDPROLOGUE;

	VPRINTALLOC; // this sets up variables 'buf' and 'len'

	for (int i = 0; i < len; i+=CHUNKSIZE) {
		int thislen = min(len - i, CHUNKSIZE);

		req[PACKET_DATALEN] = thislen + 4;
		req[PACKET_DATA + 0] = CMD_LCD_DRAWCHARS;
		req[PACKET_DATA + 1] = x;
		req[PACKET_DATA + 2] = y;
		req[PACKET_DATA + 3] = font;
		memcpy(&req[PACKET_DATA + 4], &buf[i], thislen);
		
		orc_transaction_once(orc, req, resp);
		x += resp[PACKET_DATA + 1];
	}

	free(buf);
}

void orc_lcd_console_goto(orc_t *orc, int x, int y)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 3;
	req[PACKET_DATA + 0] = CMD_CONSOLE_GOTO;
	req[PACKET_DATA + 1] = x;
	req[PACKET_DATA + 2] = y;
	orc_transaction_retry(orc, req, resp);	
}

void orc_lcd_write(orc_t *orc, int x, int y, uint8_t *data, uint8_t datalen)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 3 + datalen;
	req[PACKET_DATA + 0] = CMD_LCD_WRITE;
	req[PACKET_DATA + 1] = x;
	req[PACKET_DATA + 2] = y;
	memcpy(&req[PACKET_DATA+3], data, datalen);

	orc_transaction_retry(orc, req, resp);	
}

int orc_analog_read(orc_t *orc, int port)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 2;
	req[PACKET_DATA + 0] = CMD_ANALOG_READ;
	req[PACKET_DATA + 1] = port;
	orc_transaction_retry(orc, req, resp);

	return packet_16u(resp, 1);
}

int orc_quadphase_read(orc_t *orc, int port)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 2;
	req[PACKET_DATA + 0] = CMD_QUADPHASE_READ;
	req[PACKET_DATA + 1] = port;
	orc_transaction_retry(orc, req, resp);

	return packet_16u(resp, 1);
}

void orc_motor_set(orc_t *orc, int port, int spwm)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 3;
	req[PACKET_DATA + 0] = CMD_MOTOR_SET;
	req[PACKET_DATA + 1] = port;
	req[PACKET_DATA + 2] = spwm;
	orc_transaction_retry(orc, req, resp);
}

orc_button_state_t orc_pad_begin_poll(orc_t *orc)
{
	orc_button_state_t s;

	s.up = (orc->pad_updown&0xf0)>>4;
	s.down = orc->pad_updown&0x0f;
	s.left = (orc->pad_leftright&0xf0)>>4;
	s.right = orc->pad_leftright&0x0f;	

	return s;
}

int orc_pad_poll(orc_t *orc, orc_button_state_t *s)
{
	int res = orc->pad_switches;

	int tup = (orc->pad_updown&0xf0)>>4;
	int tdown = orc->pad_updown&0x0f;
	int tleft = (orc->pad_leftright&0xf0)>>4;
	int tright = orc->pad_leftright&0x0f;
	
	if (tup != s->up)
		res |= ORC_PAD_UP;
	if (tdown != s->down)
		res |= ORC_PAD_DOWN;
	if (tleft != s->left)
		res |= ORC_PAD_LEFT;
	if (tright != s->right)
		res |= ORC_PAD_RIGHT;
	
	s->up = tup;
	s->down = tdown;
	s->right = tright;
	s->left = tleft;

	return res;
}

int orc_pad_switches(orc_t *orc)
{
	return orc->pad_switches;
}

int orc_pad_gets(orc_t *orc)
{
	orc_button_state_t s = orc_pad_begin_poll(orc);
	int res = 0;

	pthread_mutex_lock(&orc->pad_mutex);
	while ((res = orc_pad_poll(orc, &s))==0) {
		pthread_cond_wait(&orc->pad_cond, &orc->pad_mutex);
	}
	pthread_mutex_unlock(&orc->pad_mutex);

	return res;
}

int orc_in_menu(orc_t *orc)
{
	return (orc->heartbeat_flags&1)!=0;
}

int orc_pad_connected(orc_t *orc)
{
	return (orc->heartbeat_flags&2)!=0;
}

void orc_null(orc_t *orc, int reqlen, int resplen)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 2+reqlen;
	req[PACKET_DATA + 0] = CMD_ORC_NULL;
	req[PACKET_DATA + 1] = resplen;

	orc_transaction_retry(orc, req, resp);	
}

// added by finale
void orc_pinmode_set( orc_t *orc, int port, int mode){

	CMDPROLOGUE;
	
	req[PACKET_DATALEN] = 3;
	req[PACKET_DATA + 0] = CMD_PINMODE_SET;
	req[PACKET_DATA + 1] = port;
	req[PACKET_DATA + 2] = mode;
	orc_transaction_retry(orc, req, resp);
}


// this takes in an int btw -128 and 127
void orc_motor_set_signed( orc_t *orc, int port, int pwm )
{
  // set the signed pwm value
  int spwm  = pwm;
  if( pwm < 0 ){
    spwm = -1 * ( 128 + pwm );  // twos compliment conversion, note pwm is neg
  }
  orc_motor_set( orc, port, spwm );
}


int  orc_sonar_read(orc_t *orc __attribute__ ((unused)), int port __attribute__ ((unused))){
  return -1;
}


int  orc_digital_read(orc_t *orc, int port){
  
        orc_pinmode_set( orc, port, DIGIN );

	CMDPROLOGUE;

	req[PACKET_DATALEN] = 2;
	req[PACKET_DATA + 0] = CMD_DIGIN_READ;
	req[PACKET_DATA + 1] = port;
	orc_transaction_retry(orc, req, resp);

	return packet_16u(resp, 1);
}

void  orc_digital_set(orc_t *orc, int port, int val){
  
        orc_pinmode_set( orc, port, DIGOUT );

	CMDPROLOGUE;
	
	req[PACKET_DATALEN] = 3;
	req[PACKET_DATA + 0] = CMD_DIGOUT_SET;
	req[PACKET_DATA + 1] = port;
	req[PACKET_DATA + 2] = val;
	orc_transaction_retry(orc, req, resp);
}


// between 0 and 1; 1 is full cycle on
void  orc_pwm_set(orc_t *orc, int port, float v){

        orc_pinmode_set( orc, port, PWM );

	CMDPROLOGUE;
	
	int pwm = (int) (v*65535);
	
	req[PACKET_DATALEN] = 4;
	req[PACKET_DATA + 0] = CMD_SERVO_SET;
	req[PACKET_DATA + 1] = port;
	req[PACKET_DATA + 2] = pwm >> 8 ;
	req[PACKET_DATA + 3] = pwm & 0xff ;
	orc_transaction_retry(orc, req, resp);
}

// warning: see orc manual: sets clock for entire pwm bank
void orc_clk_set(orc_t *orc, int port, int divider){

        CMDPROLOGUE;

	int chn = port/4;

	req[PACKET_DATALEN] = 3;
	req[PACKET_DATA + 0] = CMD_PWMCLK_SET;
	req[PACKET_DATA + 1] = chn;
	req[PACKET_DATA + 2] = divider&0xff;
	orc_transaction_retry(orc, req, resp);
}

// reads ticks in 60 Hz windows
int orc_quadphase_read_velocity(orc_t *orc, int port)
{
	CMDPROLOGUE;

	req[PACKET_DATALEN] = 2;
	req[PACKET_DATA + 0] = CMD_QUADPHASE_READ;
	req[PACKET_DATA + 1] = port;
	orc_transaction_retry(orc, req, resp);

	return packet_16u(resp, 3);
}


int orc_quadphase_read_velocity_signed( orc_t *orc, int port )
{
  // set the signed pwm value
  int v  = orc_quadphase_read_velocity( orc, port );
  short real_velocity = v & 0x0000FFFF; 
  return (int) real_velocity;
}