Motor_Control(varargin)

function varargout = Motor_Control(varargin)
% MOTOR_CONTROL M-file for Motor_Control.fig
%      MOTOR_CONTROL, by itself, creates a new MOTOR_CONTROL or raises the existing
%      singleton*.
%
%      H = MOTOR_CONTROL returns the handle to a new MOTOR_CONTROL or the handle to
%      the existing singleton*.
%
%      MOTOR_CONTROL('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in MOTOR_CONTROL.M with the given input arguments.
%
%      MOTOR_CONTROL('Property','Value',...) creates a new MOTOR_CONTROL or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before Motor_Control_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to Motor_Control_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Copyright 2002-2003 The MathWorks, Inc.

% Edit the above text to modify the response to help Motor_Control

% Last Modified by GUIDE v2.5 12-Apr-2010 06:39:05

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
    'gui_Singleton',  gui_Singleton, ...
    'gui_OpeningFcn', @Motor_Control_OpeningFcn, ...
    'gui_OutputFcn',  @Motor_Control_OutputFcn, ...
    'gui_LayoutFcn',  [] , ...
    'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before Motor_Control is made visible.
function Motor_Control_OpeningFcn(hObject, eventdata, handles, varargin)
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
clc;
global sPort CyclesPerRev Cycles Scale P1 P2 P3; % Define global variables
Cycles = 15;                % No of PWM cycles applied in one go.
CyclesPerRev = 8;           % No of encoder pulses per revolution
Scale = 600/CyclesPerRev;   % Scale to convert pulse count to RPM
P1 = 0.015525;              % Calibration constant 1
P2 = 0.11082;               % Calibration constant 2
P3 = 2.2737;                % Calibration constant 3
devices = instrfind; % Find all open devices
if(~isempty(devices)) % If devices are open
    fclose(devices);  % Close the devices
end
subplot(1,1,1,'Position',[0.2 0.05 0.75 0.8],'FontSize',1);
I = imread('System.jpg'); %Read the image of the system
imshow(I);              %Show the image
box on;
sPort = serial('COM4','BaudRate',9600,'Tag','BTDevice','Timeout',10,'Terminator',10,'OutputBufferSize',8)
fopen(sPort) %Open serial port

% --- Outputs from this function are returned to the command line.
function varargout = Motor_Control_OutputFcn(hObject, eventdata, handles)
varargout{1} = handles.output;

% --- Executes on button press in button_video.
function button_video_Callback(hObject, eventdata, handles)
vid = videoinput('winvideo', 1, 'RGB24_720x480'); %Define video input object
preview(vid);  % Preview the video object

% % --- Executes on button press in button_run.
function button_run_Callback(hObject, eventdata, handles)
global sPort CyclesPerRev Cycles P1 P2 P3 Scale; % Define global variables

Opto_Count_Max = 80  %Maximum optical encoder count
Kp = 2
Kd = 0.01
Ki = 1
i = 1;              % Loop counter

PWM_Levels = 100;   % No of PWM levels
Req_RPM = str2num(get(handles.edit_reqdrpm,'String')) % Read required RPM
E_Opto_Count = [0];  
e_Time = 1;
DE_Opto_Count = [0];
IE_Opto_Count = [0];
Opto_Counter = [0];
I = [1];
Cycles = 10
set(handles.text_message,'String','Running!')
if(get(handles.checkbox_def_calibration,'Value')==1) % If default calibration is checked
    P1 = 0.015525;
    P2 = 0.11082;
    P3 = 2.2737;
end
if(get(handles.radio_openloop,'Value') == 1)  % Classifying Control
    control_type = 0;                           % Open Loop
%     Cycles = 15
elseif(get(handles.radio_onoff,'Value') == 1)   % Classifying Control
    opto_gap = 0;
    control_type = 1;                           % On Off
%     Cycles = 15
elseif(get(handles.radio_differential,'Value') == 1)  % Classifying Control
    rpm_gap = str2num(get(handles.edit_rpm,'String'));
    opto_gap = rpm_gap/Scale;
    control_type = 1;                           %Differential Gap
%     Cycles = 15
elseif(get(handles.radio_p,'Value') == 1)  % Classifying Control
    control_type = 2;
    Kp = str2num(get(handles.edit_kp,'String')); %Proportional
    Kd = 0;
    Ki = 0;
%     Cycles = 15
elseif(get(handles.radio_pd,'Value') == 1)  % Classifying Control
    control_type = 2;                       % PD
    Kp = str2num(get(handles.edit_kp,'String'));
    Kd = str2num(get(handles.edit_kd,'String'));
    Ki = 0;
%     Cycles = 15
elseif(get(handles.radio_pid,'Value') == 1)  % Classifying Control
    control_type = 2;                           %PID
    Kp = str2num(get(handles.edit_kp,'String'));
    Kd = str2num(get(handles.edit_kd,'String'));
    Ki = str2num(get(handles.edit_ki,'String'));
%     Cycles = 15
end

Req_Opto_Count =Req_RPM*CyclesPerRev/(600);   % Calculating Required Optical Encoder Count from Required RPM
% DutyCycle = 0.088*Req_Opto_Count^2 + 0.171*Req_Opto_Count + 20.03+40     % Default Calibration Equation
DutyCycle = P1*Req_Opto_Count^2 + P2*Req_Opto_Count + P3 +40               % Calibration Equation
status = 1          % While the Run radio button is selected
while(status == 1)
    i= i+1;                     % increment loop counter
    I = [I i];
    if(DutyCycle>PWM_Levels)            % If Calculated Duty Cycle>Max Value
        DutyCycle = PWM_Levels;         % reduce to Max Value
    elseif(DutyCycle<0)                 % If Duty Cycle comes out to be negative
        DutyCycle = 0;                  % reduce to zero
    end

    fprintf(sPort,'%d\n',round(DutyCycle));  % Send Duty Cycle to Master BS2
    fprintf(sPort,'%d\n',Cycles);           % Send No of PWM Cycles to Master BS2
    Opto_Count = fscanf(sPort,'%d\n');      % Receive Optical Encoder Count from Master BS2
    Opto_Counter = [Opto_Counter Opto_Count];
    e_Opto_Count = Req_Opto_Count-Opto_Count;  % Calculate error in actual and required count
    E_Opto_Count = [E_Opto_Count e_Opto_Count];
    if(control_type == 0)               %Open Loop Control
        subplot(1,2,1,'Position',[0.2 0.05 0.35 0.8],'FontSize',10);
        box on;
        plot(I,Opto_Counter*Scale,'-b','Linewi',1)
        hold on;
        plot(I,Req_Opto_Count*Scale*ones(size(I)),'-r','Linewi',1);
        title('RPM Plot');
        grid on
        subplot(1,2,2,'Position',[0.60 0.05 0.35 0.8],'FontSize',10);
        box on
        plot(I,E_Opto_Count*Scale,'-r','Linewi',1);
        title('RPM Error Plot');
        grid on
        hold on
        pause(0.001)
    elseif(control_type == 1)           %On-Off & Differential Gap Control
        if(Req_Opto_Count-Opto_Count>opto_gap)  % Decide duty cycle based on error and gap
            DutyCycle = PWM_Levels;
        elseif(Opto_Count-Req_Opto_Count>opto_gap)
            DutyCycle = 0;
        end
%         if(Opto_Count>Req_Opto_Count)
%             Cycles = 1;
%         end
%         if(Opto_Count>Req_Opto_Count & opto_gap~=0)
%             Cycles = 1;
%         end
        if(abs(Opto_Count-Req_Opto_Count)<opto_gap)
            Cycles = 5;
        else
            Cycles = 15;
        end
        subplot(1,2,1,'Position',[0.2 0.05 0.35 0.8],'FontSize',10);
        plot(I,Opto_Counter*Scale,'-b','Linewi',1)
        hold on
        plot(I,Req_Opto_Count*Scale*ones(size(I)),'-r','Linewi',1);
        title('RPM Plot');
        grid on
        subplot(1,2,2,'Position',[0.60 0.05 0.35 0.8],'FontSize',10);
        plot(I,E_Opto_Count*Scale,'-r','Linewi',1);
        title('RPM Error Plot');
        grid on
        hold on
        pause(0.001)
    elseif(control_type == 2)           %P, PD & PID Control
        de_Opto_Count = (e_Opto_Count-E_Opto_Count(i-1))/e_Time; %Caculating derivative control action
        DE_Opto_Count = [DE_Opto_Count de_Opto_Count];
        ie_Opto_Count = trapz(E_Opto_Count)*e_Time;         %Calculating integral control action
        IE_Opto_Count = [IE_Opto_Count ie_Opto_Count];
        New_Opto_Count = Kp*e_Opto_Count+Kd*de_Opto_Count+Ki*ie_Opto_Count; % Calculate new optical encoder count
        if(((Opto_Count-Req_Opto_Count)>0) && Ki~=0)  % Adjust no of cycles based on current error
            Cycles = 1;
%         else
%             Cycles = 15;
        end
%         if((Opto_Count-Req_Opto_Count)>0 && Ki~=0)
%            Cycles = 5; 
%         elseif((Opto_Count-Req_Opto_Count)<-30 && Ki~=0)
%         else
%             Cycles = 15; 
%         end
        subplot(2,2,1,'Position',[0.2 0.50 0.35 0.35],'FontSize',10); % Plot all quantities
        box on
        plot(I,Opto_Counter*Scale,'-b','Linewi',1)
        hold on
        plot(I,Req_Opto_Count*Scale*ones(size(I)),'-r','Linewi',1);
        title('RPM Plot','Fontsize',12)
        grid on
        subplot(2,2,2,'Position',[0.6 0.50 0.35 0.35],'FontSize',10);
        plot(I,Kp*E_Opto_Count*Scale,'-r')
        title('Kp*RPM Error Plot','Fontsize',12)
        grid on
        hold on
        subplot(2,2,3,'Position',[0.2 0.06 0.35 0.35],'FontSize',10);
        box on
        plot(I,Kd*DE_Opto_Count*Scale,'-g')
        title('Kd*Error Derivative Plot','Fontsize',12)
        grid on
        hold on
        subplot(2,2,4,'Position',[0.6 0.06 0.35 0.35],'FontSize',10);
        box on
        plot(I,Ki*IE_Opto_Count*Scale,'-k')
        title('Ki*Error Integral Plot','Fontsize',12)
        grid on
        hold on

        pause(0.001)                    %Pause to Plot the data
        if(New_Opto_Count>Opto_Count_Max)       % Check new optical encoder count 
            New_Opto_Count = Opto_Count_Max;
        elseif(New_Opto_Count<0)
            New_Opto_Count = 0;
        end
        DutyCycle = P1*New_Opto_Count^2 + P2*New_Opto_Count + P3;                
        % Actual Calibration Equation
    end
    if (get(handles.radio_stop,'Value')==1)
        set(handles.text_message,'String','Stopping!');
        drawnow;
        status = 0;
        fprintf(sPort,'%d\n',0);  % Send 0 Duty Cycle to Master BS2 to stop motor
        fprintf(sPort,'%d\n',0);  % Send 0 No of PWM Cycles to Master BS2 to stop motor
    end
end
% fprintf(sPort,'%d\n',0);  % Send 0 Duty Cycle to Master BS2
% fprintf(sPort,'%d\n',0);           % Send 0 No of PWM Cycles to Master BS2
set(handles.text_message,'String','Stopped!');
if(get(handles.checkbox_log,'Value')==1)
    c = clock;
    xlswrite(['Data' date num2str(c(4)) num2str(c(5))],[Opto_Counter' E_Opto_Count']);
    set(handles.text_message,'String','Data Logged!') %Log data in xls file
end

% --- Executes on button press in button_calibrate.
function button_calibrate_Callback(hObject, eventdata, handles)
global sPort Cycles Scale P1 P2 P3;
syms z;
Duty_Cycle = [];
Opto_Counter = [];
Cycles = 10;
subplot(1,1,1,'Position',[0.2 0.06 0.75 0.8],'FontSize',10);
set(handles.text_message,'String','Calibrating! Please Wait...')
calibration_type = get(handles.dropdown_calibration,'Value') %Get level of accuracy
if(calibration_type ==1)  % High Accuracy
    delta_Duty=1;
elseif(calibration_type ==2) % Medium Accuracy
    delta_Duty=2;
elseif(calibration_type ==3) %Low Accuracy
    delta_Duty=3;
end

disp('Synchronized');
cla;
axis([0 80 0 120]);
xlabel('Optical Encoder Count \rightarrow','FontSize',12);
ylabel('Duty Cycle \rightarrow','FontSize',12);
title('Calibration Plot','FontSize',14);
grid on
hold on
for Duty=140:-delta_Duty:0   % Vary duty cycle to obtain the calibration plot
    if(Duty>100)
        Duty = 100;
    end
    fprintf(sPort,'%d\n',Duty);   % Send duty to Master BS2
    fprintf(sPort,'%d\n',Cycles); % Send cycles to Master BS2
    Opto_Count = fscanf(sPort,'%d\n') %Scan optical encoder count
    Duty_Cycle = [Duty_Cycle Duty];
    Opto_Counter = [Opto_Counter Opto_Count];
    plot(Opto_Counter,Duty_Cycle,'-r','Linewi',1);
    pause(0.001);
end
Opto_Counter = Opto_Counter(40/delta_Duty:140/delta_Duty);
Duty_Cycle = Duty_Cycle(40/delta_Duty:140/delta_Duty);
[fit1,gof1,out1] = fit(Opto_Counter',Duty_Cycle','poly2'); %Fit 2 degree polynomial
P1 = fit1.p1        % Coefficient 1
P2 = fit1.p2        % Coefficient 2
P3 = fit1.p3        % Coefficient 3
text(1,65,['Coefficient of determination = ' num2str(gof1.rsquare)],'FontSize',12);
text(1,55,['Root mean squared error = ' num2str(gof1.rmse)],'FontSize',12);
text(1,45,['DC = ' num2str(P1) 'x^2+' num2str(P2) 'x+' num2str(P3)],'FontSize',12);
f = P1*z^2+P2*z+P3; %Obtain calibration equation
ezplot(f,[0 80]); % Plot calibration equation
axis([0 80 0 120]);
xlabel('Optical Encoder Count \rightarrow','FontSize',12);
title('Calibration Plot','FontSize',14);
if(gof1.rmse>5)  % If rms value is more than 5 => Poor Calibration
    set(handles.text_message,'String','Poor Calibration!, Use Default Calibration')
else
    set(handles.text_message,'String','Good Calibration!')
end
if(get(handles.checkbox_log,'Value')==1)
    c = clock;
    xlswrite(['Calibration' date num2str(c(4)) num2str(c(5))],[Opto_Counter' Duty_Cycle']);
    set(handles.text_message,'String','Data Logged!') %Log data in xls file
end

% --- Executes on selection change in dropdown_calibration.
function dropdown_calibration_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function dropdown_calibration_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_kp_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function edit_kp_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_kd_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function edit_kd_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_ki_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function edit_ki_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_rpm_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function edit_rpm_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function edit_reqdrpm_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties.
function edit_reqdrpm_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in button_exit.
function button_exit_Callback(hObject, eventdata, handles)
status = 0;
devices = instrfind;
fclose(devices);
close all;

% --- Executes on button press in button_clear.
function button_clear_Callback(hObject, eventdata, handles)
subplot(1,1,1,'Position',[0.2 0.05 0.75 0.8],'FontSize',1); % Clear the plot
box on
cla

% --- Executes on button press in checkbox_log.
function checkbox_log_Callback(hObject, eventdata, handles)

% --- Executes on button press in checkbox_def_calibration.
function checkbox_def_calibration_Callback(hObject, eventdata, handles)


% --- Executes on button press in button_diagnostics.
function button_diagnostics_Callback(hObject, eventdata, handles)
global sPort Cycles Scale P1 P2 P3;
Duty_Cycle = [];
Opto_Counter = [];
Cycles = 15;
subplot(1,1,1,'Position',[0.2 0.06 0.75 0.8],'FontSize',1);
axis([0 80 0 120]);
box on;
set(handles.text_message,'String','Running Diagnostics! Please Wait...')
drawnow;
calibration_type = get(handles.dropdown_calibration,'Value')
disp('Synchronized');
cla;
for Duty=100:10:200   % Run motor at full duty to achieve highest rpm
    if(Duty>100)
        Duty = 100;
    end
    fprintf(sPort,'%d\n',Duty);
    fprintf(sPort,'%d\n',Cycles);
    Opto_Count = fscanf(sPort,'%d\n')
    Opto_Counter = [Opto_Counter Opto_Count];
end
pause(5)
set(handles.text_message,'String','Diagnostics Completed!')
drawnow;
if(max(Opto_Counter)>60)   % If full rpm is not achieved
    text(20,70,['System State: HEALTHY'],'FontSize',14);
else
    text(20,70,['System State: UNHEALTHY'],'FontSize',14);
end
text(20,80,['Diagnostics Results'],'FontSize',14);
text(20,60,['Max RPM Registered = ' num2str(max(Opto_Counter)*Scale)],...
    'FontSize',14); % Display maximum rpm
text(20,50,['Max Optical Encoder Count Registered = ' num2str(max(Opto_Counter))],...
    'FontSize',14); % Display maximum encoder count
fprintf(sPort,'%d\n',0);
fprintf(sPort,'%d\n',0);