/****************************************************************************
 *
 *  CMPE-118 basic servo driving for Lab 3
 *
 *  History
 *  When           Who     What/Why
 *  -------------- ---     --------
 *  1/30/09        jbb     simplified for lab exercise
 *
 *  Original BotLib created by Scott Early as part of CMPE-118 in Winter 2008
 */

#include <hidef.h>         /* common defines and macros */
#include <mc9s12c32.h>     /* derivative information */
#include <S12C32bits.h>    /* bit definitions  */

#include <stdio.h>
#include <stdlib.h> /* needed for rand() */
#include <termio.h>
#include <ads12.h>



/****************************************************************************
 * Private General Functions
 */
void Bot_SetPortBit(uchar* port, uchar bit, uchar value);


/****************************************************************************
 * PWM Functions
 *
 * PWM Channels 2 and 3 are used by the R/C servo functions
 */

void Bot_PWMInit(void);
void Bot_PWMSetPins(uchar pins);
char Bot_PWMSetClock(uchar clock, uchar prescale, uchar scale);
char Bot_PWMSetChannel(uchar channel, uchar enable, uchar period, uchar duty,
                       uchar scaler, uchar polarity, uchar cae);
char Bot_PWMSetPeriod(uchar channel, uchar period);
char Bot_PWMSetDuty(uchar channel, uchar duty);

/****************************************************************************
 * R/C Servo Control
 */

#define RCSERVO_PORT_PWM_0	2					// PWM channel for servo 0
#define RCSERVO_PORT_PWM_1	3					// PWM channel for servo 1

void RCServo_Init(void);

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


/****************************************************************************
 * General Functions
 */

void Bot_Init(void) {
	Bot_PWMInit();
	RCServo_Init();
    
	DDRT = 0xC0;
}

void Bot_SetPortBit(uchar* port, uchar bit, uchar value) {
	if (bit > 7) return;
	bit &= 1;
	*port = (*port & ~(1 << bit)) | (value << bit);
}



/****************************************************************************
 * PWM Functions
 */

void Bot_PWMInit(void) {
	// PWMCTL?
	PWME = 0x00;	// disable all PWM channels
    
	// Bot_PWMSetClock(clock, prescale, scale)
	Bot_PWMSetClock(0, 4, 10);	// motor drive 295Hz clock (pins 0, 1, 4, 5)
    
	Bot_PWMSetClock(1, 6, 14);	// RC Servo 52Hz clock (pins 2, 3)
	Bot_PWMSetPins(0x0F);				// enable pins 0, 1, 2, 3
}

void Bot_PWMSetPins(uchar pins) {
	MODRR = pins & 0x3F;   //0011 1111
}

char Bot_PWMSetClock(uchar clock, uchar prescale, uchar scale) {
	if (clock > 1)
		return(!0);
    
	clock &= 1;
	prescale &= 7;
	if (clock == 0) {
		PWMPRCLK = (PWMPRCLK & ~0x07) | prescale;
		PWMSCLA = scale;
	} else {
		PWMPRCLK = (PWMPRCLK & ~0x70) | prescale << 4;
		PWMSCLB = scale;
	}
	return 0;
}

char Bot_PWMSetChannel(uchar channel, uchar enable, uchar period, uchar duty,
                       uchar scaler, uchar polarity, uchar cae)
{
	if (channel > 5) return !0;
	enable &= 1;
	polarity &= 1;
	scaler &= 1;
    
	// disable channel
	PWME &= ~(1 << channel);
    
	// set registers
	Bot_PWMSetPeriod(channel, period);
	Bot_PWMSetDuty(channel, duty);
	PWMCLK = (PWMCLK & ~(1 << channel)) | scaler << channel;
	PWMPOL = (PWMPOL & ~(1 << channel)) | polarity << channel;
	PWMCAE = (PWMCAE & ~(1 << channel)) | cae << channel;
    
	// enable channel
	PWME |= enable << channel;
    
	return 0;
}

char Bot_PWMSetPeriod(uchar channel, uchar period) {
	if (channel > 5) return !0;
	*(&PWMPER0 + channel) = period;
	return 0;
}

char Bot_PWMSetDuty(uchar channel, uchar duty) {
	if (channel > 5) return !0;
	*(&PWMDTY0 + channel) = duty;
	return 0;
}




/****************************************************************************
 * R/C Servo Control
 */

/* This function sets the period of a PWM signal to roughly 19ms = 53Hz 

   You probably don't need to change this unless you want higher resolution-
   if you've already finished the lab!
 
 */
void RCServo_Init(void) {
	// Bot_PWMSetChannel(channel, enable, period, duty, clock, polarity, cae)
	Bot_PWMSetChannel(RCSERVO_PORT_PWM_0, 1, 254, 0, 1, 1, 0);
	return;
}


/* This function should set the duty cycle of a PWM signal to position an 
   R/C servo motor to the function's argument 'position'.  
  
   After RCSerco_Init() has been called to set the period of the signal, a PWM
   signal can be made by varying the duty cycle of this signal.  As provided,
   the period is set to 19ms by setting the PWMPERx register to 254 and 
   setting other prescalers.
   
   The desired duty cycle can be created by setting the PWMDTYx register
   through the Bot_PWMSetDuty() function.  When the PWM signal starts, a 
   counter is set to zero and the output goes high; this counter counts 
   up at a rate specified by various prescalers.  When this counter value
   matches PWMDTYx, the output goes low.  It remains low until the counter 
   matches PWMPERx (as set in RCServo_Init()), when the counter is reset 
   causing the output to go high.  
   
   This function is currently limited by the usage of 8 bit counters.  The 
   resolution of the duty cycle is limited by the value of PWMPERx: there 
   are only 254 gradations to which the duty cycle can be set, even fewer of
   which are useful when driving a servo.
   
   It is possible to concatenate the 8 bit registers into a 16 bit register
   for increased resolution.  This is left as an excercise for the 118er.
   See the MC9S12C Reference Manual, section 12.4.2.5, for details. .        
*/   
void RCServo_Set(char position) {  
/* Modify this function to set a duty cycle based on the desired position.
     1ms ~= -60 degrees,
   1.5ms ~=   0 degrees, 
     2ms ~=  60 degrees
 */

	uchar duty;		// raw PWM duty counter
  
  // Please, don't push the servos past this. 
	if (position < -60 || position > 60) return;
  
  // Modify this to create a duty cycle based on the desired position.  
	duty = 20; // (20/254)*19ms = 1.5ms
  
  //printf("Position: %03d, Duty: %02d\r\n", position, duty);
    
	Bot_PWMSetDuty(RCSERVO_PORT_PWM_0, duty);
}

/*------------------------------- Footnotes -------------------------------*/
#ifdef SERVO_TEST

#include <stdio.h>
#include <termio.h>
#include "servobasic.h"


void main(void) {

  char position = 0;

  TERMIO_Init();
  
  printf("Starting up...\n\r");

  Bot_Init();
  printf("Botlib initialized...\n\r");
  
  RCServo_Set(position);
  
  printf("Servo set to 0 degrees...\n\r");
  
  /* Changes two degrees with every key hit... 
                                           ...once you finish RCServo_Set()
   */  
  while(TERMIO_GetChar()) {
    position += 2;
    
    if ( position > 60 ) {
      position = -60;
    }
    RCServo_Set(position);
    printf("now at %d degrees\n\r", (int)position);
  }

}
#endif