/********************  rotary.c -- brent@mbari.org  ***************************
* $Source:
*	Copyright (C) 2003 MBARI
*	MBARI Proprietary Information.  All rights reserved.
* $Id:
*
*  feasibility test for bang-bang control of rotary valve actuator
*
*******************************************************************************/

#include <msp430x16x.h>

#define enable	   0x2
#define clockwise  0x1
#define brake	   0x4
#define PWMperiod  100		//PWM period is 100*MCLK

void Init_Sys(void);                             // function prototype

void msleep (unsigned msecs)
{
  while (msecs > 0) {
    volatile unsigned u = 1000;
    while (--u);
    --msecs;
  }
}


void rotate (int force)
/*
	rotate the shaft with given force
	force is -100..100
	negative forces are counterclockwise
*/
{
	if (force > 0) {
	  P4OUT = enable | clockwise;
	  TBCCR1 = force;
	  TBCCTL1 = OUTMOD_7;	  //PWM set at 0, then reset at limit
	}else if (force < 0) {
	  P4OUT = enable;
	  TBCCR1 = -force;
	  TBCCTL1 = OUTMOD_7;
	}else{
	  P4OUT = brake;
 	  TBCCTL1 = OUTMOD_1;	  // PWM output remains set
  	}	  
}

void main(void)
{
  Init_Sys();                                    // initialize system
  _EINT();                                       // enable interrupts

  for (;;) {
  	msleep (1000);
  	rotate (10);  //10% clockwise
  	msleep (5000);
  	rotate (-20);  //-20% counterclockwise
	msleep (2500);
	rotate (0);	  //stop
  }
}


//-----------------------------------------------------------------------------
// setup system and peripherals
//-----------------------------------------------------------------------------
void Init_Sys(void)
{
  WDTCTL = WDTPW | WDTHOLD;                      // stop watchdog timer
  BCSCTL1 = XT2OFF | RSEL2 | RSEL1;              // DCO needs to be >= 1Mhz for 100kHz I2C
  
  // D169 specific initialisation
  P1OUT = 0x10;                                  // disable audio output stage
  P1DIR = 0x3f;                                  // P1.6/P1.7 inp, all other outp
  P2OUT = 0x00;                                  // clear P2 output register
  P2DIR = 0xff;                                  // unused pins as outp  
  P3SEL = 0x0a;                                  // assign I2C pins to module
  P3OUT = 0x00;                                  // clear P3 output register
  P3DIR = 0xff;                                  // unused pins as outp  
  P4OUT = 0x00;                                  // clear P4 output register
  P4DIR = 0xff;                                  // unused pins as outp  
  P5OUT = 0x00;                                  // clear P5 output register
  P5DIR = 0xff;                                  // unused pins as outp  
  P6OUT = 0x00;                                  // clear P6 output register
  P6DIR = 0xfe;                                  // unused pins as outp  

  TBCCR0 = PWMperiod-1;		// Set the PWM counts per period (1/frequency)
  TBCTL = TBSSEL_2 | MC_1;  // start 16-bit up-counter driven directly by SMCLK
  P4SEL = enable;			// the PWM enable bit is driven by the comparator
  
  P1SEL = 0x10;		//drive SMCLK out P1.4 to U3 pin 1
}