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