int16_t motor_command(int16_t command){
PWM_SetDutyCycle(PWM_PORTY04, abs_val(command));
if(command>0){
DIRECTION = FORWARD;
}else{
DIRECTION = REVERSE;
}
return 0;
}
ll pollard_rho(ll n) {
int i = 0, k = 2;
ll x = 3, y = 3; // random seed = 3, other values possible
while (1) {
i++;
x = (mulmod(x, x, n) + n - 1) % n; // generating function
ll d = gcd(abs_val(y - x), n); // the key insight
if (d != 1 && d != n) return d; // found one non-trivial factor
if (i == k) y = x, k *= 2;
}
}
void Fgetrf_pivotize_(int m, int n, float* a, int lda, int* ipiv) {
int i, j, col_offs, max_row, buffer;
for (j = 0; j < n; ++j) {
ipiv[j] = j;
}
for (j = 0; j < n; ++j) {
col_offs = j * lda;
max_row = j;
/* Find max within a column */
for (i = j + 1; i < m; ++i)
if (abs_val(a[col_offs + ipiv[i]]) > abs_val(a[col_offs + ipiv[max_row]]))
max_row = i;
if (max_row != j) {
Fgetrf_swap_rows_(n, a, lda, ipiv[max_row], ipiv[j]);
buffer = ipiv[max_row];
ipiv[max_row] = ipiv[j];
ipiv[j] = buffer;
}
}
}
int calculate_acceleration(struct particle* p, int n, struct particle **arr, int dimensions) {
int i;
struct particle* neighbor;
p->x_acc = 0;
p->y_acc = 0;
for(i = 0; i < n; i++) {
neighbor = arr[i];
if(neighbor == p) continue; // A particle does not affect itself
if(dimensions == 1) {
p->x_acc += (neighbor->x - p->x)/power(abs_val(neighbor->x - p->x),3);
}
if(dimensions == 2) {
// Calculate this buddy
;
}
}
}
static void fmtfp(char *buffer, size_t *currlen, size_t maxlen,
long double fvalue, int min, int max, int flags, prfun outch)
{
char iconvert[20];
char fconvert[20];
int signvalue = 0;
int iplace = 0;
int fplace = 0;
int padlen = 0;
int zpadlen = 0;
long intpart;
long fracpart;
long double ufvalue;
/* AIX manpage says the default is 0, but Solaris says the default
is 6, and sprintf on AIX defaults to 6. */
if(max<0) max=6;
ufvalue=abs_val(fvalue);
if(fvalue<0)
signvalue='-';
else if (flags & DP_F_PLUS) // Do a sign(+/i).
signvalue='+';
else if (flags & DP_F_SPACE)
signvalue=' ';
intpart=(long)ufvalue;
/* Sorry, we only support 9 digits past the decimal because of our
conversion method */
if(max>9) max=9;
/* We "cheat" by converting the fractional part to integer by
multiplying by a factor of 10. */
fracpart=round((pow10(max))*(ufvalue-intpart));
if(fracpart>=pow10(max))
{
intpart++;
fracpart-=(long)pow10 (max);
}
// Convert integer part.
do
{
iconvert[iplace++]=digitval[intpart % 10];
intpart=(intpart / 10);
} while(intpart && (iplace<20));
if(iplace==20) iplace--;
iconvert[iplace]=0;
// Convert fractional part.
do
{
fconvert[fplace++]=digitval[fracpart % 10];
fracpart=(fracpart / 10);
} while(fracpart && (fplace<20));
if(fplace==20) fplace--;
fconvert[fplace]=0;
// -1 for decimal point, another -1 if we are printing a sign.
padlen=min-iplace-max-1-((signvalue)?1:0);
zpadlen=max-fplace;
if(zpadlen<0)
zpadlen=0;
if(padlen<0)
padlen=0;
if(flags & DP_F_MINUS)
padlen=-padlen; // Left justifty.
if((flags & DP_F_ZERO) && (padlen>0))
{
if(signvalue)
{
outch(buffer, currlen, maxlen, signvalue);
--padlen;
signvalue=0;
}
while (padlen>0)
{
outch(buffer, currlen, maxlen, '0');
--padlen;
}
}
while(padlen>0)
{
outch(buffer, currlen, maxlen, ' ');
--padlen;
}
if(signvalue)
outch(buffer, currlen, maxlen, signvalue);
while(iplace>0)
outch(buffer, currlen, maxlen, iconvert[--iplace]);
/* Decimal point. This should probably use locale to find the correct
char to print out. */
outch(buffer, currlen, maxlen, '.');
while(fplace>0)
outch(buffer, currlen, maxlen, fconvert[--fplace]);
while(zpadlen>0)
{
outch(buffer, currlen, maxlen, '0');
--zpadlen;
}
while(padlen<0)
{
outch(buffer, currlen, maxlen, ' ');
++padlen;
}
}
