# (C) 2007 Julian von Mendel (prog@derjulian.net)
# 
# $LastChangedBy: jvm $
# $LastChangedDate: 2007-12-24 03:03:09 +0100 (Mon, 24 Dec 2007) $
# $Revision: 312 $

vektoraddition zur xy-bestimmung
================================
(imkreisdrehanteil ist irrelevant, da cos(0)+cos(120)+cos(240)=0)
geg:
    d1, d2, d3 (gemessen)
ges:
    x, y
lsg:
    x=cos(phi)*drehzahl
    y=sin(phi)*drehzahl
    x=x1+x2+x3
    y=y1+y2+y3
beispiele:
    5,5,10 (x-verschiebung)
    x1=cos(120)*5=-2.5
    y1=sin(120)*5=4.33
    x2=cos(240)*5=-2.5
    y2=sin(240)*5=-4.33
    x3=cos(0)*10=10
    y3=sin(0)*10=0
    x=x1+x2+x3=5
    y=y1+y2+y3=0

    5,5,0 (y-verschiebung)
    x1=cos(120)*5=-2.5
    y1=sin(120)*5=4.33
    x2=cos(240)*5=-2.5
    y2=sin(240)*5=-4.33
    x3=cos(0)*0=0
    y3=sin(0)*0=0
    x=x1+x2+x3=-5
    y=y1+y2+y3=0

    5,5,5 (drehung)
    x1=cos(120)*5=-2.5
    y1=sin(120)*5=4.33
    x2=cos(240)*5=-2.5
    y2=sin(240)*5=-4.33
    x3=cos(0)*5=5
    y3=sin(0)*5=0
    x=x1+x2+x3=0
    y=y1+y2+y3=0

bestimmung des startwinkels (bezogen auf x-achse in mathematisch positivem umlaufsinn)
======================================================================================
geg: deltax, deltay
ges: startwinkel
lsg:
    if (deltax == 0) {
        if (sgn(deltay) == 1)
            return 90.0;
        return 270.0;
    } else if (deltay == 0) {
        if (sgn(deltax) == 1)
            return 0.0;
        return 180.0;
    }
    if (sgn(deltax) == 1 && sgn(deltay) == 1)
        return (ratan(deltay / deltax));
    else if (sgn(deltax) == -1 && sgn(deltay) == 1)
        return (ratan(deltay / deltax) + 180.0);
    else if (sgn(deltax) == 1 && sgn(deltay) == -1)
        return (ratan(deltay / deltax) + 360.0);
    /* if (sgn(deltay) == -1 && sgn(deltax) == -1) */
        return (ratan(deltay / deltax) + 180.0);
    
bestimmung der motorgeschwindigkeit auf basis der notwendigen x-, y-verschiebung
================================================================================
geg: deltax, deltay
ges: d1v, d2v, d3v
lsg:
    d1v=cos(0 - startwinkel)
    d2v=cos(120 - startwinkel)
    d3v=cos(240 - startwinkel)

winkelgeschwindigkeits-bestimmung
=================================
geg:
    d1,d2,d3, (gemessen)
    d1v,d2v,d3v (aus deltaxy neu berechnet)
ges:
    dd (imkreisdrehanteil)
lsg:
    dd=d1-d1v*c
    dd=d2-d2v*c
    dd=d3-d3v*c

    dd+d1v*c=d1
    c=(d1-dd)/d1v

    d1-d1v*c=d2-d2v*c
    c=(d2-d1)/(d2v-d1v)

    c=(d3-dd)/d3v
    c*d3v=d3-dd
    dd=d3-c*d3v
beispiele:
    dx=5,5,10;dxv=dx => c=1;dd=0
    dx=5,10,5;dxv=dx => c=1;dd=0
    dx=10,5,5;dxv=dx => c=1;dd=0
    dx=10,10,15;dxv=5,5,10 => c=1;dd=5

