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 20static 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 finalevoid 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 127void 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 onvoid  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 bankvoid 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 windowsint 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;}