Close

CORDIC

A project log for Robotic Table Soccer with Inverted Mecanum Drives

Two threadless leadscrews with opposite "thread" directions. Brushless motor control of the drives. Linear and rotational magnetic encoders.

Paul GouldPaul Gould 03/26/2020 at 16:480 Comments

Read ADC

    aa1 = ADC_SAR_GetResult16(0u);
    bb1 = ADC_SAR_GetResult16(1u);
    cc1 = ADC_SAR_GetResult16(2u);
    dd1 = ADC_SAR_GetResult16(3u);

Do CORDIC Calculation

int cordic(int32 aa, int32 bb, int32 cc, int32 dd)
{
    int hor, ver;
    hor = aa - cc;
    ver = bb - dd;
    double abs_val_sq = (hor * hor) + (ver * ver);
    double inv_amp = 1.0 / (sqrt(abs_val_sq));
    double hor_f = ((double)hor) * inv_amp;
    double ver_f = ((double)ver) * inv_amp;
    double hor_s = asin(hor_f);
    double ver_s = acos(ver_f);
    double ang_r = 0.0;
    double ang = 0.0;
    int ang_cordic=0;
    
    if (hor_s>0)
    {
        if (ver_s > pid2)
            ang_r = ver_s + (pi-hor_s);
        else
            ang_r =  ver_s + hor_s;
    }
    else
    {
        if (ver_s > pid2)
            ang_r = pi + (pi - ver_s) + (pi - hor_s);
        else
            ang_r =  pi + pi + (pi - ver_s) + (pi + hor_s);
    }
    
    ang = ang_r * rad_deg;
        
    if (abs_val_sq > 100000) // good (Todo crc or party)
    {
        ang_cordic = (int)(ang_r * rad_12); // | 0x8000;  
    }
    else // bad
    {
        ang_cordic =  ((int)(ang_r * rad_12));
    }
    return (ang_cordic);
}

Discussions