/****************************************************************************
 Module
  ServoModule.c

 Revision
   1.0.1

 Description
   This is a module for staging all the servo commands for ME218B Project
   Team 3.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 03/01/19       bibit   incorporated file into dev branch with Gameplay
 02/27/19       robbie  created file
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ServoModule.h"
#include "PWM_Module.h"
#include "GameplayHSM.h"

#include "inc/hw_pwm.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_nvic.h"
#include "inc/hw_ssi.h"
#include "inc/hw_Timer.h"

#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/pwm.h"

/*----------------------------- Module Defines ----------------------------*/

//See defines for PWM Bits in header file
#define TrashChuteClosedDuty 7
#define TrashChuteOpenDuty 3
#define RecycleChuteClosedDuty 9
#define RecycleChuteOpenDuty 6
#define RecycleSortDuty 6
#define SortResetDuty 9
#define TrashSortDuty 13
#define USADuty 11
#define FlagResetDuty 7
#define MexicoDuty 3

#define ServoFreq 50  //Hz

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine.They should be functions
   relevant to the behavior of this state machine
*/

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well.
// type of state variable should match htat of enum in header file

// with the introduction of Gen2, we need a module level Priority var as well

/*------------------------------ Module Code ------------------------------*/

void SetupAllServoFrequencies(void)
{
  //Enable PWM lines

  HWREG(PWM0_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM2EN | PWM_ENABLE_PWM3EN);
  HWREG(PWM1_BASE + PWM_O_ENABLE) |= (PWM_ENABLE_PWM2EN | PWM_ENABLE_PWM3EN);
  // set all servo frequencies to same ServoFreq
  PWM_SetFrequency(ServoFreq, AllServos);
}

void SetServoStartingPositions(void)
{
  // start the sort servo in neutral state
  ResetSorter();

  // keep the recyling and trash chutes closed
  CloseTrashChute();
  CloseRecycleChute();

  // send flag to neutral position
  ResetFlags();
}

void CloseTrashChute(void)
{
  //SG90 Servo
  //Set duty to close chute
  PWM_SetDuty(TrashChuteClosedDuty, TrashServo);
}

void OpenTrashChute(void)
{
  //SG90 Servo
  //Set duty to open chute
  PWM_SetDuty(TrashChuteOpenDuty, TrashServo);
}

void OpenRecycleChute(void)
{
  //SG90 Servo
  //Set duty to open chute
  PWM_SetDuty(RecycleChuteOpenDuty, RecycleServo);
}

void CloseRecycleChute(void)
{
  //SG90 Servo
  //Set duty to close chute
  PWM_SetDuty(RecycleChuteClosedDuty, RecycleServo);
}

void SortTrash(void)
{
  //HK15131
  //Set duty to move sorter to trash bin
  PWM_SetDuty(TrashSortDuty, SortServo);
}

void SortRecycle(void)
{
  //HK15131
  //Set duty to move sorter to recycle bin
  PWM_SetDuty(RecycleSortDuty, SortServo);
}

void ResetSorter(void)
{
  //HK15131
  //Set duty to move sorter to receiving position
  PWM_SetDuty(SortResetDuty, SortServo);
}

void RaiseMexcio(void)
{
  //SG90 Servo
  //Set duty to raise Mexican Flag
  PWM_SetDuty(MexicoDuty, FlagServo);
}

void RaiseUSA(void)
{
  //SG90 Servo
  //Set duty to raise Mexican Flag
  PWM_SetDuty(USADuty, FlagServo);
}

void ResetFlags(void)
{
  //SG90 Servo
  //Set duty to raise Mexican Flag
  PWM_SetDuty(FlagResetDuty, FlagServo);
}

/***************************************************************************
 private functions
 ***************************************************************************/

/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/