void samrt_servo_init(void)
{
  pinMode(SMART_SERVO_LED_R,GPIO_PMD_OUTPUT);
  pinMode(SMART_SERVO_LED_G,GPIO_PMD_OUTPUT);
  pinMode(SMART_SERVO_LED_B,GPIO_PMD_OUTPUT);   
  pinMode(SMART_SERVO_SLEEP,GPIO_PMD_OUTPUT);

  digitalWrite(SMART_SERVO_LED_R,0);
  digitalWrite(SMART_SERVO_LED_G,1);
  digitalWrite(SMART_SERVO_LED_B,0);
  digitalWrite(SMART_SERVO_SLEEP,1);
	
//  pinMode(SMART_SERVO_PW1,GPIO_PMD_OUTPUT);
//  pinMode(SMART_SERVO_PW2,GPIO_PMD_OUTPUT);
//  digitalWrite(SMART_SERVO_PW1,0);
//  digitalWrite(SMART_SERVO_PW2,0);
  pwm_init(SMART_SERVO_PW1,500);
  pwm_init(SMART_SERVO_PW2,500);

//  pinMode(SMART_SERVO_PW1,GPIO_PMD_OUTPUT);
//  pinMode(SMART_SERVO_PW2,GPIO_PMD_OUTPUT);
//  digitalWrite(SMART_SERVO_PW1,1);
//  digitalWrite(SMART_SERVO_PW2,1);
	
  smart_servo_cur_pos = adc_get_position_value();
  smart_servo_pre_pos = smart_servo_cur_pos;
	smart_servo_target_pos = smart_servo_cur_pos;
  smart_servo_speed = SMART_SERVO_PER_SPPED_MAX;
}

