void GoCircle(void)
{
//struct Point pos;
double radian;
double error_dis,error_angle;
double Vout_D,Vout_A;
radian = GetLineAngle(GPS.position,Center);
//得到理论坐标与理论角度
//pos.x = Center.x + R*cos(radian);
//pos.y = Center.y + R*sin(radian);
//角度误差和距离误差
error_dis = GetLength(GPS.position,Center) - R;
error_angle = radian - GPS.radian;
if(error_angle>pi)
error_angle-=2*pi;
if(error_angle<-pi)
error_angle+=2*pi;
//PID调整
Vout_D = DistancePID(error_dis,Pid_List[1]);
Vout_A = 1000*AnglePID(error_angle,Pid_List[0]);
Speed_Y = Speed;
Speed_X = Vout_D;
Speed_Rotation = Vout_A;
SetSpeed(Speed_X,Speed_Y,Speed_Rotation);
//LCD显示
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("A:");
LCD_WriteDouble(GPS.radian);
LCD_SetXY(0,1);
LCD_WriteString("X:");
LCD_WriteDouble(GPS.position.x);
LCD_SetXY(9,1);
LCD_WriteString(" Y:");
LCD_WriteDouble(GPS.position.y);
LCD_SetXY(0,2);
LCD_WriteString("error_angle:");
LCD_WriteDouble(error_angle);
LCD_SetXY(0,3);
LCD_WriteString("error_dis:");
LCD_WriteDouble(error_dis);
LCD_SetXY(0,4);
LCD_WriteString("Vout_A:");
LCD_WriteDouble(Vout_A);
LCD_SetXY(0,5);
LCD_WriteString("Vout_D:");
LCD_WriteDouble(Vout_D);
LCD_SetXY(0,6);
LCD_WriteDouble(radian);
}
//virtual
void PSVGPathElement::Render4(CHTMRenderContext* pC, LDraw::Bitmap* pBitmap, Gui::RenderContext* renderContext, double scaleX, double scaleY, bool bOffscreen, bool bDrawBehaviors)
{
ASSERT(0);
#if 0
SVGPathElement* psvgElement = static_cast<SVGPathElement*>(m_pNode);
pGraphics->SetSmoothingMode(LDraw::SmoothingModeHighQuality/*pGraphics->GetSmoothingMode()*/);
SVGPathSegList* seglist = psvgElement->m_d->m_animated->m_animVal->m_value->m_normalizedseglist;
#if 0
if (seglist)
#endif
if (m_pGdipGraphicsPathF)
{
//LDraw::GraphicsPathF GraphicsPathF(m_computedFillRule == FillRule_nonzero? LDraw::FillModeWinding: LDraw::FillModeAlternate);
//DrawPathSegList(seglist, &GraphicsPathF, NULL);
double fillOpacity = m_computedFillOpacity;
double strokeOpacity = m_computedStrokeOpacity;
if (!bOffscreen)
{
fillOpacity *= m_computedOpacity;
strokeOpacity *= m_computedOpacity;
}
CManagedBrush pFillBrush = GetFillBrush(fillOpacity, scaleX, scaleY);
CManagedBrush pStrokeBrush = GetStrokeBrush(strokeOpacity, scaleX, scaleY);
if (pFillBrush != NULL)
{
pGraphics->FillPath(pFillBrush, m_pGdipGraphicsPathF/*&GraphicsPathF*/);
}
if (pStrokeBrush != NULL)
{
LDraw::Pen* pPen = GetGdipPen(pStrokeBrush);
if (pPen)
{
pGraphics->DrawPath(pPen, m_pGdipGraphicsPathF/*&GraphicsPathF*/);
delete pPen;
}
}
// Draw markers
{
if (m_pMarkerStartElement || m_pMarkerEndElement || m_pMarkerMidElement)
{
double tangentIn;
double tangentOut;
double x, y;
double x0, y0;
CArray<MarkerVertex,MarkerVertex&> vertices;
int numberOfItems = seglist->m_items.GetSize();
int istartOfSubPath = 0;
// int isubpath = 0;
for (int i = 0; i < numberOfItems; i++)
{
CLSVGPathSegImpl* seg = seglist->m_items[i];
LSVGPathSegType segType = seg->get_pathSegType();
if (segType == PATHSEG_MOVETO_ABS)
{
CComQIPtr<ILSVGPathSegMovetoAbs> typeseg = seg;
typeseg->get_x(&x);
typeseg->get_y(&y);
istartOfSubPath = i;
// isubpath = 0;
MarkerVertex vertex;
vertex.x = x;
vertex.y = y;
vertex.tangent = 0; // tangent isn't known at this point
vertices.Add(vertex);
}
else if (segType == PATHSEG_LINETO_ABS)
{
CComQIPtr<ILSVGPathSegLinetoAbs> typeseg = seg;
typeseg->get_x(&x);
typeseg->get_y(&y);
tangentIn = GetLineAngle(x0, y0, x, y)*180/M_PI;
// isubpath++;
// Adjust the tangent of previous vertex
if (i > 0)
{
double tangent = (vertices[i-1].tangent + tangentIn)/2;
vertices[i-1].tangent = tangent;
}
// Add new vertex
MarkerVertex vertex;
vertex.x = x;
vertex.y = y;
vertex.tangent = tangentIn; // tangentOut isn't yet accounted for
vertices.Add(vertex);
}
else if (segType == PATHSEG_CURVETO_CUBIC_ABS)
{
CComQIPtr<ILSVGPathSegCurvetoCubicAbs> typeseg = seg;
double x1, y1;
double x2, y2;
double x3, y3;
typeseg->get_x(&x3);
typeseg->get_y(&y3);
typeseg->get_x1(&x1);
typeseg->get_y1(&y1);
typeseg->get_x2(&x2);
typeseg->get_y2(&y2);
double t = 1;
double cx = 3 * (x1 - x0);
double cy = 3 * (y1 - y0);
double bx = 3 * (x2 - x1) - cx;
double by = 3 * (y2 - y1) - cy;
double ax = x3 - x0 - cx - bx;
double ay = y3 - y0 - cy - by;
// Calculate angle
{
double dx = ax * 3*t*t + bx * 2*t + cx;
double dy = ay * 3*t*t + by * 2*t + cy;
{
double distance = sqrt(dx*dx + dy*dy);
double distance2 = distance;
if (distance2 == 0.0) distance2 = 0.00001;
if (dy < 0) distance2 =-distance2;
double angle = acos(dx/distance2);
if (dy < 0) angle += M_PI;
tangentIn = angle * 180/M_PI;
}
}
x = x3;
y = y3;
// Adjust the tangent of previous vertex
if (i > 0)
{
vertices[i-1].tangent = (vertices[i-1].tangent + tangentIn)/2;
}
// Add new vertex
MarkerVertex vertex;
vertex.x = x;
vertex.y = y;
vertex.tangent = tangentIn; // tangentOut isn't yet accounted for
vertices.Add(vertex);
}
else if (segType == PATHSEG_CLOSEPATH)
{
// Adjust the tangent of the first vertex of the subpath
vertices[istartOfSubPath].tangent = (vertices[istartOfSubPath].tangent+tangentOut)/2;
istartOfSubPath = i+1;
}
else
ASSERT(0);
tangentOut = tangentIn;
x0 = x;
y0 = y;
}
int count = vertices.GetSize();
if (count > 0)
{
if (m_pMarkerStartElement)
{
m_pMarkerStartElement->DrawMarker(pC, pGraphics, this, &vertices[0]);
}
if (m_pMarkerMidElement)
{
for (int i = 1; i < count-1; i++)
{
m_pMarkerMidElement->DrawMarker(pC, pGraphics, this, &vertices[i]);
}
}
if (m_pMarkerEndElement)
{
m_pMarkerEndElement->DrawMarker(pC, pGraphics, this, &vertices[count-1]);
}
}
}
}
}
#endif
}