c-code
======
float arctan(float deltax, float deltay) {
	if (deltax == 0) {
		if (sgn(deltay) == 1)
			return 90.0;
		return 270.0;
	} else if (deltay == 0) {
		if (sgn(deltax) == 1)
			return 0.0;
		return 180.0;
	}
	if (sgn(deltax) == 1 && sgn(deltay) == 1)
		return (ratan(deltay / deltax));
	else if (sgn(deltax) == -1 && sgn(deltay) == 1)
		return (ratan(deltay / deltax) + 180.0);
	else if (sgn(deltax) == 1 && sgn(deltay) == -1)
		return (ratan(deltay / deltax) + 360.0);
	/* if (sgn(deltay) == -1 && sgn(deltax) == -1) */
		return (ratan(deltay / deltax) + 180.0);
}

	void drive_position(int16_t edgecount0, int16_t edgecount1,
			int16_t edgecount2) {
		float deltax, deltay;
		float angle;
		float d0b, d1b, d2b, dd;

		/* calculate x, y speed with vector addition */
		/* 		x[wheel-nr]=cos(phi[wheel-nr])*edgecount[wheel-nr]
    	 *		y[wheel-nr]=sin(phi[wheel-nr])*edgecount[wheel-nr]
    	 *		deltax=x[0]+x[1]+x[2]
    	 *		deltay=y[0]+y[1]+y[2]
    	 */
		deltax = rcos(0) * edgecount0 +
				rcos(120) * edgecount1 +
				rcos(240) * edgecount2;
		deltay = rsin(0) * edgecount0 +
				rsin(120) * edgecount1 +
				rsin(240) * edgecount2;

		/* calculate wheel rotation speed ratios needed to achieve
		 * given x, y movement (same as in drive_cycle, only more
		 * general) */
		angle = arctan(deltax, deltay);
		d0b = d1b = d2b = 0;
		if (rround(deltax, 2) != 0 || rround(deltay, 2) != 0) {
		    d0b = rcos(0 - angle) * (-1);
		    d1b = rcos(120 - angle) * (-1);
		    d2b = rcos(240 - angle) * (-1);
		}

		/* calculate robot rotation speed by determining the constant which
		 * was multiplied with the x, y speed */
		/*		dd=d[wheel-nr]-d[wheel-nr]b*c
		 * 		dd+d[wheel-nr]b*c=d[wheel-nr]
    	 *		c=(d[wheel-nr]-dd)/d[wheel-nr]b
		 *
    	 * 		d0-d0b*c=d1-d1b*c
    	 *		c=(d1-d0)/(d1b-d0b)=(d2-dd)/d2b
    	 * 		c*d2b=d2-dd
    	 * 		dd=d2-c*d2b
		 */
		if ((edgecount1 - edgecount0) == 0 || (d1b - d0b) == 0) {
			dd = edgecount2 - d2b;
		} else {
			dd = edgecount2 -
					(edgecount1 - edgecount0) / (d1b - d0b) * d2b;
		}

		/* calculate reclined distance (see graph) */
    	d0b = max(fabs(edgecount0 - dd), fabs(edgecount1 - dd),
    			fabs(edgecount2 - dd)) / 100 *
            	(rcos(6 * angle) / 100 * (1 - sqrt(3) / 2) / 2 + 1 -
            	(1 - sqrt(3) / 2) / 2);

        /* calculate x, y movement out of reclined distance and start angle */
    	deltax = rcos(angle) * d0b / 100;
    	deltay = rsin(angle) * d0b / 100;

		/* make sure angle is between 0° and 360° */
		angle = mod(round(dd *
				DEGREE_PER_EDGE, 1), 360.0);
		if (angle < 0) {
			angle += 360;
		}

		/* modify position based on calculated speeds */
		drive_setpositionx(deltax * MM_PER_EDGE);
		drive_setpositiony(deltay * MM_PER_EDGE);
		drive_setpositionangle(angle);
	}