Skip to content

Commit

Permalink
Add preliminary LEGO support
Browse files Browse the repository at this point in the history
  • Loading branch information
@richlegrand
richlegrand committed Jan 25, 2019
1 parent 2adc6ca commit bb27239
Show file tree
Hide file tree
Showing 4 changed files with 38 additions and 50 deletions.
2 changes: 1 addition & 1 deletion src/device/main_m4/inc/exec.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@

#define FW_MAJOR_VER 3
#define FW_MINOR_VER 0
#define FW_BUILD_VER 11
#define FW_BUILD_VER 12
#ifdef LEGO
#define FW_TYPE "LEGO"
#else
Expand Down
39 changes: 9 additions & 30 deletions src/device/main_m4/src/line.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3708,7 +3708,6 @@ int line_legoLineData(uint8_t *buf, uint32_t buflen)
uint16_t maxy;
Line2 *primary;
uint32_t x;
static int8_t intersectionPresent = -1;
static uint8_t lastData[4];

// override these because LEGO mode doesn't support
Expand Down Expand Up @@ -3754,37 +3753,17 @@ int line_legoLineData(uint8_t *buf, uint32_t buflen)
}
buf[1] = codeVal;


primary = findLine(g_primaryLineIndex);
if (primary)
{
if (primary->m_i1)
buf[2] = primary->m_i1->m_object.m_n;
else if (primary->m_i0)
buf[2] = primary->m_i0->m_object.m_n;
}
if (g_newIntersection)
{
primary = findTrackedLine(g_primaryLineIndex);
if (primary)
{
if (g_goalPoint.equals(primary->m_p0))
intersectionPresent = 0;
else
intersectionPresent = 1;
}
g_newIntersection = false;
}
if (intersectionPresent>=0)
{
primary = findTrackedLine(g_primaryLineIndex);
if (primary)
{
if (intersectionPresent==0)
{
if (primary->m_i1)
buf[2] = primary->m_i1->m_object.m_n;
}
else
{
if (primary->m_i0)
buf[2] = primary->m_i0->m_object.m_n;
}
}
if (buf[2]==(uint8_t)-1)
intersectionPresent = -1;
}

memcpy(lastData, buf, 4);
#endif
Expand Down
45 changes: 27 additions & 18 deletions src/device/main_m4/src/serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
static uint8_t lastLamp = 0;
uint8_t c, reverse, lamp;
uint16_t d, x, y;
int16_t turn;
int8_t turn;
int16_t turn16;
uint16_t numBlobs;
uint32_t temp, width, height, r, g, b;
Iserial *serial = ser_getSerial();
Expand Down Expand Up @@ -145,12 +146,14 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
width = max->m_right - max->m_left;
height = max->m_bottom - max->m_top;
*(uint16_t *)buf = max->m_model; // signature
temp = ((max->m_left + width/2)*819)>>10;
temp = ((max->m_left + width/2)*829)>>10;
buf[2] = temp; // x
buf[3] = max->m_top + height/2; // y
temp = (width*819)>>10;
temp = ((max->m_top + height/2)*1262)>>10;
buf[3] = temp; // y
temp = (width*829)>>10;
buf[4] = temp; // width
buf[5] = height; // height
temp = (height*1262)>>10;
buf[5] = temp; // height
if (max->m_model>CL_NUM_SIGNATURES)
{
temp = ((int32_t)max->m_angle*91)>>7;
Expand Down Expand Up @@ -185,12 +188,14 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
width = max->m_right - max->m_left;
height = max->m_bottom - max->m_top;
buf[0] = numBlobs; // number of blocks that match signature
temp = ((max->m_left + width/2)*819)>>10;
temp = ((max->m_left + width/2)*829)>>10;
buf[1] = temp; // x
buf[2] = max->m_top + height/2; // y
temp = (width*819)>>10;
temp = ((max->m_top + height/2)*1262)>>10;
buf[2] = temp; // y
temp = (width*829)>>10;
buf[3] = temp; // width
buf[4] = height; // height
temp = (height*1262)>>10;
buf[4] = temp; // height
}
#endif
return 5;
Expand Down Expand Up @@ -218,12 +223,14 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
width = max->m_right - max->m_left;
height = max->m_bottom - max->m_top;
buf[0] = numBlobs; // number of cc blocks that match
temp = ((max->m_left + width/2)*819)>>10;
temp = ((max->m_left + width/2)*829)>>10;
buf[1] = temp; // x
buf[2] = max->m_top + height/2; // y
temp = (width*819)>>10;
temp = ((max->m_top + height/2)*1262)>>10;
buf[2] = temp; // y
temp = (width*829)>>10;
buf[3] = temp; // width
buf[4] = height; // height
temp = (height*1262)>>10;
buf[4] = temp; // height
temp = ((int32_t)max->m_angle*91)>>7;
buf[5] = temp; // angle
}
Expand All @@ -232,12 +239,15 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
}
else if (c==0x5a)
{
if (serial->receive((uint8_t *)&turn, 2)==2 &&
if (serial->receive((uint8_t *)&turn, 1)==1 &&
serial->receive((uint8_t *)&reverse, 1)==1 &&
serial->receive((uint8_t *)&lamp, 1)==1 &&
error==false)
{
line_setNextTurnAngle(turn);
turn16 = turn;
turn16 *= 180; // scale accordingly
turn16 /= 127;
line_setNextTurnAngle(turn16);
if (lastReverse!=0xffff && lastReverse!=reverse)
line_reversePrimary();
if (lastLamp!=lamp)
Expand Down Expand Up @@ -302,7 +312,7 @@ uint16_t lego_getData(uint8_t *buf, uint32_t buflen)
else
{
width = max->m_right - max->m_left;
temp = ((max->m_left + width/2)*819)>>10;
temp = ((max->m_left + width/2)*829)>>10;
buf[0] = temp;
}
}
Expand Down Expand Up @@ -629,8 +639,7 @@ void ser_loadParams()
{
#ifndef LEGO
prm_add("Data out port", 0, PRM_PRIORITY_1,
//"Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C", UINT8(0), END);
"Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y", UINT8(0), END);
"Selects the port that's used to output data (default Arduino ICSP SPI) @c Interface @s 0=Arduino_ICSP_SPI @s 1=SPI_with_SS @s 2=I2C @s 3=UART @s 4=analog/digital_x @s 5=analog/digital_y @s 6=LEGO_I2C", UINT8(0), END);
prm_add("I2C address", PRM_FLAG_HEX_FORMAT, PRM_PRIORITY_1-1,
"@c Interface Sets the I2C address if you are using I2C data out port. (default 0x54)", UINT8(I2C_DEFAULT_SLAVE_ADDR), END);
prm_add("UART baudrate", 0, PRM_PRIORITY_1-2,
Expand Down
2 changes: 1 addition & 1 deletion src/device/main_m4/workspace.uvmpw
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@

<project>
<PathAndName>..\main_m0\main_m0.uvproj</PathAndName>
<NodeIsExpanded>1</NodeIsExpanded>
</project>

<project>
Expand All @@ -24,6 +23,7 @@

<project>
<PathAndName>..\libpixy_m4\libpixy_m4.uvproj</PathAndName>
<NodeIsExpanded>1</NodeIsExpanded>
</project>

</ProjectWorkspace>

0 comments on commit bb27239

Please sign in to comment.