Inverted Mecanum Drives Robotic Table Soccer

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);
}