MOVIDRIVE MDX61B

#include <constb.h>
#include <iob.h>

#pragma var 380 399
#pragma globals 400 581

long counter;

#define SPEED_MAX 15000

long speed;

// PROFIBUS DP
GSPODATA10 podata;
// PO1 - Counter
// PO2 - Control word // 0 - enable/stop, 1 - perform positioning
// PO3 - Actual distance
// PO4 - Distance setting
// PO5 - Distance near
// PO6 - Distance far
// PO7 - Distance window
SSPIDATA10 pidata;
// PI1 - Counter
// PI2 - Status word
// PI3 - Actual speed

long host_counter;
long cw;
long distance;
long distance_setting;
long distance_window;

Task2_IO()
{
  _GetSys(podata, GS_PODATA);
  host_counter = (podata.PO1 & 0xFFFF);
  cw = (podata.PO2 & 0xFFFF);
  distance = (podata.PO3 & 0xFFFF);
  distance_setting = (podata.PO4 & 0xFFFF);
  distance_window = (podata.PO7 & 0xFFFF);
  //_SetSys(SS_ACTPOS, podata.PO?);
  pidata.PI1 = counter;
  pidata.PI2 = StatusWord;
  pidata.PI3 = ControlWord;
  _GetSys(pidata.PI3, GS_ACTSPEED);
  _SetSys(SS_PIDATA, pidata);
}

direction_CW()
{
  speed = SPEED_MAX;
  _SetSys(SS_N11, speed);
  speed /= 3;
  _SetSys(SS_N12, speed);
  speed /= 5;
  _SetSys(SS_N13, speed);
}

direction_CCW()
{
  speed = -SPEED_MAX;
  _SetSys(SS_N11, speed);
  speed /= 3;
  _SetSys(SS_N12, speed);
  speed /= 5;
  _SetSys(SS_N13, speed);
}

stop()
{
  _BitSet(ControlWord, 1);
  _BitClear(ControlWord, 2);

  _BitClear(ControlWord, 4);
  _BitClear(ControlWord, 5);
}

run1()
{
  _BitClear(ControlWord, 1);
  _BitSet(ControlWord, 2);

  _BitSet(ControlWord, 4);
  _BitClear(ControlWord, 5);
}

run2()
{
  _BitClear(ControlWord, 1);
  _BitSet(ControlWord, 2);

  _BitClear(ControlWord, 4);
  _BitSet(ControlWord, 5);
}

run3()
{
  _BitClear(ControlWord, 1);
  _BitSet(ControlWord, 2);

  _BitSet(ControlWord, 4);
  _BitSet(ControlWord, 5);
}

#define MOVE_FORWARD 1
#define MOVE_BACKWARD 2
#define GO_POSITION_MODE 3

long delta;
long direction;
long target_position;
long mode;

go_position()
{
  if (distance < target_position) {
    direction_CW();
    direction = 0;
  } else {
    direction_CCW();
    direction = 1;
  }
  while (cw == mode) {
    if (direction == 0) {
      delta = target_position - distance;
    } else {
      delta = distance - target_position;
    }
    if (delta < 1) {
      stop();
      break;
    } else {
      if (delta < distance_window / 3) {
        run3();
      } else if (delta < distance_window) {
        run2();
      } else {
        run1();
      }
    }
  }
  stop();
}

long d0, dd;
long guard_counter;

Task3_Guard()
{
  guard_counter = 0;
  while (1) {
    d0 = distance;
    _Wait(1000);
    if (d0 < distance) {
      dd = distance - d0;
    } else {
      dd = d0 - distance;
    }
    if ((pidata.PI3 != 0) && (dd < 20)) {
      ++guard_counter;
      if (3 < guard_counter) {
        mode = 0;
        _Wait(3000);
        guard_counter = 0;
      }
    } else {
      guard_counter = 0;
    }
  }
}

main()
{
  _WdOn(5000);
  while ((StatusWord & 0b100) == 0)
    continue;
  _WdOff();

  target_position = distance;

  podata.BusType = GS_BT_FBUS;
  podata.Len = pidata.Len = 10;
  counter = cw =
  podata.PO2 = podata.PO1 = 0;
  _SetTask(ST2_START, Task2_IO);
  _SetTask(ST3_START, Task3_Guard);

  _Wait(3000);
  counter = host_counter;
  while (1) {
    if (cw == 0) {
      _Wait(100);
      _BitSet(ControlWord, 1);
    }
    if (counter != host_counter) {
      mode = cw;
      switch (cw) {
      case MOVE_FORWARD : {
        target_position = (podata.PO5 & 0xFFFF);
        go_position();
        break;
      }
      case MOVE_BACKWARD : {
        target_position = (podata.PO6 & 0xFFFF);
        go_position();
        break;
      }
      case GO_POSITION_MODE : {
        target_position = distance_setting;
        go_position();
        break;
      }
      default : {
        target_position = distance;
        _BitSet(ControlWord, 1);
      }
      }
      counter = host_counter;
    }
    //_SystemCall(SC_N0, SpeedZero); // Call if speed is zero
    //_WaitSystem( SC_N0 ); // Wait until speed is zero
  }
}