int main (void)
{
    SystemInit();

    pwm_init(PWM_CH0);
    pwm_io_config();
    //P2.7 will output pwm wave with period for 1000us and pulse for 400us
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(400, PWM_PSCAL_DIV));
    pwm_enable(PWM_CH0, MASK_ENABLE);
    
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(800, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(1000, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(0, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(400, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(800, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(1000, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(0, PWM_PSCAL_DIV));
    pwm_config(PWM_CH0, PWM_PSCAL_DIV, PWM_COUNT_US(1000, PWM_PSCAL_DIV), PWM_COUNT_US(800, PWM_PSCAL_DIV));
    pwm_enable(PWM_CH0, MASK_DISABLE);
    pwm_io_dis_config();

    pwm_init(PWM_CH1);
    pwm_io_config();
    //P2.6 will output pwm wave with period for 1500us and pulse for 1000us
    pwm_config(PWM_CH1, 119, PWM_COUNT_US(1500, 119), PWM_COUNT_US(1000, 119));
    pwm_enable(PWM_CH1, MASK_ENABLE);
    pwm_io_dis_config();

    while (1)                                /* Loop forever */
    {

    }
}

void set_dvfs(unsigned int domain, unsigned int index)
{
	int cur, to;
	static int init_flag = 0;
	if (!init_flag) {
		pwm_init(pwm_f);
		pwm_init(pwm_ao_a);
		init_flag = 1;
	}
	cur = dvfs_get_voltage(domain);
	for (to = 0; to < ARRAY_SIZE(pwm_voltage_table); to++) {
		if (domain == 0) {
			if (pwm_voltage_table[to][1] >= cpu_dvfs_tbl[index].volt_mv) {
				break;
			}
		}
		if (domain == 1) {
			if (pwm_voltage_table[to][1] >= vlittle_dvfs_tbl[index].volt_mv) {
				break;
			}
		}
	}
	if (to >= ARRAY_SIZE(pwm_voltage_table)) {
		to = ARRAY_SIZE(pwm_voltage_table) - 1;
	}
	if (cur < 0 || cur >=ARRAY_SIZE(pwm_voltage_table)) {
		if (domain == 0)
			P_AO_PWM_PWM_A = pwm_voltage_table[to][0];
		if (domain == 1)
			P_PWM_PWM_F  = pwm_voltage_table[to][0];
		_udelay(200);
		return ;
	}
	while (cur != to) {
		/*
		 * if target step is far away from current step, don't change
		 * voltage by one-step-done. You should change voltage step by
		 * step to make sure voltage output is stable
		 */
		if (cur < to) {
			if (cur < to - 3) {
				cur += 3;
			} else {
				cur = to;
			}
		} else {
			if (cur > to + 3) {
				cur -= 3;
			} else {
				cur = to;
			}
		}
		if (domain == 0)
			P_AO_PWM_PWM_A = pwm_voltage_table[cur][0];
		if (domain == 1)
			P_PWM_PWM_F  = pwm_voltage_table[to][0];
		_udelay(100);
	}
	_udelay(200);
}

/**
 * \brief Example 1 main application routine
 */
int main( void )
{
    /* The 4 PWM config structs */
    struct pwm_config pwm_cfg[4];

    /* Initialize sysclock */;
    sysclk_init();

    /* Set up all 4 PWM channels with 500 Hz frequency. We want to use TCE0
       since this is where our LEDs are connected (PE0-PE3) */
    pwm_init(&pwm_cfg[0], PWM_TCE0, PWM_CH_A, 500); /* LED0 / PE0 */
    pwm_init(&pwm_cfg[1], PWM_TCE0, PWM_CH_B, 500); /* LED1 / PE1 */
    pwm_init(&pwm_cfg[2], PWM_TCE0, PWM_CH_C, 500); /* LED2 / PE2 */
    pwm_init(&pwm_cfg[3], PWM_TCE0, PWM_CH_D, 500); /* LED3 / PE3 */

    /* Start all 4 PWM channels with different duty cycles
      (which means different LED intensity).
      Since the LEDs are active low, 0% duty cycle means constantly lit */
    pwm_start(&pwm_cfg[0], 97);
    pwm_start(&pwm_cfg[1], 85);
    pwm_start(&pwm_cfg[2], 60);
    pwm_start(&pwm_cfg[3], 0);

    while(1) {
        /* Nothing to do */
    }
}

void init(void)
{
	sys_init();
	pwm_init(&pwm0);
	pwm_init(&pwm1);
	pwm_init(&pwm2);
	pwm_init(&pwm3);
}

void power_init(int mode)
{
	pwm_init(pwm_b);
	pwm_init(pwm_d);
	serial_puts("set vcck to ");
	serial_put_dec(CONFIG_VCCK_INIT_VOLTAGE);
	serial_puts(" mv\n");
	pwm_set_voltage(pwm_b, CONFIG_VCCK_INIT_VOLTAGE);
	serial_puts("set vddee to ");
	serial_put_dec(CONFIG_VDDEE_INIT_VOLTAGE);
	serial_puts(" mv\n");
	pwm_set_voltage(pwm_d, CONFIG_VDDEE_INIT_VOLTAGE);
}

int main (void)
{
	mode_pointer = &mode_time; //assigns the mode_pointer initially to time mode
		
	pwm_init();
	timer_init();
	analog_init();
	pinchange_init();
	power_register_init();
	sei(); // global set enable interrupts
		
	while(1)
    {
		PRR &= ~(1<<PRADC); //turns on the ADC comparator
		PORTA |= (1<<MODE_SELECT_POWER); //turn on to power pot
		ADCSRA |= (1<<ADSC); // starts AtoD conversion by flipping ADC Start Conversion bit in AD Control and Status Register A
		while(ADCSRA & (1<<ADSC));  // loops while waiting for ADC to finish
		PORTA &= ~(1<<MODE_SELECT_POWER); //turn pot back off to conserve power
		PRR |= (1<<PRADC); //shuts down the ADC and comparator
		
		
		
		(*mode_pointer)();  //uses a pointer to call the function for the specific mode
		
		sleep_cpu();
		//just hang out and wait for interrupts
		
    }
	
	return 1;
}

void init_modules()
{
	gpio_init();
	an_init();
	spi_init();
	pwm_init();
}

int main(void) {
  WDTCTL = WDTPW + WDTHOLD;   // Stop WDT
  // configure the CPU clock (MCLK)
  // to run from SMCLK: DCO @ 16MHz and SMCLK = DCO
  // If no DCO calibration => stop here by loop
  if(CALBC1_16MHZ==0xff)
    while(1)
      _NOP();
  _BIC_SR(GIE);
  DCOCTL = 0;
  BCSCTL1= CALBC1_16MHZ;
  DCOCTL = CALDCO_16MHZ;
  
  clock_init();
  uart_init();
  motor_init();
  protocols_init();
  pwm_init();

  // ----------
  // **action**
  // ----------
  _BIS_SR(GIE);
  
  FOREVER {
    // read input
    input_scanner();
    // handle any pending state changes in motor
    motor_step();
    // send status reports to pi
    reporter();
  }
}

void lib_low_level_init()
{
	#ifdef _GPIO_H_
	gpio_init();
	#endif

	#ifdef _UART_H_
	uart_init(); 
	#endif

	#ifdef _TIMER_H_
	timer_init();
	#endif

	#ifdef _PWM_H_
	pwm_init();
	#endif

	#ifdef _I2C_H_
	i2c_0_init();
	#endif

	#ifdef _SPI_H_
	spi_init();
	#endif


	#ifdef _ADC_H_
	adc_init();
	#endif
}

int main(void)
{
  int16_t signe = 1;
  
  DDRG=0x3;
  PORTG=0x0;

  pwm_init();
  

  while(1)
    {
      pwm_set(NUM, 1 * signe); // not 0 to test for sign problems
      wait_ms(2* DELAY);

      pwm_set(NUM, P_MAX /2 * signe);
      wait_ms(DELAY);
      
      pwm_set(NUM, P_MAX* signe);
      wait_ms(DELAY);
      
      pwm_set(NUM, P_MAX /2 * signe);
      wait_ms(DELAY);

      signe *= -1;
    }

  return 0;
}

int main()
{
	unsigned int i;
	pwm_init();
	
	while(1);
	while(1)
	{
		for(i=0;i<=100;i++)
		{
			PWMMR1 	-= 10;
			PWMLER	= (1<<1);		// Load values to PWMMR1 
			delay(100);
		}

		for(i=0;i<=100;i++)
		{
			PWMMR1 	+= 10;
			PWMLER	= (1<<1);		// Load values to PWMMR1 
			delay(100);
		}
		
	}
	  
	
	
}

Motors::Motors(void) {
  if(pwm_init(MOT_PWM, MODE, FREQU, MAX_PWM) == 0) {
    setState(ERROR);
    return;
  }

  if(gpio_init(MOT1_DIR1, GPIO_OUT) != 0) {
    setState(ERROR);
    return;
  }

  if(gpio_init(MOT1_DIR2, GPIO_OUT) != 0) {
    setState(ERROR);
    return;
  }

  if(gpio_init(MOT2_DIR1, GPIO_OUT) != 0) {
    setState(ERROR);
    return;
  }

  if(gpio_init(MOT2_DIR2, GPIO_OUT) != 0) {
    setState(ERROR);
    return;
  }

  left().put(0);
  right().put(0);
}

/**
 * @brief Function for application main entry.
 */
int main(void)
{
    uint8_t new_duty_cycle;
    uint32_t err_code;

    lfclk_init();

    // Start APP_TIMER to generate timeouts.
    APP_TIMER_INIT(APP_TIMER_PRESCALER, OP_QUEUES_SIZE, NULL);

    /*Initialize low power PWM for all 3  channels of RGB or 3 channels of leds on pca10028*/
    pwm_init();

    while (true)
    {
        /* Duty cycle can also be changed from main context. */
        new_duty_cycle = low_power_pwm_2.period - low_power_pwm_2.duty_cycle;
        err_code = low_power_pwm_duty_set(&low_power_pwm_2, new_duty_cycle);
        APP_ERROR_CHECK(err_code);
        nrf_delay_ms(500);

        new_duty_cycle = low_power_pwm_2.period - low_power_pwm_2.duty_cycle;
        err_code = low_power_pwm_duty_set(&low_power_pwm_2, new_duty_cycle);
        APP_ERROR_CHECK(err_code);
        nrf_delay_ms(500);
    }
}

/*********************************************************************
  
  Main Program Loop

**********************************************************************/
int main()
{

  /* Initializations */
  debug_init();     /* This should be first. */
  timer_init();

  /* This should be before any GPIO activities. */
  uint32 ret_val = bcm2835_init(); 
  if ( ret_val == 0 )
  {
    DEBUG_MSG_ERROR("bcm2835_init() failed.");
  }

  pwm_init();
  pump_init();
  therm_init();
  pid_init();

  pump_start();

  /* Take temperature as input from console. */
  float setpoint;
  printf("Set your desired temperature: ");
  scanf("%f", &setpoint);
  pid_update_temp_setpoint(setpoint);

  pid_gain_params pid_gain;
  pid_gain.k_p = 1;
  pid_gain.k_d = 1;
  pid_gain.k_i = 1;
  pid_gain.k_windup = 1;
  pid_set_gain(&pid_gain);

  /* Main Program Loop */
  while (1)
  {
    pwm_run();
    therm_capture();
    pid_loop();
  }

  pump_stop();

  /* De-initializations */
  pump_deinit();
  pid_deinit();
  pwm_deinit();

  /* This should be after all GPIO activities. */
  ret_val = bcm2835_close();
  if ( ret_val == 0 )
  {
    DEBUG_MSG_ERROR("bcm2835_close() failed.");
  }
  timer_deinit();
  debug_deinit();   /* This should be last. */

  return 0;
}

/*---------------------------------------------------------------------------*/
void board_init()
{
	uint8_t int_disabled = ti_lib_int_master_disable();

	/* Turn on relevant PDs */
	wakeup_handler();

	/* Enable GPIO peripheral */
	ti_lib_prcm_peripheral_run_enable(PRCM_PERIPH_GPIO);

	/* Apply settings and wait for them to take effect */
	ti_lib_prcm_load_set();
	while (!ti_lib_prcm_load_get()) ;

	lpm_register_module(&srf_module);

	/* Re-enable interrupt if initially enabled. */
	if (!int_disabled) {
		ti_lib_int_master_enable();
	}

	/* Init of PWM */
	pwm_init();

}

int main (void) {		
	
	adc_init();
	pwm_init();
	uart_init(0);
	
	SysTick_Config(48000000/1000); // systick every 1 ms
	//             Hz      /ticks per sec
	
	while (1) {
		adc_measure(&adcval);
		counter++;
		if(counter == 1)
			adcval_prev = adcval;
		else
			adcval_prev = (adcval_prev + adcval)/2;

		if(counter > filter_limit-1)
		{
			adcval_filtered = adcval_prev;
			if(adcval_filtered < min_distance)
				pwm_systick_control(10);
			else if (adcval_filtered < 10000)
				pwm_systick_control(adcval_filtered%100);
			else
				pwm_systick_control(100);
			counter = 0;
		}
		
	}
	
	return(0);
}

/** Main initialisation. */
static void
main_init (void)
{
#ifndef HOST
    /* Disable watchdog (enabled in bootloader). */
    MCUSR &= ~(1 << WDRF);
    wdt_disable ();
#endif
    /* Serial port */
    uart0_init ();
    /* Enable interrupts */
    sei ();
    /* Main timer */
    timer_init ();
    timer_wait ();
    /* TWI communications */
    asserv_init ();
    mimot_init ();
    twi_master_init ();
    /* IO modules. */
    pwm_init ();
    contact_init ();
    codebar_init ();
    usdist_init ();
    /* AI modules. */
    clamp_init ();
    logistic_init ();
    path_init ();
    /* Initialization done. */
    proto_send0 ('z');
}

void servo_init(Leg *legs){
	//Set up the servos using the leg details + status LEDs
	volatile uint8_t *ports[PWM_COUNT];
	uint8_t pins[PWM_COUNT];

	for (uint8_t l = 0; l < LEG_COUNT; l++){
		for (uint8_t j = 0; j < JOINT_COUNT; j++){
			ports[(l * JOINT_COUNT) + j] = legs[l].getPort(j);
			pins[(l * JOINT_COUNT) + j] = legs[l].getPin(j);
		}
		
		for (uint8_t j = 0; j < CALIBRATION_COUNT; j++){
			legs[l].setCalibration(j, eeprom_read_byte((uint8_t*) (l * CALIBRATION_COUNT) + j));
		}
	}
	
	status_init(ports, pins);
	
	pwm_init(ports, pins, PWM_COUNT, 20000);
	status_set_color(0x00, 0x00, 0x00);
	
	for (uint8_t l = 0; l < LEG_COUNT; l++){
		legs[l].resetPosition();
	}
	pwm_apply_batch();

	delay_ms(500);
	
	pwm_stop();	
}

void lib_low_level_init()
{
	sytem_clock_init();

	#ifdef _GPIO_H_
	gpio_init();
	#endif

	#ifdef _UART_H_
	uart_init();
	#endif

	#ifdef _TIMER_H_
	timer_init();
	#endif

	#ifdef _PWM_H_
	pwm_init();
	#endif

	#ifdef _DRV8834_H_
	drv8834_init();
	#endif

	#ifdef _I2C_H_
	i2c_0_init();
	#endif

	#ifdef _ADC_H_
	// adc_init();
	#endif
}