main_003

close all; clear; clc;

global debugLevel_01;
       debugLevel_01 = 1;

% Initialize values
L1 = 110.0; % Link Length in mm
L2 = 140.0; % Link Length in mm
SUM_L1_L2 = L1 + L2;

initalizeArmCoordinates;
printArmCoordinates;

%%%%%%%%%%%%%%%%%%%%
%%%%  STEP 1  %%%%%%
%%%%%%%%%%%%%%%%%%%%
title_string = sprintf('Robotic Arm: INITIAL POSITION');
output_file_name = 'out_01_Initial_Position.jpg';
plot_properly;

%%%%%%%%%%%%%%%%%%%%
%%%%  STEP 2  %%%%%%
%%%%%%%%%%%%%%%%%%%%
 % Call the rotate function
 % rotate_motor_logically( motor_no,  angleInDegree, lx, ly, debugFlag)
 % Negative angle is Ant-clockwise
 % Positive angle is Clockwise
 % Try changing the angle: Note that if first motor is rotated second is
 % also moved!
  [ successFlag, lx, ly ] = rotate_motor_logically( 1,  -60, lx, ly, 1);

% Plot the robot arm AFTER rotating Motor 1
title_string = sprintf('Robotic Arm: AFTER ROTATING Motor 1');
output_file_name = 'out_02_Rotate_M1.jpg';
plot_properly;

%%%%%%%%%%%%%%%%%%%%
%%%%  STEP 3  %%%%%%
%%%%%%%%%%%%%%%%%%%%
% Call the rotate function
% rotate_motor_logically( motor_no,  angleInDegree, lx, ly, debugFlag)
% Negative angle is Ant-clockwise
% Positive angle is Clockwise
% Try changing the angle: Note that if second motor is rotated first link
% remains on the same position, obviously
[ successFlag, lx, ly ] = rotate_motor_logically( 2,  40, lx, ly, 1);

% Plot the robot arm AFTER rotating Motor 2
title_string = sprintf('Robotic Arm: AFTER ROTATING Motor 2');
output_file_name = 'out_03_Rotate_M2.jpg';
plot_properly;

 lx[1] = 0.00      lx[2] = 110.00    lx[3] = 110.00 
 ly[1] = 0.00      ly[2] = 0.00      ly[3] = 140.00 

	DEBUG: Before Rotating motor 1
	DEBUG:  lx[1] = 0.00      lx[2] = 110.00    lx[3] = 110.00 
	DEBUG:  ly[1] = 0.00      ly[2] = 0.00      ly[3] = 140.00 

	DEBUG: After Rotating motor 1 by -60.000000 degree
	DEBUG: nlx[1] = 0.00     nlx[2] = 55.00    nlx[3] = -66.24 
	DEBUG: nly[1] = 0.00     nly[2] = 95.26    nly[3] = 165.26 

	DEBUG: Before Rotating motor 2
	DEBUG:  lx[1] = 0.00      lx[2] = 55.00     lx[3] = -66.24 
	DEBUG:  ly[1] = 0.00      ly[2] = 95.26     ly[3] = 165.26 

	DEBUG: After Rotating motor 2 by 40.000000 degree
	DEBUG: nlx[1] = 0.00     nlx[2] = 55.00    nlx[3] = 7.12   
	DEBUG: nly[1] = 0.00     nly[2] = 95.26    nly[3] = 226.82