weaver.bak

Welding Robot controller C Embedded Program on micro-controller.

/******************************************************************************
* welding mobile robot control programme
* weaver control module
* @author: thai nguyen nhut dien
* @date: may 2006
******************************************************************************/
#define W_DIR    PIN_C5
#define W_ENA    PIN_E2


int1   POS_DIR      = 0;
int1   wdir         = 0;
int1   atBorder     = 0;
int16  pos          = 0;
signed int16  prev_pos     = 0;

void resetWeaver() {

  output_high(W_ENA);
  if (!useWeaver)	return;
  set_adc_channel(1);
  delay_us(10);
  pos = read_adc();

  if (pos > 500) {
    output_bit(W_DIR, !POS_DIR);
    set_pwm2_duty(400);
    do {
      pos = read_adc();
      if (pos < 80 || pos > 950) {
        output_low(W_ENA);
        //set_pwm2_duty(0);
        display(1001);
      }
      delay_ms(SAMPLING_TIME);
    } while (pos > 500);

  }else if (pos < 500) {
    output_bit(W_DIR, POS_DIR);
    set_pwm2_duty(400);
    do {
      pos = read_adc();
      if (pos < 80 || pos > 950) {
        output_low(W_ENA);
        display(1002);
      }
      delay_ms(SAMPLING_TIME);
    } while (pos < 500);
  }
  output_low(W_ENA);
}

void detectWeaver() {

  set_adc_channel(1);

  delay_us(10);
  pos = read_adc();

  if ( (pos<50)||(pos>980) ) {
    useWeaver = 0;
    display(7777);
    delay_ms(1500);
  }
  else {
  	useWeaver = 1;

    // detect positive direction
    output_bit(W_DIR, POS_DIR);
    pos = read_adc();

    set_pwm2_duty(300);
    output_high(W_ENA);
    delay_ms(30);
    output_low(W_ENA);
    
    if (read_adc()<pos) {
      POS_DIR = !POS_DIR;
    }
    // reset weaver
     resetWeaver();
  }
  output_low(W_ENA);
}

void controlWeaver() {
  if (!useWeaver) return;
  output_high(W_ENA);
  pos = read_adc();
  if (pos < 80 || pos > 950) {
    output_low(W_ENA);
    run = 0;
    display(1003);
    delay_ms(500);
    return;
  }else if ((pos < (500-inAmp)) && (wdir == !POS_DIR)) {
    wdir = POS_DIR;
    output_low(W_ENA);
    atBorder = 1;
  }else if ((pos > (500+inAmp)) && (wdir == POS_DIR)) {
    wdir = !POS_DIR;
    output_low(W_ENA);
    atBorder = 1;
  }else {
  	 output_bit(W_DIR, wdir);
    //printf("biendo %ld\r",inAmp)
  }
}