#include<stdio.h>
#include <math.h>
int main() {
int i=5000;
int v=30000;
double teta=15;
int i_alpha= (int)((float)i*cos(teta*3.1415/180));
int i_beta= (int)((float)i*sin(teta*3.1415/180));
int v_alpha= (int)((float)v*cos(teta*3.1415/180));
int v_beta= (int)((float)v*sin(teta*3.1415/180));
const int L = (3.0 / 2.0) * 120;
const int lambda = 666;
int R = (3.0 / 2.0) * 350;
volatile int x1=0;
volatile int x2=0;
const int L_ia = (L * i_alpha)/1000; // diveded by 1000 because of value in milliamps
const int L_ib = (L * i_beta)/1000;
const int R_ia = (R * i_alpha)/1000;
const int R_ib = (R * i_beta)/1000;
const int lambda_2 = lambda*lambda;
const int gamma_half = 7;
for (int i = 0;i < 20;i++){
volatile int err = lambda_2 - ((x1 - L_ia)*(x1 - L_ia) + (x2 - L_ib)*(x2 - L_ib));
int gamma_tmp = gamma_half;
/*if (utils_truncate_number_abs(&err, lambda_2 * 0.2)) {
gamma_tmp *= 10.0;
}*/
int temp5=(((x1 - L_ia) * err)>>gamma_tmp);
int temp6=(((x2 - L_ib) * err)>>gamma_tmp);
volatile int x1_dot = -R_ia + v_alpha + temp5 ;
volatile int x2_dot = -R_ib + v_beta + temp6 ;
x1 += x1_dot >>14; // *1/16000
x2 += x2_dot >>14;
int e_alpha= x1 - L_ia;
int e_beta= x2 - L_ib;
//(int)atan2((double)e_beta,(double)e_alpha)*180/3.1416
printf(" %i, %i, %i, %i, %i,%i\r\n", err, x1,x2,x1_dot,x2_dot,(int)(atan2((double)e_beta,(double)e_alpha)*180/3.1416));
}
}