% Power Supply parameters
% -------------------------------------------------------
%
% pwr_supply_terminal_voltage = pwr_supply_nominal_voltage - pwr_supply_input_resistance * current.
% 
% Nominal voltage: pwr_supply_nominal_voltage
pwr_supply_nominal_voltage=30;		% [Volts] (What the battery is rated at)
 
% Input resistance: pwr_supply_input_resistance
pwr_supply_input_resistance=0.15;	% [Ohms]; this is assumed,
                                        % based on the Scout II
                                        % lead-acid battery values.
                                        % I don't really know what
                                        % the value is.

% Motor parameters (Maxon A-max 22, 6 Watt family: #110151)
% -------------------------------------------------------
%
% Gear Ratio (Catalogue Value)
mtr_gear_ratio_catalogue=333; 		% Maxon Gear #143995
					% (not "absolute
                                        % reduction", which is
                                        % 69255/208)
% Distal Joint Gear Ratio (Catalogue Value)
distal_mtr_gear_ratio_catalogue=479;    % Maxon Gear #144000
					% (not "absolute
                                        % reduction", which is
                                        % 124659/260)					
% Gear Efficiency (Catalogue Value)
mtr_gear_effic_catalogue=0.49; 		% Maxon Gear #143995
                                        % (1=100% efficiency)

% Speed constant (Catalogue, bare motor)
mtr_spd_const_catalogue=450;		% [rpm/volt] 

% Speed constant (with Maxon gearhead #143995), converted to
%                                           [deg/(sec*volt)]
mtr_spd_const=mtr_spd_const_catalogue*360/60* ...
    mtr_gear_ratio_catalogue; 		% [deg/(sec*volt)] 

% Speed constant (with Maxon gearhead #14400), converted to
%                                           [deg/(sec*volt)]
distal_mtr_spd_const=mtr_spd_const_catalogue*360/60* ...
    distal_mtr_gear_ratio_catalogue; 		% [deg/(sec*volt)] 


% Stall Torque (Catalogue)
mtr_stall_torque_catalogue=0.0234;	% [Nm] stall torque, no
                                        % gearbox

% Basalar and Thoracic:
mtr_stall_torque_withgear=mtr_stall_torque_catalogue* ...
    mtr_gear_effic_catalogue*mtr_gear_ratio_catalogue;% [Nm] stall torque, with gearbox

% Distal...
distal_mtr_stall_torque_withgear=mtr_stall_torque_catalogue* ...
    mtr_gear_effic_catalogue*distal_mtr_gear_ratio_catalogue;% [Nm] stall torque, with gearbox


% No Load Speed (Catalogue)
mtr_noload_speed_catalogue = 10500;		% [rpm]

% No Load Speed (with Maxon gearhead #143995), converted to [deg/sec]
mtr_noload_spd_withgear=mtr_noload_speed_catalogue*360/60*(1/ ...
						  mtr_gear_ratio_catalogue); % [deg/sec]

% Distal No Load Speed (with Maxon gearhead #14400), converted to [deg/sec]
distal_mtr_noload_spd_withgear=mtr_noload_speed_catalogue*360/60*(1/ ...
						  distal_mtr_gear_ratio_catalogue); % [deg/sec]


% Import the ADAMS-produced data (changed to CSV in MS Excel)
% Column 1: time
% Column 2: .thoracic_joint_1.Element_Torque.Z
% Column 3: .thoracic_joint_1.Angular_Velocity.Z
% Column 4: .basalar_joint_1.Element_Torque.Z
% Column 5: .basalar_joint_1.Angular_Velocity.Z
% Column 6: .distal_joint_1.Element_Torque.Z
% Column 7: .distal_joint_1.Angular_Velocity.Z
% Column 8: .BODY.CM_Velocity.X
% Column 9: .BODY.CM_Velocity.Z

m = csvread('scorpion_mcgill_threequartersspeed.csv');
 
avg_spd=sum(sqrt(m(:,8).^2+m(:,9).^2))/length(m(:,1)); % [mm/sec]

sim_torque_thoracic=m(:,2)/1000;	% thoracic joint torque;
					% convert from [mNm] to [Nm]
sim_angvel_thoracic=m(:,3);		% thoracic joint angular
                                        % velocity
sim_torque_basalar=m(:,4)/1000;		% basalar joint torque;
					% convert from [mNm] to [Nm]
sim_angvel_basalar=m(:,5);		% basalar joint angular
                                        % velocity
sim_torque_distal=m(:,4)/1000;		% basalar joint torque;
					% convert from [mNm] to [Nm]
sim_angvel_distal=m(:,5);		% basalar joint angular velocity

% Draw the Torque-Speed Curve Graph

figure(1);
subplot(3,1,1);				% THORACIC JOINT
  % add the lines of the motor speed-torque saturation zone.
  slope=(0-mtr_noload_spd_withgear)/(mtr_stall_torque_withgear-0);
  y_intercept=mtr_noload_spd_withgear;
  pnt1_x=-mtr_stall_torque_withgear;
  pnt1_y=slope*pnt1_x+y_intercept;
  pnt2_x=0;
  pnt2_y=slope*pnt2_x+y_intercept;
  pnt3_x=mtr_stall_torque_withgear;
  pnt3_y=slope*pnt3_x+y_intercept;
  pnt4_x=-pnt1_x;
  pnt4_y=-pnt1_y;
  pnt5_x=pnt2_x;
  pnt5_y=-pnt2_y;
  pnt6_x=-pnt3_x;
  pnt6_y=-pnt3_y;
  hold on
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x pnt1_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y pnt1_y]);
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y],'r*');
  % plot the simulation results
  plot(sim_torque_thoracic, sim_angvel_thoracic,'g*');
  hold off

  axis([1.5*pnt1_x 3.5*pnt3_x -1.5*pnt1_y 1.5*pnt1_y])

  xlabel('Torque [Nm]');
  ylabel('Speed [deg/sec]');
  title('Scorpion Thoracic Motor Torque-Speed Curve');
  text(pnt3_x,pnt3_y+20,' + Stall Torque');
  text(pnt2_x,pnt2_y+20,[' + No Load Speed = ', num2str(mtr_noload_spd_withgear), ' [deg/sec]']);
  text(pnt1_x,pnt1_y+20,[' - Stall Torque = ',num2str(-mtr_stall_torque_withgear), ' [Nm]']);
  
subplot(3,1,2);				% BASALAR JOINT
  % add the lines of the motor speed-torque saturation zone.
  slope=(0-mtr_noload_spd_withgear)/(mtr_stall_torque_withgear-0);
  y_intercept=mtr_noload_spd_withgear;
  pnt1_x=-mtr_stall_torque_withgear;
  pnt1_y=slope*pnt1_x+y_intercept;
  pnt2_x=0;
  pnt2_y=slope*pnt2_x+y_intercept;
  pnt3_x=mtr_stall_torque_withgear;
  pnt3_y=slope*pnt3_x+y_intercept;
  pnt4_x=-pnt1_x;
  pnt4_y=-pnt1_y;
  pnt5_x=pnt2_x;
  pnt5_y=-pnt2_y;
  pnt6_x=-pnt3_x;
  pnt6_y=-pnt3_y;
  hold on
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x pnt1_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y pnt1_y]);
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y],'r*');
  % plot the simulation results
  plot(sim_torque_basalar, sim_angvel_basalar,'g*');
  hold off

  axis([1.5*pnt1_x 3.5*pnt3_x -1.5*pnt1_y 1.5*pnt1_y])

  xlabel('Torque [Nm]');
  ylabel('Speed [deg/sec]');
  title('Scorpion Basalar Motor Torque-Speed Curve');
  text(pnt3_x,pnt3_y+20,' + Stall Torque');
  text(pnt2_x,pnt2_y+20,[' + No Load Speed = ', num2str(mtr_noload_spd_withgear), ' [deg/sec]']);
  text(pnt1_x,pnt1_y+20,[' - Stall Torque = ',num2str(-mtr_stall_torque_withgear), ' [Nm]']);
  
  subplot(3,1,3);				% DISTAL JOINT
  % add the lines of the motor speed-torque saturation zone.
  slope=(0-distal_mtr_noload_spd_withgear)/(distal_mtr_stall_torque_withgear-0);
  y_intercept=distal_mtr_noload_spd_withgear;
  pnt1_x=-distal_mtr_stall_torque_withgear;
  pnt1_y=slope*pnt1_x+y_intercept;
  pnt2_x=0;
  pnt2_y=slope*pnt2_x+y_intercept;
  pnt3_x=distal_mtr_stall_torque_withgear;
  pnt3_y=slope*pnt3_x+y_intercept;
  pnt4_x=-pnt1_x;
  pnt4_y=-pnt1_y;
  pnt5_x=pnt2_x;
  pnt5_y=-pnt2_y;
  pnt6_x=-pnt3_x;
  pnt6_y=-pnt3_y;
  hold on
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x pnt1_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y pnt1_y]);
  plot([pnt1_x pnt2_x pnt3_x pnt4_x pnt5_x pnt6_x],[pnt1_y pnt2_y ...
		    pnt3_y pnt4_y pnt5_y pnt6_y],'r*');
  % plot the simulation results
  plot(sim_torque_distal, sim_angvel_distal,'g*');
  hold off

  axis([1.5*pnt1_x 3.5*pnt3_x -1.5*pnt1_y 1.5*pnt1_y])

  xlabel('Torque [Nm]');
  ylabel('Speed [deg/sec]');
  title('Scorpion Distal Motor Torque-Speed Curve');
  text(pnt3_x,pnt3_y+20,' + Stall Torque');
  text(pnt2_x,pnt2_y+20,[' + No Load Speed = ', num2str(distal_mtr_noload_spd_withgear), ' [deg/sec]']);
  text(pnt1_x,pnt1_y+20,[' - Stall Torque = ',num2str(- distal_mtr_stall_torque_withgear), ' [Nm]']);