motion_control.m

pkg load symbolic

function ang_vel = getAngVel (lin_vel, radius, theta)
    ang_vel = abs(lin_vel) * sin(theta) / radius;
endfunction

function ang_vel_diff = getAngVelDiff (mb_len, radius, theta, t0, t1, t2, t3)
    ang_vel_diff = 2 * mb_len / radius * theta * (t3 - t0 + t2 - t1);
endfunction

function save_plot(ang_vel, t0, t1, t2, t3, fname)
    x_coords = [t0, t1, t2, t3];
    y_coords = [0, ang_vel, ang_vel, 0];
    plot(x_coords, y_coords);
    fname = strcat("other/", fname, ".jpg");
    print(fname, "-djpg");
endfunction

function main(verbose=false)
    if(verbose==true)
        disp("\nAll units are on meters, seconds, and radians");
    endif

    primary_ang_vel = 600;

    L = 0.2;
    r = 2;
    th = 135;
    t0 = 0; t1 = 0.1; t2 = 0.5; t3 = 0.6;

    ang_vel_diff = getAngVelDiff(L, r, th, t0, t1, t2, t3);
    secondary_ang_vel = primary_ang_vel + ang_vel_diff;

    disp(["\nPrimary wheel angular velocity (rad/s): ", num2str(primary_ang_vel)]);
    disp(["Angular velocity difference (rad/s): ", num2str(ang_vel_diff)]);
    disp(["Secondary wheel angular velocity (rad/s): ", num2str(secondary_ang_vel)]);

    disp(["\nGenerating primary wheel plot..."]);
    save_plot(primary_ang_vel, t0, t1, t2, t3, "primary_wheel");
    disp(["Generating secondary wheel plot..."]);
    save_plot(secondary_ang_vel, t0, t1, t2, t3, "secondary_wheel");

endfunction

main(verbose=true);