#include <stdio.h>
#include <ME218_C32.h>
#include <ADS12.h>
#include <PWMS12.h>
#include <timers12.h>

/******Module Variables******/
//#define TEST
#define SPEED_DUTY_CYCLE_RATIO          11
#define TARGET_PWM_CHANNEL        PWMS12_CHAN0       //Port T0 channel for target motor PWM 
#define SWAG_PWM_CHANNEL          PWMS12_CHAN1       //Port T1 for SWAG motor  
#define LOWEST_SPEED_DUTY_CYCLE         50
#define MAXIMUM_SPEED_DUTY_CYCLE        60
#define SWAG_DUTY_CYCLE                 100
#define ONE_SECOND                      1000         //ms
#define SWAG_MOTOR_PULSETIME            250

/******Module Functions******/
void InitMotors(void);
void ChangeMotorSpeed(unsigned char speed_port);
void DispenseSWAG(unsigned int timer_number);

/*****************************************
Function:    InitMotors(void)
Description: Performs any initialization necessary for all motors to run.
Inputs:      None
Output:      None
Revised:     CH        11/8/07
******************************************/
void InitMotors(void)
{
     PWMS12_Init();                             //Initialize PWM subsystem  
     
}

/****************************************
Function:    ChangeMotorSpeed(unsigned char speed_port)
Description: Based on an analog input  (i.e. potentiometer), changes speed
             of a motor specified by the port that governs its speed
Input:			 unsigned char, which designates which port to read input from
Output:			 None
Revised:     CH		    11/8/07       Note: Change input parameter to 
                                    designate which motor instead of which port to read from?
                               			Add another input parameter to specify which PWM channel?
******************************************/ 
void ChangeMotorSpeed(unsigned char speed_port)
{
	    unsigned short value;
		unsigned short dutycycle;
		signed char setPWMresult;
  		value = ADS12_ReadADPin( speed_port );	            //Read AD pin of speed port
  		dutycycle = value/SPEED_DUTY_CYCLE_RATIO*(MAXIMUM_SPEED_DUTY_CYCLE - LOWEST_SPEED_DUTY_CYCLE)/100 + LOWEST_SPEED_DUTY_CYCLE;	//Calculate duty cycle for PWM  
  		printf("\r\n Duty cycle is %d \r\n", dutycycle);
  		setPWMresult = PWMS12_SetDuty(  (unsigned char) dutycycle, TARGET_PWM_CHANNEL );  //set PWM 
  		if (setPWMresult ==  PWMS12_ERR) 
  		{
  		  	 printf("\r\n Error setting PWM to motor for port %d \r\n", speed_port);
  		}
  		
}

/*****************************************
Function: DispenseSWAG(unsigned int timer_number)
Description:  Pulses the SWAG motor for a set amount of time
Input:   unsigned int timer_number
Output:   None
Revised:  CH        11/16/07
******************************************/
void DispenseSWAG(unsigned int timer_number) 
{
  	 TMRS12_InitTimer(timer_number,SWAG_MOTOR_PULSETIME);
  	 while (TMRS12_IsTimerExpired(timer_number) == 0) 
  	 {
  	    PWMS12_SetDuty(SWAG_DUTY_CYCLE, SWAG_PWM_CHANNEL);
  	 }
  	 PWMS12_SetDuty(0, SWAG_PWM_CHANNEL);
}

/*****************************************
Function:    EndMotors(void)
Description: Stops the motor at the end of the game.
Inputs:      None
Output:      None
Revised:     MP        11/15/07
******************************************/
void EndMotors(void)
{
     PWMS12_SetDuty(0,TARGET_PWM_CHANNEL);
     
}



/*****************************************
Function: main() for test purposes
Revised:    CH      11/8/07
*******************************************/
#ifdef TEST
#define DIFFICULTY_DIAL_PORT 6
void main( void )
{
     ADS12ReturnTyp ADinit_result;
     ADinit_result = ADS12_Init("IIIIIIIA");   //Set AD0 to read target speed input
     if (ADinit_result == ADS12_Err) 
     {
      printf("\r\n Error in initializing AD \r\n");
     } else
     {
    	 InitMotors();
   	  /* while(1)
    	 {
    	      unsigned short value;
    	      unsigned short dutycycle;
    	      value = (unsigned short) ADS12_ReadADPin( TARGET_SPEED_PORT );
    	      printf("\r\n Pin AD0 reads %d \r\n", value);
						dutycycle = value/SPEED_DUTY_CYCLE_RATIO;
						printf("\r\n Calculated duty cycle is %d \r\n", dutycycle);
    	 }	 */
    	 while(1)
    	 {
    	  ChangeMotorSpeed( TARGET_SPEED_PORT );
    	 }
     }
}


#endif