PIDs:
rtPID:
float pos=0;
float prev_pos=0;
float pos_change=0;
float r_kP=1;
float r_kD=1;
float r_error=0;
float r_derivative=0;
float r_preverror=0;
float r_pow=0;
float r_loops=0;
void rtPID(float r_dis, float r_sec){
r_loops=0;
while (r_sec>r_loops){
r_loops++;
r_preverror=r_error;
prev_pos = pos;
pos = 0.5*(R.position(degrees)+L.position(degrees));
pos_change = pos-prev_pos;
r_error = r_dis-i.heading(degrees);
r_derivative = r_error-r_preverror;
r_pow= (r_error*r_kP)+r_derivative*r_kD;
R.spin(reverse,r_pow,volt);
L.spin(fwd,r_pow,volt);
wait(15,msec);
}
}
ltPID:
float l_kP=1;
float l_kD=1;
float l_error=0;
float l_derivative=0;
float l_preverror=0;
float l_pow=0;
float l_loops=0;
float l_diss=0;
void ltPID(float l_dis, float l_sec){
l_loops=0;
l_diss = 360-l_dis
while(l_sec>l_loops){
l_loops++;
l_preverror=l_error;
l_error = l_diss-(360-i.heading(degrees));
l_derivative = l_error-l_preverror;
l_pow= (l_error*l_kP)+l_derivative*l_kD;
R.spin(fwd,l_pow,volt);
L.spin(reverse,l_pow,volt);
wait(15,msec);
}
}
latPID
float kP=0;
float kD=0;
float error=0;
float derivative=0;
float preverror=0;
float lpow=0;
float loops=0;
float diss=0;
void latPID(float dis, float sec){
loops=0;
diss = dis*1;
while(dis>loops){
loops++;
preverror=error;
error = diss-(0.5*(R.position(degrees)+L.position(degrees)));;
derivative = error-preverror;
lpow= (error*kP)+derivative*kD;
R.spin(fwd,lpow,volt);
L.spin(fwd,lpow,volt);
wait(15,msec);
}
}
cPID
float c_kP=0;
float c_kD=0;
float c_error=0;
float c_derivative=0;
float c_preverror=0;
float c_pow=0;
float c_loops=0;
float c_diss=0;
float rc=1;
float lc=1;
float r_dis=0;
float l_dis=0;
void cPID(float c_dis, float c_sec, float curve){
rc = ((curve*-1)/2);
lc = ((curve*-1)/2);
c_loops=0;
c_diss = c_dis*1;
while(c_dis>c_loops){
c_loops++;
c_preverror=c_error;
r_dis =
c_error = c_diss-(0.5*(R.position(degrees)+L.position(degrees)));;
c_derivative = c_error-c_preverror;
c_pow= (c_error*c_kP)+c_derivative*c_kD;
R.spin(fwd,c_pow*rc,volt);
L.spin(fwd,c_pow*lc,volt);
wait(15,msec);
}
}
Tracking(odom):
double pi = 3.14159265359;//makes using pi easier
double prev_pos_lat=0;//explained in function
double prev_pos_lon=0;//explained in function
double pos_lat=0;//explained in function
double pos_lon=0;//explained in function
double curr_x=0;//explained in function
double curr_y=0;//explained in function
double curr_theta=0;//explained in function
double delta_x=0;//explained in function
double delta_y=0;//explained in function
double delta_theta=0;//explained in function
double prev_x=0;//explained in function
double prev_y=0;//explained in function
double prev_theta=0;//explained in function
double of_s=0;///absolute value of offset of horizontal tracking wheel
double of_r=0;///absolute value of vertical of horizontal tracking wheel
double rad_heading=0;//explained in function
double of_x=0;//explained in function
double of_y = 0;//explained in function
double curr_delta=0;//explained in function
double avg_orien=0;//explained in function
double tracking = true;//explained in function
double polar_radius=0;//explained in function
double polar_theta=0;//explained in function
double delta_r=0;//explained in function
double delta_s=0;//explained in function
double polar=0;//explained in function
double polar_y=0;//explained in function
void track (){
while(tracking){//status if to track or not
rad_heading = i.heading(degrees) * (pi/180);//converts inertial heading to radians
prev_theta = curr_theta;//value of previous theta
curr_theta = rad_heading;//makes the variable make sense, this is the current thets
delta_theta = curr_theta - prev_theta;//change in theta
prev_pos_lon = pos_lon;//previous l_t position
prev_pos_lat = pos_lat;//previous b_t position
pos_lat = l_t.position(degrees);//current l_t position
pos_lon = b_t.position(degrees);//current b_t position
delta_r = (pos_lat - prev_pos_lat)*3.25*pi;//change in l_t position, and converts it into inches
delta_s = (pos_lon - prev_pos_lon)*3.25*pi;//change in b_t position, and converts it into inches
if(delta_theta==0){//avoiding division of 0
of_x = delta_r;//if the bot does not rotate, then its just how far the tracking wheel moved
of_y = delta_s;//same here as last comment
}
else{
of_x = (2*sin(curr_delta/2)) * ((delta_r/delta_theta)+of_r);//if the bot rotates then nitty gritty
of_y = (2*sin(curr_delta/2)) * ((delta_s/delta_theta)+of_s);//same here for y
}
avg_orien = prev_theta + (delta_theta/2);//average orientation calculations
polar_radius = sqrt((of_x*of_x)+(of_y*of_y));//convert into polar coordinates
polar_theta = atan(of_y/of_x) + (avg_orien*-1);//converting theta of polar coordinates and adding average orientation
delta_x = polar * cos (polar_y);//converting back with average orientation
delta_x = polar * sin (polar_y); //same here
prev_x = curr_x;//previous x value
prev_y = curr_y;//previous y value
curr_x = delta_x + prev_x;//adds previous value and change in it to get current x
curr_y = delta_y + prev_y;//adds previous y and change in y to get current y
}
}
TurnLateralTurn PurePursuit:
double turn_1;
double turn_2;
double lateral;
double x_dis;
double y_dis=0;
void TLT_PurePursuit(float d_x,float d_y,float theta){
x_dis = d_x - curr_x;
y_dis = d_y - curr_y;
lateral = sqrt((x_dis*x_dis)+(y_dis*y_dis));
turn_1 = atan(y_dis/x_dis);
turn_2 = theta;
if(turn_1>i.heading(degrees)){
ltPID(turn_1,turn_1/2);
}
else{
rtPID(turn_1,turn_1/2);
}
latPID(lateral, lateral/150);
if(turn_2>i.heading(degrees)){
ltPID(turn_2,turn_2/2);
}
else{
rtPID(turn_2,turn_2/2);
}
}
Please tell me if anything is wrong with any of these! It took very long to improve onto this, so any help, time or critisism would be appreciated!