/****************************************************************************

Module
ServoMotor.c
Revision
1.0.1
Description
This is a template file for implementing a simple service under the
Gen2 Events and Services Framework.
Notes
History
When Who
--------------------
****************************************************************************/

/*----------------------------- 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 <stdio.h>
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_Port.h"
#include "ES_DeferRecall.h"
#include "ES_Timers.h"
#include "termio.h"
#include "BITDEFS.h" // standard bit definitions to make things more readable

// the headers to access the GPIO subsystem

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

// the headers to access the TivaWare Library

#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"

// headers from service files

#include "ServoMotor.h"

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

//servo Motor Channels:
//PB4 : Servo1
//PB5 : Servo2

#define Forward 0
#define Backward 1
#define CWTurn 2
#define CCWTurn 3
#define LeftMotor 0
#define RightMotor 1

//#define ALL_BITS (0xff<<2)

// 40,000 ticks per mS assumes a 40Mhz clock, we will use SysClk/32 for PWM
#define PWMTicksPerMS 40000/32
// set 50 Hz frequency so 20mS period
#define PeriodInMS 20 //set the servo motor period as 20ms
// program generator A to go to 1 at rising comare A, 0 on falling compare A
#define GenA_Normal (PWM_1_GENA_ACTCMPAU_ONE | PWM_1_GENA_ACTCMPAD_ZERO )
// program generator B to go to 1 at rising comare B, 0 on falling compare B
#define GenB_Normal (PWM_1_GENB_ACTCMPBU_ONE | PWM_1_GENB_ACTCMPBD_ZERO )
// for convenience to write data to registers
#define BitsPerNibble 4

// timer length constants

#define TicksPerMS 40000
#define TEN_MSEC 10
#define ONE_SEC 1000

#define LoadPWMPow 11
#define FirePWM 9

//load: 7 latch, 11 power

//fire: 8 latch
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/

// with the introduction of Gen2, we need a module level Priority variable

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

#define defaultLat 7
#define defaultPow 4
void DefaultBall(void){
//put servo motors into default position by setting default PWMs: defaultLat and
defaultPow to the two motors
HWREG( PWM0_BASE+PWM_O_1_CMPA) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
defaultLat)/100;
HWREG( PWM0_BASE+PWM_O_1_CMPB) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
defaultPow)/100;
}

#define LoadPWMLat 7 //PWM 3-13, PWM increases, rotate CCW

void LoadBall(void){
//put latch servo motor into LoadBall posiotn by setting the LoadPWMLat to it
HWREG( PWM0_BASE+PWM_O_1_CMPA) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
LoadPWMLat)/100;
}

#define ChargePWMPow 12
void ChargeBall(void){
//put power servo motor to ChargeBall position by setting ChargePWMPow to it
HWREG( PWM0_BASE+PWM_O_1_CMPB) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
ChargePWMPow)/100;
}

#define FirePWMLat 9
void FireBall(void){
//put latch servo motor to FireBall position by setting FirePWM to it
HWREG( PWM0_BASE+PWM_O_1_CMPA) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
FirePWM)/100;
}

#define ResetPWMPow 3
void ResetBall(void){
//put power servo motor to ResetBall position by setting ResetPWMPow to it
 HWREG( PWM0_BASE+PWM_O_1_CMPB) = (HWREG(PWM0_BASE+PWM_O_1_LOAD)) * (100 -
ResetPWMPow)/100;
}

//servo motor PWM are PB4: M0PWM2 (M0PWM1GenA) & PB5:M0PWM3 (M0PWM1GenB)
void InitServo(void){
// PB4(M0PWM2) is used to generate PWM (up-down counting)
// start by enabling the clock to the PWM Module (PWM0)
HWREG(SYSCTL_RCGCPWM) |= SYSCTL_RCGCPWM_R0;

// enable the clock to Port B

HWREG(SYSCTL_RCGCGPIO) |= BIT1HI;

// Select the PWM clock as System Clock/32

HWREG(SYSCTL_RCC) = (HWREG(SYSCTL_RCC) & ~SYSCTL_RCC_PWMDIV_M) |
(SYSCTL_RCC_USEPWMDIV | SYSCTL_RCC_PWMDIV_32);

// make sure that the PWM module clock has gotten going
while ((HWREG(SYSCTL_PRPWM) & SYSCTL_PRPWM_R0) != SYSCTL_PRPWM_R0)
;

// disable the PWM while initializing

HWREG( PWM0_BASE+PWM_O_1_CTL ) = 0;

// program generators to go to 1 at rising compare A/B, 0 on falling compare A/B

// GenA_Normal = (PWM_0_GENA_ACTCMPAU_ONE | PWM_0_GENA_ACTCMPAD_ZERO )
HWREG( PWM0_BASE+PWM_O_1_GENA) = GenA_Normal;
// GenB_Normal = (PWM_0_GENB_ACTCMPBU_ONE | PWM_0_GENB_ACTCMPBD_ZERO )
HWREG( PWM0_BASE+PWM_O_1_GENB) = GenB_Normal; //we don't really need to use GenB

// Set the PWM period. Since we are counting both up & down, we initialize
// the load register to 1/2 the desired total period. We will also program
// the match compare registers to 1/2 the desired high time
HWREG( PWM0_BASE+PWM_O_1_LOAD) = ((PeriodInMS * PWMTicksPerMS))>>1; // Period
20ms

// Set the initial Duty cycle on A to 50% by programming the compare value
// to 1/2 the period to count up (or down). Technically, the value to program
// should be Period/2 - DesiredHighTime/2, but since the desired high time is
1/2
// the period, we can skip the subtract
HWREG( PWM0_BASE+PWM_O_1_CMPA) = HWREG( PWM0_BASE+PWM_O_1_LOAD)>>1; // init
servo to have 50% duty cycle, possibly need to tune

// Set the initial Duty cycle on B to 0%

HWREG( PWM0_BASE+PWM_O_1_CMPB) = HWREG( PWM0_BASE+PWM_O_0_LOAD)>>1; //Right
Motor Duty cycle 50%

// enable the PWM outputs

HWREG( PWM0_BASE+PWM_O_ENABLE) |= (PWM_ENABLE_PWM2EN | PWM_ENABLE_PWM3EN);

// now configure the Port B pins to be PWM outputs

// start by selecting the alternate function for PB4
HWREG(GPIO_PORTB_BASE+GPIO_O_AFSEL) |= (BIT4HI|BIT5HI);

// now choose to map PWM to those pins, this is a mux value of 4 that we
// want to use for specifying the function on bit4 and bit5
HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) =
(HWREG(GPIO_PORTB_BASE+GPIO_O_PCTL) & 0xff00ffff) +
(4<<(4*BitsPerNibble)) + (4<<(5*BitsPerNibble));

// Enable pins 4 on Port B for digital I/O

 HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (BIT4HI|BIT5HI);

// make pins 4 on Port B into outputs

HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (BIT4HI|BIT5HI);

// set the up/down count mode, enable the PWM generator and make
// both generator updates locally synchronized to zero count
HWREG(PWM0_BASE+ PWM_O_1_CTL) = (PWM_1_CTL_MODE | PWM_1_CTL_ENABLE |
PWM_1_CTL_GENAUPD_LS |
PWM_1_CTL_GENBUPD_LS);
}

/*------------------------------- Footnotes -------------------------------*/

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