/*******************************************************************************
  MPLAB Harmony Application Source File
  
  Company:
    Microchip Technology Inc.
  
  File Name:
    app.c

  Summary:
    This file contains the source code for the MPLAB Harmony application.

  Description:
    This file contains the source code for the MPLAB Harmony application.  It 
    implements the logic of the application's state machine and it may call 
    API routines of other MPLAB Harmony modules in the system, such as drivers,
    system services, and middleware.  However, it does not call any of the
    system interfaces (such as the "Initialize" and "Tasks" functions) of any of
    the modules in the system or make any assumptions about when those functions
    are called.  That is the responsibility of the configuration-specific system
    files.
 *******************************************************************************/

// DOM-IGNORE-BEGIN
/*******************************************************************************
Copyright (c) 2013-2014 released Microchip Technology Inc.  All rights reserved.

Microchip licenses to you the right to use, modify, copy and distribute
Software only when embedded on a Microchip microcontroller or digital signal
controller that is integrated into your product or third party product
(pursuant to the sublicense terms in the accompanying license agreement).

You should refer to the license agreement accompanying this Software for
additional information regarding your rights and obligations.

SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF
MERCHANTABILITY, TITLE, NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE.
IN NO EVENT SHALL MICROCHIP OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER
CONTRACT, NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR
OTHER LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE OR
CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT OF
SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
(INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
 *******************************************************************************/
// DOM-IGNORE-END


// *****************************************************************************
// *****************************************************************************
// Section: Included Files 
// *****************************************************************************
// *****************************************************************************

#include "app.h"

// *****************************************************************************
// *****************************************************************************
// Section: Global Data Definitions
// *****************************************************************************
// *****************************************************************************

// *****************************************************************************
/* Application Data

  Summary:
    Holds application data

  Description:
    This structure holds the application's data.

  Remarks:
    This structure should be initialized by the APP_Initialize function.
    
    Application strings and buffers are be defined outside this structure.
*/

APP_DATA appData;


uint32_t currentRef[SAMPLES_NBR];
uint32_t pwmIn[SAMPLES_NBR];
uint32_t measureOut[SAMPLES_NBR];

const char tutoMessage[] = "\r\nThis application identifies the motor by applying a series of steps. It has 3 states, each identified by a LED.\r\n\r\n"
"***READY STATE - LED 1***\r\nIn this state, the application is ready to start the identification routine. You should start data capture in the PC terminal and press SWITCH 1 to start the identification.\r\n\r\n"
"***BUSY STATE - LED 2***\r\nIn this state, the identification and data display are being performed. Don't interact with the terminal or the MCU when LED 2 is on.\r\n\r\n"
"***DONE STATE - LED 3***\r\nIn this state, the identification has already been performed and displayed. You should close de data capture in the PC terminal and press SWITCH 2 if you want to restart the application.\r\n\r\n";

static const PORTS_CHANNEL mdriver_port_channel_map[] =
{
    PORT_CHANNEL_K,
    PORT_CHANNEL_J
};

static const PORTS_BIT_POS mdriver_port_bit_pos_map[] =
{
    PORTS_BIT_POS_6,
    PORTS_BIT_POS_3
};

// Control variables
float error, lastError, controllerOutput, lastControllerOutput;

// *****************************************************************************
// *****************************************************************************
// Section: Application Callback Functions
// *****************************************************************************
// *****************************************************************************

/* TODO:  Add any necessary callback functions.
*/


// *****************************************************************************
// *****************************************************************************
// Section: Application Local Functions
// *****************************************************************************
// *****************************************************************************


/* TODO:  Add any necessary local functions.
*/

float controllerAlgorithm( void)
{
    error = (float) currentRef[appData.samplesRead] - measureOut[appData.samplesRead];
    controllerOutput = 5*error - 4.091*lastError + lastControllerOutput;
    lastError = error;
    lastControllerOutput = controllerOutput;
    return controllerOutput;
}
// *****************************************************************************
// *****************************************************************************
// Section: Application Initialization and State Machine Functions
// *****************************************************************************
// *****************************************************************************

/*******************************************************************************
  Function:
    void APP_Initialize ( void )

  Remarks:
    See prototype in app.h.
 */

