clear % time dt = 0.01; % test command profile t = [0:dt:5]; vxcmd = interp1([0, 1, 2, 3, 4, 5], [0, 50, 50, 0, 0, 0], t); vycmd = interp1([0, 1, 2, 3, 4, 5], [0, 50, 50, 0, 0, 0], t); rcmd = interp1([0, 1, 2, 3, 4, 5], [0, 45, 0, 0, 0, 0], t); % estimated swerve drive parameters k_v = 200; % max speed = 15 ft/sec, converted to inches tau_v = 0.05; % speed time constant k_t = 3000; % max speed 3000 deg/sec tau_t = 0.05; % rotation time constant % gyro loop claws kvgyro = 3.0; krgyro = 1.0; taugyro = 0.0; kff = 0.95; % gyro_on = true; gyro_on = false; % inititial conditions theta1 = 0; thetad1 = 0; thetadd1 = 0; v1 = 0; vd1 = 0; vt1 = 0; theta2 = 0; thetad2 = 0; thetadd2 = 0; v2 = 0; vd2 = 0; vt2 = 0; theta3 = 0; thetad3 = 0; thetadd3 = 0; v3 = 0; vd3 = 0; vt3 = 0; theta4 = 0; thetad4 = 0; thetadd4 = 0; v4 = 0; vd4 = 0; vt4 = 0; % gyro loop ic psidot = 0; rerrint = 0; ff = 0; % initial controller state template init_state.terrint = 0; init_state.tcz = 0; init_state.verrint = 0; init_state.vcz = 0; init_state.rev = false; init_state.tv_cmd = 0; % init controller states for 4 swerve modules state1 = init_state; state2 = init_state; state3 = init_state; state4 = init_state; function dbcmd = db(cmd) db = 0.04; dbcmd = sign(cmd).*limit(0, 1, (abs(cmd) - db)/(1 - db)); end function [theta, thetad, v, vt] = update_state(theta, thetad, thetadd, v, vd, dt) theta = theta + thetad*dt; thetad = thetad + thetadd*dt; v = v + vd*dt; kvt = 0.0185; vt = kvt*thetad; end % loop over time for n = 2:length(t) % propagate swerve states % module 1 [theta1(n), thetad1(n), v1(n) vt1(n)] ... = update_state(theta1(n - 1), thetad1(n - 1), thetadd1(n - 1), ... v1(n - 1), vd1(n - 1), dt); % module 2 [theta2(n), thetad2(n), v2(n), vt2(n)] ... = update_state(theta2(n - 1), thetad2(n - 1), thetadd2(n - 1), ... v2(n - 1), vd2(n - 1), dt); % module 3 [theta3(n), thetad3(n), v3(n), vt3(n)] ... = update_state(theta3(n - 1), thetad3(n - 1), thetadd3(n - 1), ... v3(n - 1), vd3(n - 1), dt); % module 4 [theta4(n), thetad4(n), v4(n), vt4(n)] ... = update_state(theta4(n - 1), thetad4(n - 1), thetadd4(n - 1), ... v4(n - 1), vd4(n - 1), dt); % estimate psidot cr = [14, 14]; p1 = [24.75, 24.75] - cr; p2 = [ 3.25, 24.75] - cr; p3 = [ 3.25, 3.25] - cr; p4 = [24.75, 3.25] - cr; vrx = v1(n)*cosd(theta1(n)) - v3(n)*cosd(theta3(n)); vry = v1(n)*sind(theta1(n)) - v3(n)*sind(theta3(n)); psidot1 = - vrx/(p1(2) - p3(2)); psidot2 = vry/(p1(1) - p3(1)); psidot(n) = 180/pi*(psidot1 + psidot2)/2; % gyro stabilized rcmd % PI controller with feed forward ff = rcmd(n); rerr = rcmd(n) - psidot(n); rerrint = rerrint + rerr*dt; rcmd_gyro = kvgyro/krgyro*(taugyro*rerr + rerrint) + kff*ff; if (!gyro_on) rerrint = 0; rcmd_gyro = rcmd(n); end % control laws % swerve kinematics [vc1, tc1, ... vc2, tc2, ... vc3, tc3, ... vc4, tc4] = swerve_cmd(vxcmd(n), vycmd(n), rcmd_gyro); % diagnostic outputs vcommand(n) = vc1; tcommand(n) = tc1; % module steering controls [tdcmd1, state1] = steering_claws(tc1, state1, theta1(n), thetad1(n), dt); [tdcmd2, state2] = steering_claws(tc2, state2, theta2(n), thetad2(n), dt); [tdcmd3, state3] = steering_claws(tc3, state3, theta3(n), thetad3(n), dt); [tdcmd4, state4] = steering_claws(tc4, state4, theta4(n), thetad4(n), dt); % module speed controls [vcmd1, state1] = vel_claws(vc1, state1, v1(n), dt); [vcmd2, state2] = vel_claws(vc2, state2, v2(n), dt); [vcmd3, state3] = vel_claws(vc3, state3, v3(n), dt); [vcmd4, state4] = vel_claws(vc4, state4, v4(n), dt); % swerve acceleration states % module 1 thetadd1(n) = 1/tau_t*(k_t*db(tdcmd1) - thetad1(n)); vd1(n) = 1/tau_v*(k_v*db(vcmd1) - v1(n)); % module 2 thetadd2(n) = 1/tau_t*(k_t*db(tdcmd2) - thetad2(n)); vd2(n) = 1/tau_v*(k_v*db(vcmd2) - v2(n)); % module 3 thetadd3(n) = 1/tau_t*(k_t*db(tdcmd3) - thetad3(n)); vd3(n) = 1/tau_v*(k_v*db(vcmd3) - v3(n)); % module 4 thetadd4(n) = 1/tau_t*(k_t*db(tdcmd4) - thetad4(n)); vd4(n) = 1/tau_v*(k_v*db(vcmd4) - v4(n)); % end of time domain simulation end % diagnostic outputs of module 1 figure(1); subplot(2, 1, 1); plot(t, tcommand, t, theta1); title('Steering command, module 1'); ylabel('deg'); xlabel('sec'); legend({'Cmd', 'Angle'}); grid on; subplot(2, 1, 2); plot(t, vcommand, t, v1 + vt1); title('Velocity command, module 1'); ylabel('in/sec'); xlabel('sec'); legend({'Cmd', 'Vel'}); grid on; % estimate kinematics robotxb = [12, -12, -12, 12, 12]; robotyb = [6, 6, -6, -6, 6]; robotxb = [24.75, 3.25, 3.25, 24.75, 24.75] - 7; robotyb = [24.75, 24.75, 3.25, 3.25, 24.75] - 7; thetar = 0; for n = 2:length(t) robotxb(n,:) = robotxb(n - 1,:) ... + [v1(n) + vt1(n), ... v2(n) + vt2(n), ... v3(n) + vt3(n), ... v4(n) + vt4(n), ... v1(n) + vt1(n)] ... .* cosd([theta1(n) + thetar(n - 1), ... theta2(n) + thetar(n - 1), ... theta3(n) + thetar(n - 1), ... theta4(n) + thetar(n - 1), ... theta1(n) + thetar(n - 1)])*dt; robotyb(n,:) = robotyb(n - 1,:) ... + [v1(n) + vt1(n), ... v2(n) + vt2(n), ... v3(n) + vt3(n), ... v4(n) + vt4(n), ... v1(n) + vt1(n)] ... .* sind([theta1(n) + thetar(n - 1), ... theta2(n) + thetar(n - 1), ... theta3(n) + thetar(n - 1), ... theta4(n) + thetar(n - 1), ... theta1(n) + thetar(n - 1)])*dt; thetar(n) = atan2d(robotyb(n, 1) - mean(robotyb(n, 1:4)), ... robotxb(n, 1) - mean(robotxb(n, 1:4))) ... - atan2d(robotyb(1, 1), robotxb(1, 1)); end figure(2); plot(robotyb(1:0.5/dt:end,:)', robotxb(1:0.5/dt:end,:)', '1'); grid on; axis('equal'); title('Robot Path'); ylabel('North (in)'); xlabel('East (in)');