void i2c_process(void) {
//###Process incoming I²C-Data###
//Target Colors
if(twibuffer[0]<=170) target[0] = twibuffer[0];
if(twibuffer[1]<=170) target[1] = twibuffer[1];
if(twibuffer[2]<=170) target[2] = twibuffer[2];
//Fade type
fade[0] = twibuffer[3];
fade[1] = twibuffer[4];
fade[2] = twibuffer[5];
//###Write new values to twibuffer so we can read current status###
twibuffer[6] = color[0];
twibuffer[7] = color[1];
twibuffer[8] = color[2];
twibuffer[9] = LOW_BYTE(colort[0]);
twibuffer[10] = HIGH_BYTE(colort[0]);
twibuffer[11] = LOW_BYTE(colort[1]);
twibuffer[12] = HIGH_BYTE(colort[1]);
twibuffer[13] = LOW_BYTE(colort[2]);
twibuffer[14] = HIGH_BYTE(colort[2]);
}
static void lcm_update_black(unsigned int x, unsigned int y,unsigned int width, unsigned int height, unsigned short data)
{
unsigned int x0 = x;
unsigned int y0 = y;
unsigned int x1 = x0 + width;
unsigned int y1 = y0 + height + 2;
unsigned int k, i;
set_lcm_register(0x2A00, HIGH_BYTE(x0), 0);
set_lcm_register(0x2A01, LOW_BYTE(x0), 0);
set_lcm_register(0x2A02, HIGH_BYTE(x1), 0);
set_lcm_register(0x2A03, LOW_BYTE(x1), 0);
set_lcm_register(0x2B00, HIGH_BYTE(y0), 0);
set_lcm_register(0x2B01, LOW_BYTE(y0), 0);
set_lcm_register(0x2B02, HIGH_BYTE(y1), 0);
set_lcm_register(0x2B03, LOW_BYTE(y1), 0);
send_ctrl_cmd(0x2C00);
for (i = x0; i < x1; i++)
{
for (k = y0; k < y1; k++)
{
send_data_cmd(data);
send_data_cmd(data);
}
}
}
static void get_default_name (char *default_name, zword addr)
{
if (addr != 0) {
zbyte len;
int i;
LOW_BYTE (addr, len);
addr++;
for (i = 0; i < len; i++) {
zbyte c;
LOW_BYTE (addr, c);
addr++;
if (c >= 'A' && c <= 'Z')
c += 'a' - 'A';
default_name[i] = c;
}
default_name[i] = 0;
if (strchr (default_name, '.') == NULL)
strcpy (default_name + i, ".AUX");
} else strcpy (default_name, auxilary_name);
}/* get_default_name */
/*
* z_copy_table, copy a table or fill it with zeroes.
*
* zargs[0] = address of table
* zargs[1] = destination address or 0 for fill
* zargs[2] = size of table
*
* Note: Copying is safe even when source and destination overlap; but
* if zargs[1] is negative the table _must_ be copied forwards.
*
*/
void z_copy_table (void)
{
zword addr;
zword size = zargs[2];
zbyte value;
int i;
if (zargs[1] == 0) /* zero table */
for (i = 0; i < size; i++)
storeb ((zword) (zargs[0] + i), 0);
else if ((short) size < 0 || zargs[0] > zargs[1]) /* copy forwards */
for (i = 0; i < (((short) size < 0) ? - (short) size : size); i++) {
addr = zargs[0] + i;
LOW_BYTE (addr, value)
storeb ((zword) (zargs[1] + i), value);
}
else /* copy backwards */
for (i = size - 1; i >= 0; i--) {
addr = zargs[0] + i;
LOW_BYTE (addr, value)
storeb ((zword) (zargs[1] + i), value);
}
}/* z_copy_table */
static void lcm_update_black(unsigned int x, unsigned int y,unsigned int width, unsigned int height, unsigned short data)
{
unsigned int x0 = x;
unsigned int y0 = y;
unsigned int x1 = x0 + width;
unsigned int y1 = y0 + height + 2;
unsigned int k, i;
send_ctrl_cmd(0x2A);
send_data_cmd(HIGH_BYTE(x0));
send_data_cmd(LOW_BYTE(x0));
send_data_cmd(HIGH_BYTE(x1));
send_data_cmd(LOW_BYTE(x1));
send_ctrl_cmd(0x2B);
send_data_cmd(HIGH_BYTE(y0));
send_data_cmd(LOW_BYTE(y0));
send_data_cmd(HIGH_BYTE(y1));
send_data_cmd(LOW_BYTE(y1));
send_ctrl_cmd(0x2C);
for (i = x0; i < x1; i++)
{
for (k = y0; k < y1; k++)
{
send_data_cmd(data);
}
}
}
// script to rotate in place through deg degrees
// deg > 0 turn CCW; deg < 0 turn CW
void create_script_turn(int deg, int speed) { // degrees, vel in mm/sec
CREATE_BUSY;
create_write_byte(152); // start script
create_write_byte(17); // script length
create_write_byte(147); // on pin 20
create_write_byte(0); // output 0
create_write_byte(137); // move
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
if (deg > 0){ // CCW case
create_write_byte(0);
create_write_byte(1); }
else { // CW case
create_write_byte(255);
create_write_byte(255); }
create_write_byte(157); // wait for angle done
create_write_byte(HIGH_BYTE(deg));
create_write_byte(LOW_BYTE(deg));
create_write_byte(137); // stop move
create_write_byte(0); // no speed
create_write_byte(0);
create_write_byte(0); // no angle
create_write_byte(0);
create_write_byte(147); // on pin 20
create_write_byte(0); // output 1
// end of script
create_write_byte(153); // run script
CREATE_FREE;
}
// script to trace an arc of radius rad until deg is reach
// NOTE: if the turn is not in the direction of deg, the arc won't end
void create_script_arc(int rad, int deg, int speed) { // rad in mm, degrees, vel in mm/sec
CREATE_BUSY;
create_write_byte(152); // start script
create_write_byte(17); // script length
create_write_byte(147); // on pin 20
create_write_byte(0); // output 0
create_write_byte(137); // move
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(rad));
create_write_byte(HIGH_BYTE(rad));
create_write_byte(157); // wait for angle done
create_write_byte(HIGH_BYTE(deg));
create_write_byte(LOW_BYTE(deg));
create_write_byte(137); // stop move
create_write_byte(0); // no speed
create_write_byte(0);
create_write_byte(0); // no angle
create_write_byte(0);
create_write_byte(147); // on pin 20
create_write_byte(0); // output 1
// end of script
create_write_byte(153); // run script
CREATE_FREE;
}
// script to move with individual motor control
// dist mm with r_speed mm/sec on right wheel and l_speed on left
void create_script_move_direct(int dist, int r_speed, int l_speed) {
CREATE_BUSY;
create_write_byte(152); // start script
create_write_byte(17); // script length
create_write_byte(147); // on pin 20
create_write_byte(0); // output 0
create_write_byte(145); // move
create_write_byte(HIGH_BYTE(r_speed));
create_write_byte(LOW_BYTE(r_speed));
create_write_byte(HIGH_BYTE(l_speed));
create_write_byte(LOW_BYTE(l_speed));
create_write_byte(156); // wait for distance done
create_write_byte(HIGH_BYTE(dist));
create_write_byte(LOW_BYTE(dist));
create_write_byte(137); // stop move
create_write_byte(0); // no speed
create_write_byte(0);
create_write_byte(0); // no angle
create_write_byte(0);
create_write_byte(147); // on pin 20
create_write_byte(0); // output 1
// end of script
create_write_byte(153); // run script
CREATE_FREE;
}
void _dma_io(u8_t DMA_channel, unsigned char page, unsigned int offset, unsigned int length, u8_t mode)
{
/* Don't let anyone else mess up what we're doing. */
asm_disable_interrupt();
/* Set up the DMA channel so we can use it. This tells the DMA */
/* that we're going to be using this channel. (It's masked) */
io_outb(MaskReg[DMA_channel], 0x04 | DMA_channel);
/* Clear any data transfers that are currently executing. */
io_outb(ClearReg[DMA_channel], 0x00);
/* Send the specified mode to the DMA. */
io_outb(ModeReg[DMA_channel], mode);
/* Send the offset address. The first byte is the low base offset, the */
/* second byte is the high offset. */
io_outb(AddrPort[DMA_channel], LOW_BYTE(offset));
io_outb(AddrPort[DMA_channel], HI_BYTE(offset));
/* Send the physical page that the data lies on. */
io_outb(PagePort[DMA_channel], page);
/* Send the length of the data. Again, low byte first. */
io_outb(CountPort[DMA_channel], LOW_BYTE(length));
io_outb(CountPort[DMA_channel], HI_BYTE(length));
/* Ok, we're done. Enable the DMA channel (clear the mask). */
io_outb(MaskReg[DMA_channel], DMA_channel);
/* Re-enable interrupts before we leave. */
asm_enable_interrupt();
}
// Attributes stored in RAM are always re-initialized upon start-up.
void emAfRf4ceMsoInitAttributes(void)
{
uint8_t i, j;
MEMSET(emAfRf4ceMsoRibAttributes,
0x00,
sizeof(EmAfRf4ceMsoRibAttributes)*EMBER_RF4CE_PAIRING_TABLE_SIZE);
// Initialize the device type of all the peripheral ID entries to 0xFF, which
// means "unused" in this context.
for(i=0; i<EMBER_RF4CE_PAIRING_TABLE_SIZE; i++) {
for(j=0; j<EMBER_AF_PLUGIN_RF4CE_MSO_PERIPHERAL_ID_ENTRIES; j++) {
emAfRf4ceMsoRibAttributes[i].peripheralIds[j].deviceType = 0xFF;
}
// Initialize the validation configuration attribute fields to the default
// values.
emAfRf4ceMsoRibAttributes[i].validationConfiguration[0] =
LOW_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_LINK_LOST_WAIT_TIME_MS);
emAfRf4ceMsoRibAttributes[i].validationConfiguration[1] =
HIGH_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_LINK_LOST_WAIT_TIME_MS);
emAfRf4ceMsoRibAttributes[i].validationConfiguration[2] =
LOW_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_AUTO_CHECK_VALIDATION_PERIOD_MS);
emAfRf4ceMsoRibAttributes[i].validationConfiguration[3] =
HIGH_BYTE(EMBER_AF_PLUGIN_RF4CE_MSO_AUTO_CHECK_VALIDATION_PERIOD_MS);
}
}
static void sw_clear_panel(unsigned int color)
{
unsigned int x0 = 0;
unsigned int y0 = 0;
unsigned int x1 = x0 + FRAME_WIDTH - 1;
unsigned int y1 = y0 + FRAME_HEIGHT - 1;
unsigned int x, y;
send_ctrl_cmd(0x2A);
send_data_cmd(HIGH_BYTE(x0));
send_data_cmd(LOW_BYTE(x0));
send_data_cmd(HIGH_BYTE(x1));
send_data_cmd(LOW_BYTE(x1));
send_ctrl_cmd(0x2B);
send_data_cmd(HIGH_BYTE(y0));
send_data_cmd(LOW_BYTE(y0));
send_data_cmd(HIGH_BYTE(y1));
send_data_cmd(LOW_BYTE(y1));
send_ctrl_cmd(0x2C); // send DDRAM set
// 18-bit mode (256K color) coding
for (y = y0; y <= y1; ++ y) {
for (x = x0; x <= x1; ++ x) {
lcm_util.send_data(color);
}
}
}
void Roomba_Drive(int16_t velocity, int16_t radius )
{
uart_putchar(DRIVE);
uart_putchar(HIGH_BYTE(velocity));
uart_putchar(LOW_BYTE(velocity));
uart_putchar(HIGH_BYTE(radius));
uart_putchar(LOW_BYTE(radius));
}
void Roomba_Direct_Drive(int16_t right_velocity, int16_t left_velocity)
{
uart_putchar(DIRECT_DRIVE);
uart_putchar(HIGH_BYTE(right_velocity));
uart_putchar(LOW_BYTE(right_velocity));
uart_putchar(HIGH_BYTE(left_velocity));
uart_putchar(LOW_BYTE(left_velocity));
}
// this function drives the right wheel at r_speed and the left wheel at l_speed
// speeds for all of these functions are +/-500mm/sec.
void create_drive_direct(int r_speed, int l_speed)
{
CREATE_BUSY;
create_write_byte(145);
create_write_byte(HIGH_BYTE(r_speed));
create_write_byte(LOW_BYTE(r_speed));
create_write_byte(HIGH_BYTE(l_speed));
create_write_byte(LOW_BYTE(l_speed));
CREATE_FREE;
}
// This command drives the robot along a curve with radius (in mm) radius; and at a speed (mm/sec) of speed
// a radius of 32767 will drive the robot straight
// a radius of 1 will spin the robot CCW
// a radius of -1 will spin the robot CW
// Negative radii will be right turns, positive radii left turns
void create_drive (int speed, int radius)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(radius));
create_write_byte(LOW_BYTE(radius));
CREATE_FREE;
}
// special version of command spins robot CCW with the wheels turning at speed speed
void create_spin_CCW (int speed)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(1));
create_write_byte(LOW_BYTE(1));
CREATE_FREE;
}
// special version of command above drives robot at speed speed. Negative speed will drive robot backwards
void create_drive_straight (int speed)
{
CREATE_BUSY;
create_write_byte(137);
create_write_byte(HIGH_BYTE(speed));
create_write_byte(LOW_BYTE(speed));
create_write_byte(HIGH_BYTE(32767));
create_write_byte(LOW_BYTE(32767));
CREATE_FREE;
}
static void
get_string_descriptor()
{
#if OLD_STRING_DESCR
if(LOW_BYTE(usb_setup_buffer.wValue) == 0) {
usb_send_ctrl_response((const unsigned char *)string_languages->
lang_descr, string_languages->lang_descr->bLength);
} else {
const struct usb_st_string_descriptor *descriptor;
unsigned char l;
const struct usb_st_string_descriptor *const *table;
const struct usb_st_string_language_map *map;
if(LOW_BYTE(usb_setup_buffer.wValue) > string_languages->max_index) {
usb_error_stall();
return;
}
l = string_languages->num_lang;
map = string_languages->map;
table = map->descriptors; /* Use first table if language not found */
while(l > 0) {
if(map->lang_id == usb_setup_buffer.wIndex) {
table = map->descriptors;
break;
}
map++;
l--;
}
PRINTF("Lang id %04x = table %p\n", usb_setup_buffer.wIndex,
(void *)table);
descriptor = table[LOW_BYTE(usb_setup_buffer.wValue) - 1];
usb_send_ctrl_response((const unsigned char *)descriptor,
descriptor->bLength);
}
#else
const struct usb_st_string_descriptor *descriptor;
descriptor = (struct usb_st_string_descriptor *)
usb_class_get_string_descriptor(usb_setup_buffer.wIndex,
LOW_BYTE(usb_setup_buffer.wValue));
if(!descriptor) {
usb_error_stall();
return;
}
usb_send_ctrl_response((const unsigned char *)descriptor,
descriptor->bLength);
#endif
}
EmberStatus ezspEndDeviceBindRequest(EmberNodeId localNodeId,
EmberEUI64 localEui64,
uint8_t endpoint,
uint16_t profile,
uint8_t inCount,
uint8_t outCount,
uint16_t *inClusters,
uint16_t *outClusters,
EmberApsOption options)
{
uint8_t i;
uint8_t *payload = zigDevRequestBuffer + ZDO_MESSAGE_OVERHEAD;
uint8_t offset = ZDO_MESSAGE_OVERHEAD + 14; // Add 2 bytes for our NWK Address
// Add 8 bytes for our EUI64
// Add 1 byte for endpoint
// Add 2 bytes for Profile Id
// Add 1 byte for in Cluster Count
uint8_t length = (offset +
(inCount * 2) + // Times 2 for 2 byte Clusters
1 + // Out Cluster Count
(outCount * 2)); // Times 2 for 2 byte Clusters
if (length > EZSP_MAX_FRAME_LENGTH)
return EMBER_NO_BUFFERS;
payload[0] = LOW_BYTE(localNodeId);
payload[1] = HIGH_BYTE(localNodeId);
MEMMOVE(payload + 2, localEui64, 8);
payload[10] = endpoint;
payload[11] = LOW_BYTE(profile);
payload[12] = HIGH_BYTE(profile);
payload[13] = inCount;
for (i = 0; i < inCount; i++) {
zigDevRequestBuffer[(i * 2) + offset] = LOW_BYTE(inClusters[i]);
zigDevRequestBuffer[(i * 2) + offset + 1] = HIGH_BYTE(inClusters[i]);
}
offset += (inCount * 2);
zigDevRequestBuffer[offset] = outCount;
offset++;
for (i = 0; i < outCount; i++) {
zigDevRequestBuffer[(i * 2) + offset] = LOW_BYTE(outClusters[i]);
zigDevRequestBuffer[(i * 2) + offset + 1] = HIGH_BYTE(outClusters[i]);
}
return sendZigDevRequestBuffer(EMBER_ZIGBEE_COORDINATOR_ADDRESS,
END_DEVICE_BIND_REQUEST,
options,
length);
}
/**@brief Function for simulating a bike speed event.
*
* @param[in] p_context Timeout context.
*/
static void speed_pulse_event_simulate(void * p_context)
{
// Current speed event time is the previous event time added with the event interval.
m_speed_event_time += SPEED_EVENT_INTERVAL;
// Update current speed event time.
m_tx_buffer[SPEED_EVENT_TIME_LSB_INDEX] = LOW_BYTE(m_speed_event_time);
m_tx_buffer[SPEED_EVENT_TIME_MSB_INDEX] = HIGH_BYTE(m_speed_event_time);
// Update current speed event count.
++m_speed_revolution_count;
m_tx_buffer[CUMULATIVE_SPEED_LSB_INDEX] = LOW_BYTE(m_speed_revolution_count);
m_tx_buffer[CUMULATIVE_SPEED_MSB_INDEX] = HIGH_BYTE(m_speed_revolution_count);
}
/**@brief Function for simulating a bike cadence event.
*
* @param[in] p_context Timeout context.
*/
static void cadence_pulse_event_simulate(void * p_context)
{
// Current cadence event time is the previous event time added with the event interval.
m_cadence_event_time += CADENCE_EVENT_INTERVAL;
// Update current cadence event time.
m_tx_buffer[CADENCE_EVENT_TIME_LSB_INDEX] = LOW_BYTE(m_cadence_event_time);
m_tx_buffer[CADENCE_EVENT_TIME_MSB_INDEX] = HIGH_BYTE(m_cadence_event_time);
// Update current cadence event count.
++m_cadence_revolution_count;
m_tx_buffer[CUMULATIVE_CADENCE_LSB_INDEX] = LOW_BYTE(m_cadence_revolution_count);
m_tx_buffer[CUMULATIVE_CADENCE_MSB_INDEX] = HIGH_BYTE(m_cadence_revolution_count);
}
/* Returns true if the configuration value changed */
static int
set_configuration()
{
notify_user(USB_EVENT_CONFIG);
if (usb_configuration_value != LOW_BYTE(usb_setup_buffer.wValue)) {
usb_configuration_value = LOW_BYTE(usb_setup_buffer.wValue);
usb_arch_set_configuration(usb_configuration_value);
usb_send_ctrl_status();
return 1;
} else {
usb_send_ctrl_status();
return 0;
}
}
int create_do_move(int dist, int speed) {
if ((91-_create_script_indx)<8) {
printf("max script exceeded\n");
return(_create_script_indx); } // not enough script space left
_create_script[_create_script_indx++]=137; // move command
_create_script[_create_script_indx++]=HIGH_BYTE(speed);
_create_script[_create_script_indx++]=LOW_BYTE(speed);
_create_script[_create_script_indx++]=128; // no angle (go straight)
_create_script[_create_script_indx++]=0;
_create_script[_create_script_indx++]=156; // wait for distance done
_create_script[_create_script_indx++]=HIGH_BYTE(dist);
_create_script[_create_script_indx++]=LOW_BYTE(dist);
return(0);
}
int create_do_move_direct(int dist, int r_speed, int l_speed) { // individual motor control move
if ((91-_create_script_indx)<8) {
printf("max script exceeded\n");
return(_create_script_indx); } // not enough script space left
_create_script[_create_script_indx++]=145; // move command
_create_script[_create_script_indx++]=HIGH_BYTE(r_speed);
_create_script[_create_script_indx++]=LOW_BYTE(r_speed);
_create_script[_create_script_indx++]=HIGH_BYTE(l_speed);
_create_script[_create_script_indx++]=LOW_BYTE(l_speed);
_create_script[_create_script_indx++]=156; // wait for distance done
_create_script[_create_script_indx++]=HIGH_BYTE(dist);
_create_script[_create_script_indx++]=LOW_BYTE(dist);
return(0);
}
uint8_t receiveSingleFrame() {
uint8_t ret = LW232_OK;
uint8_t idx = 0;
if (CAN_OK == readMsgBufID(&lw232_CanId, &lw232_PacketLen, lw232_Buffer)) {
if (lw232_CanId > 0x1FFFFFFF) {
ret = LW232_ERR; // address if totally wrong
}
else if (checkPassFilter(lw232_CanId)) {// do we want to skip some addresses?
if (isExtendedFrame()) {
printChar(LW232_TR29);
printFullByte(HIGH_BYTE(HIGH_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(HIGH_WORD(lw232_CanId)));
printFullByte(HIGH_BYTE(LOW_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(LOW_WORD(lw232_CanId)));
}
else {
printChar(LW232_TR11);
printNibble(HIGH_BYTE(LOW_WORD(lw232_CanId)));
printFullByte(LOW_BYTE(LOW_WORD(lw232_CanId)));
}
//write data len
printNibble(lw232_PacketLen);
//write data
for (idx = 0; idx < lw232_PacketLen; idx++) {
printFullByte(lw232_Buffer[idx]);
}
//write timestamp if needed
if (lw232_TimeStamp != LW232_TIMESTAMP_OFF) {
uint32_t time = 0; //millis();
if (lw232_TimeStamp == LW232_TIMESTAMP_ON_NORMAL) {
// standard LAWICEL protocol. two bytes.
time %= 60000;
} else {
// non standard protocol - 4 bytes timestamp
printFullByte(HIGH_BYTE(HIGH_WORD(time)));
printFullByte(LOW_BYTE(HIGH_WORD(time)));
}
printFullByte(HIGH_BYTE(LOW_WORD(time)));
printFullByte(LOW_BYTE(LOW_WORD(time)));
}
}
}
else {
ret = LW232_ERR;
}
return ret;
}
U16 C_DM9000::DeviceWriteEeprom(
U32 uWordAddress,
U16 uValue)
{
// assign the register offset
DeviceWritePort(DM9_EPADDR,uWordAddress);
// put data
DeviceWritePort(DM9_EPLOW, LOW_BYTE(uValue));
DeviceWritePort(DM9_EPHIGH,HIGH_BYTE(uValue));
// issue EEPROM write enable<4> and write command<1>
DeviceWritePort(DM9_EPCNTL,MAKE_MASK2(4,1));
// wait until status bit<0> cleared
DevicePolling(DM9_EPCNTL,MAKE_MASK(0),0x00);
// stop command
DeviceWritePort(DM9_EPCNTL,0);
// extra delay
NdisStallExecution(1000);
return uValue;
}
bool Processor::is_terminator(zchar key) {
if (key == ZC_TIME_OUT)
return true;
if (key == ZC_RETURN)
return true;
if (key >= ZC_HKEY_MIN && key <= ZC_HKEY_MAX)
return true;
if (h_terminating_keys != 0) {
if (key >= ZC_ARROW_MIN && key <= ZC_MENU_CLICK) {
zword addr = h_terminating_keys;
zbyte c;
do {
LOW_BYTE(addr, c);
if (c == 255 || key == translate_from_zscii(c))
return true;
addr++;
} while (c != 0);
}
}
return false;
}
INT8U Can232::receiveSingleFrame() {
INT8U ret = LW232_OK;
INT8U idx = 0;
if (CAN_OK == readMsgBufID(&lw232CanId, &lw232PacketLen, lw232Buffer)) {
if (lw232CanId > 0x1FFFFFFF) {
ret = LW232_ERR; // address if totally wrong
}
else if (checkPassFilter(lw232CanId)) {// do we want to skip some addresses?
if (isExtendedFrame()) {
Serial.print(LW232_TR29);
HexHelper::printFullByte(HIGH_BYTE(HIGH_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(HIGH_WORD(lw232CanId)));
HexHelper::printFullByte(HIGH_BYTE(LOW_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(lw232CanId)));
}
else {
Serial.print(LW232_TR11);
HexHelper::printNibble(HIGH_BYTE(LOW_WORD(lw232CanId)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(lw232CanId)));
}
//write data len
HexHelper::printNibble(lw232PacketLen);
//write data
for (idx = 0; idx < lw232PacketLen; idx++) {
HexHelper::printFullByte(lw232Buffer[idx]);
}
//write timestamp if needed
if (lw232TimeStamp != LW232_TIMESTAMP_OFF) {
INT32U time = millis();
if (lw232TimeStamp == LW232_TIMESTAMP_ON_NORMAL) {
// standard LAWICEL protocol. two bytes.
time %= 60000;
} else {
// non standard protocol - 4 bytes timestamp
HexHelper::printFullByte(HIGH_BYTE(HIGH_WORD(time)));
HexHelper::printFullByte(LOW_BYTE(HIGH_WORD(time)));
}
HexHelper::printFullByte(HIGH_BYTE(LOW_WORD(time)));
HexHelper::printFullByte(LOW_BYTE(LOW_WORD(time)));
}
}
}
else {
ret = LW232_ERR;
}
return ret;
}
int create_do_arc(int rad, int deg, int speed) {
// warning: CCW movement increases degree, CW decreases
// opposite signs on rad and vel cause CW rotation
// if direction is CW, this won't stop if deg > 0
if ((91-_create_script_indx)<8) {
printf("max script exceeded\n");
return(_create_script_indx); } // not enough script space left
_create_script[_create_script_indx++]=137; // move command
_create_script[_create_script_indx++]=HIGH_BYTE(speed);
_create_script[_create_script_indx++]=LOW_BYTE(speed);
_create_script[_create_script_indx++]=HIGH_BYTE(rad);
_create_script[_create_script_indx++]=LOW_BYTE(rad);
_create_script[_create_script_indx++]=157; // wait for angle done
_create_script[_create_script_indx++]=HIGH_BYTE(deg);
_create_script[_create_script_indx++]=LOW_BYTE(deg);
return(0);
}
/* This function could use a rewrite */
static void convdata(unsigned char *srcbuffer, unsigned char *dstbuffer,
int width, int mode)
/* This is a tricky (read: crap) function (read: hack) which is why I probably
spent more time commenting it than programming it. The thing to remember
here is that the scanner uses interpolated scanlines, so it's
RRRRRRRGGGGGGBBBBBB not RGBRGBRGBRGBRGB. So, the calling function just
increments the destination pointer slightly to handle green, then a bit
more for blue. If you don't understand, tough. */
{
int count;
int i, j, k;
for (count = 0; count < width; count++)
{
/* The scanner stores data in a bizzare butchered 10-bit
format. I'll try to explain it in 100 words or less:
Scanlines are made up of groups of 4 pixels. Each group of
4 is stored inside 5 bytes. The first 4 bytes of the group
contain the lowest 8 bits of one pixel each (in the right
order). The 5th byte contains the most significant 2 bits
of each pixel in the same order. */
i = srcbuffer[count + (count >> 2)]; /* Low byte for pixel */
j = srcbuffer[(((count / 4) + 1) * 5) - 1]; /* "5th" byte */
j = j >> ((count % 4) * 2); /* Get upper 2 bits of intensity */
j = j & 0x03; /* Can't hurt */
/* And the final 10-bit pixel value is: */
k = (j << 8) | i;
/* now we return this as a 16 bit value */
k = k << 6;
if (mode == 1) /* Scanner -> Grey */
{
dstbuffer[count * 2] = HIGH_BYTE(k);
dstbuffer[(count * 2) + 1] = LOW_BYTE(k);
}
else if (mode == 2) /* Scanner -> RGB */
{
dstbuffer[count * 3 * 2] = HIGH_BYTE(k);
dstbuffer[(count * 3 * 2) + 1] = LOW_BYTE(k);
}
}
}