motor.c

Welding Robot controller C Embedded Program on micro-controller.

#define GEAR_RATIO       1
#define GEARBOX_RATIO    120
#define WHEEL_RADIUS     25

#define Kp    0.28 // 2.75    // 11.55.8.5
#define Ki    0.15
#define DIR     PIN_A5  // motor direction
#define ENA     PIN_E1   // motor enable

signed int16  pwm = 0;     // pwm duty
float  pwm1 = 0;
float  cur    = 0;         // current
float  err    = 0;
float  err_last    = 0;

int    dcount = 0;
int    scount = 0;

#int_rtcc
void timer0()
{
  int16  temp=0;
  if (!run) return;
  scount++;
  if (scount==3) {

     temp = get_timer1();
     //cur = 0.3636(float)(temp);
     cur = 0.1212*(float)(temp);
     printf("xung=%ld\r",temp);
     //cur = (pulse / 360) * (1000 / 10) / (GEARBOX_RATIO * GEAR_RATIO) * (2 * PI * WHEEL_RADIUS);
     //cur3 = (pulse / 360) * (1000 / 30) / (GEARBOX_RATIO * GEAR_RATIO) * (2 * PI * WHEEL_RADIUS);
     temp=0;
     scount=0;
     set_timer1(0);
  }
  set_timer0(60);
}


void controlRobotSpeed(int speed) {
  float temp;

  if (!speed) {
    output_low(ENA);
    cur = 0;
  }else {

    err  = (float) speed - cur;
    temp = Kp*err;
    temp+= Ki*err_last;
    err_last=err;

    //if (err>0) pwm +=(int16)temp;
    //else  pwm -=(signed int16)temp;
    //if (err>0) pwm1 +=temp;
    //else  pwm1 -=temp;
    pwm1 +=temp;

    printf("err=%f\r",temp);

    pwm  = (signed int16)(pwm1);
    pwm  = pwm < 50 ? 50 : pwm > 1023 ? 1023 : pwm;
    set_pwm1_duty(pwm);

    output_bit(DIR, !input(DIRECTION));
    output_high(ENA);
  }

  // delay display update
  if (++dcount == 100) {
    dcount = 0;
    displayFloat(cur);
  }
}