void APP_Initialize ( void )
{
    /* Place the App state machine in its initial state. */
    appData.state = APP_STATE_INIT;
    
    
    /* TODO: Initialize your application's state machine and other
     * parameters.
     * 
     */
    
    uint32_t vectorCounter;
    for (vectorCounter = 0; vectorCounter < SAMPLES_NBR; vectorCounter ++)
    {
//        uint32_t stepDuration = 500;
//        currentRef[vectorCounter] =(uint32_t) 2200 + 50 * ((vectorCounter / stepDuration));
        
        if(vectorCounter <1000)
            currentRef[vectorCounter] = 2300;
        else if(vectorCounter <2000)
            currentRef[vectorCounter] = 2500;
        else if(vectorCounter <3000)
            currentRef[vectorCounter] = 2100;
        else if(vectorCounter <4000)
            currentRef[vectorCounter] = 1800;
        else
            currentRef[vectorCounter] = 1600;
        
//        if(vectorCounter <1000)
//            currentRef[vectorCounter] = 2300;
//        else 
//            currentRef[vectorCounter] = 2500;
        
        measureOut[vectorCounter] = 0;
    }
    
    lastError = 0.0;
    lastControllerOutput = 0.0;
    
    DRV_TMR0_Start();
    DRV_OC0_Start();
    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INA],
            mdriver_port_bit_pos_map[MDRIVER_PIN_INA], 0);
    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INB],
            mdriver_port_bit_pos_map[MDRIVER_PIN_INB], 1);
    PLIB_OC_PulseWidth16BitSet(OC_ID_3, 0);
    
    appData.dataReady = false;
    appData.samplesRead = 0;
    
    uartPutS("\r\n**************************************\r\n");
    uartPutS("***Motor Identification Application***");
    uartPutS("\r\n**************************************\r\n");
    uartPutS(tutoMessage);
    
    BSP_LEDOn(BSP_LED_1);
}


/******************************************************************************
  Function:
    void APP_Tasks ( void )

  Remarks:
    See prototype in app.h.
 */

void APP_Tasks ( void )
{

    /* Check the application's current state. */
    switch ( appData.state )
    {
        /* Application's initial state. */
        case APP_STATE_INIT:
        {
            if (BSP_SwitchStateGet(BSP_SWITCH_1) ==  BSP_SWITCH_STATE_PRESSED)
            {
                BSP_LEDOff(BSP_LED_1);
                BSP_LEDOn(BSP_LED_2);
                DRV_ADC0_Open(); 
                DRV_TMR1_Start();
                PLIB_OC_PulseWidth16BitSet(OC_ID_3, pwmIn[appData.samplesRead]);
                appData.state = APP_STATE_CONTROL;
            }
            
            break;
        }

        case APP_STATE_CONTROL:
        {
            if (appData.dataReady == true)
            {
                measureOut[appData.samplesRead] = appData.sampleResult;
                float controllerAction = controllerAlgorithm();
                if (controllerAction >= 0)
                {
                    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INA],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INA], 0);
                    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INB],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INB], 1);
                    pwmIn[appData.samplesRead+1] = (uint32_t) controllerAction;
                    if (pwmIn[appData.samplesRead+1] > 4500)
                        pwmIn[appData.samplesRead+1] = 4500;
                    else if (pwmIn[appData.samplesRead+1] < 500)
                        pwmIn[appData.samplesRead+1] = 500;
                }
                else
                {
                    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INA],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INA], 1);
                    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INB],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INB], 0);
                    pwmIn[appData.samplesRead+1] = (uint32_t) -controllerAction;
                    if (pwmIn[appData.samplesRead+1] > 4500)
                        pwmIn[appData.samplesRead+1] = 4500;
                    else if (pwmIn[appData.samplesRead+1] < 500)
                        pwmIn[appData.samplesRead+1] = 500;
                }
                appData.samplesRead++;
                appData.dataReady = false;
            }
            if (appData.samplesRead == SAMPLES_NBR)
            {
                DRV_ADC0_Close(); 
                DRV_TMR1_Stop();
                PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INA],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INA], 0);
                    PLIB_PORTS_PinWrite(PORTS_ID_0,mdriver_port_channel_map[MDRIVER_PIN_INB],
                        mdriver_port_bit_pos_map[MDRIVER_PIN_INB], 0);
                appData.state = APP_STATE_DATA_TRANSMISSION;
                break;
            }
            PLIB_OC_PulseWidth16BitSet(OC_ID_3, pwmIn[appData.samplesRead]);

            break;
        }

        /* TODO: implement your application state machine.*/
        
        case APP_STATE_DATA_TRANSMISSION:
        {
            uint32_t vectorCounter;
            char lineToPrint[100];
            for(vectorCounter = 0; vectorCounter < appData.samplesRead; vectorCounter++)
            {
                sprintf(lineToPrint, "%u,%u,%u\r\n", currentRef[vectorCounter], pwmIn[vectorCounter], measureOut[vectorCounter]);
                uartPutS(lineToPrint);
            }
            BSP_LEDOff(BSP_LED_2);
            BSP_LEDOn(BSP_LED_3);
            appData.state = APP_STATE_DONE;
            break;   
        }
        
        case APP_STATE_DONE:
        {
            if (BSP_SwitchStateGet(BSP_SWITCH_2) == BSP_SWITCH_STATE_PRESSED)
            {
                SYS_RESET_SoftwareReset();
            }
            break;   
        }

        /* The default state should never be executed. */
        default:
        {
            /* TODO: Handle error in application's state machine. */
            break;
        }
    }
}

 

/*******************************************************************************
 End of File
 */
