/*
Web cam tower
Author: vonPongrac
----------------------------------------------------------
INPUTS
----------------------------------------------------------
P1.0 - analog signal, potentiometer
P2.0 - down
P2.1 - left
P2.2 - right
P2.3 - up
----------------------------------------------------------
OUTPUTS
----------------------------------------------------------
P1.3 - servo 1CH, vertical
P1.4 - servo 2CH, horizontal
P1.6 - LED
----------------------------------------------------------

june 2013
IAR Embedded Workbench IDE
*/

#include "io430.h"

#define MIN 110  // minimal pulse length, 0°
#define MAX 647  // maximal pulse length, 180°
#define koraki 180  // maksimum number of steps

unsigned int kanal[2]={BIT3,BIT4}; // channels for servos
unsigned int STEP=(MAX-MIN)/koraki; // pulze length of one step

void delayms(unsigned int tms); // delay function, 1 ms resolution
int readADC(); // reading analog value
void servo(unsigned int korak1,unsigned int korak2); // generating servo signal

void main( void )
{
  // position vector
  unsigned int pozicija[koraki+1]; 
  int i=0;
  for(i=0;i<koraki+1;i++)
  {
    pozicija[i]=MIN+i*STEP;
  }
  // Stop Watch Dog Timer
  WDTCTL = WDTPW + WDTHOLD;
    // internal oscilator
  BCSCTL1=CALBC1_1MHZ;    // frequency to 1MHZ
  DCOCTL=CALDCO_1MHZ;     // set DCMx and MODx (DCO step and mode) for 1MHz
     // Initialisation
  // Outputs
  P1SEL=0x01;    // P1.0 is used as ADC
  P1DIR|=0x58;   // P1.3, P1.4, P1.6 as outputs
  P1DIR&=0x58;   // unused pins as inputs
  P1OUT=0x00;    // pull-down resistors on unused pins
  P1REN=~0x58;   // set internal resistors on unused pins (ON)
  // Vhodi
  P2SEL=0x00;   // set initial value (not every ports initial value is 0x00)
  P2DIR&=~0xFF; // inputs
  P2OUT=0x0F;   // pull-up on used pins, pull-down on unused
  P2REN=0xFF;   // set internal resistors (ON)
  // ADC
  ADC10AE0=0x01; // turn ON ADC on P1.0
  ADC10CTL0=0; //Reset ADC10CTL0 (ENC=0)
  ADC10CTL1=ADC10SSEL_3; //Clock for ADC10 is SMCLK
  ADC10CTL0=ADC10SHT_3 + SREF_0 + ADC10ON; // Sample and Hold is 64 cycles, 
                                           // ADC10ON (enable the core), Vref+=Vcc, Vref-=GND
  ADC10CTL1&=~(INCH_7);  // clear selected channels
  
  int mer=0; // analog value in quantz
  int smer1=16; // initial position servo No.1
  int smer2=103; // initial position servo No.2
  
  P1OUT&=~0x58; // clear outputs
  servo(pozicija[smer1],pozicija[smer2]); // initial position
  // Loop
  while(1)
  {
    mer=readADC(); // read analog value
    if(~P2IN&0x02) // LEFT
    {
      smer2++;
      if(smer2>180)smer2=180;  // maximal value is 180 (maxsimal number of steps)
      P1OUT|=0x40;  // LED ON
    }else if(~P2IN&0x01) // DOWN
    {
      smer1--;
      if(smer1<0)smer1=0;  // minimal value is 0
      P1OUT|=0x40;  // LED ON
    }else if(~P2IN&0x04)  // RIGHT
    {
      smer2--;
      if(smer2<0)smer2=0;
      P1OUT|=0x40;  // LED ON
    }else if(~P2IN&0x08) // UP
    {
      smer1++;
      if(smer1>180)smer1=180;
      P1OUT|=0x40;  // LED ON
    }else
    {
      P1OUT&=~0x40; // LED OFF
    }
    servo(pozicija[smer1],pozicija[smer2]); // set new position
    delayms(mer>>3); // delay in ms
    
  }
}

void delayms(unsigned int tms)
{
  for(unsigned int i=0;i<tms;i++)
  {
     __delay_cycles(1000); // 1 cycle is 1 us, 1000 cycles is 1 ms
  }
}

int readADC()
{
  ADC10CTL1|=INCH_0; // set channel
  ADC10CTL0|=ENC+ADC10SC;  // enable and start converting
  while(!(ADC10CTL0&ADC10IFG));  // wait until end of convertion flag is on
  ADC10CTL0&=~ADC10IFG;  // clear flag
  return ADC10MEM;  // return value
}

void servo(unsigned int korak1,unsigned int korak2)
{
  P1OUT&=~(kanal[0]+kanal[1]);
  
  P1OUT|=kanal[0];
  for(int i=0;i<korak1;i++)
    __delay_cycles(1);
  P1OUT&=~kanal[0];
  delayms(20);
  P1OUT|=kanal[1];
  for(int i=0;i<korak2;i++)
    __delay_cycles(1);
  P1OUT&=~kanal[1];
  delayms(20);
}