taskCtrl.c

Go to the documentation of this file.

#include "taskCtrl.h"
#include "packetHandleTarget.h"
#include "sh7045lib.h"


static void watchDogTimer(runCtrl_t *run) {
  if (run->watch_dog_msec > 0) {
    if (--run->watch_dog_msec == 0) {
      run->mode.ctrl = MODE_SERVO_CTRL;
      run->mode.straight_ctrl = MODE_STOP_FIRST;
      run->mode.rotate_ctrl = MODE_NO_CTRL;
    }
  }
}


static void resetWatchDogTimer(runCtrl_t *run) {
  run->watch_dog_msec = run->watch_dog_msec_max;
}


void update_tRunCtrlState(runCtrl_t *run) {
  int straight_mm_vel, rotate_div16_vel;
  int cnt[2];
  int i;

  if (++run->msec >= 1000) {
    run->msec = 0;
    ++(run->sec);
  }
  watchDogTimer(run);

  for (i = 0; i < 2; ++i) {
    updateEncDiff(&run->enc[i]);
  }
  for (i = 0; i < 2; ++i) {
    cnt[i] =
      run->bodyInfo.mtr_direction[i] * run->bodyInfo.whl[i]->enc->diff;
  }
  updateBodyPosition(&run->bodyPos, cnt[WHL_RIGHT], cnt[WHL_LEFT]);
  updateCoordinatePosition(&run->gl_crd, &run->bodyPos);
  updateVelocityInfo(&run->velInfo, &run->bodyPos);
  convertCoordinateInfo(&run->gl_crd, &run->gl_crd_offset);

  if (run->mode.ctrl != MODE_SERVO_CTRL) {
    run->mode.straight_ctrl = MODE_NO_CTRL;
    run->mode.rotate_ctrl = MODE_NO_CTRL;
  }
  switch (run->mode.ctrl) {
  case MODE_SERVO_CTRL:
    run->run_crd = run->gl_crd;
    convertCoordinateInfo(&run->run_crd, &run->run_crd_offset);
    straight_mm_vel = calcStraightRefVel(&run->straight, &run->mode,
                                         &run->bodyPos, &run->velInfo,
                                         &run->run_crd);
    rotate_div16_vel = calcRotateRefVel(&run->rotate, &run->mode,
                                     &run->bodyPos, &run->velInfo,
                                     &run->run_crd);
    setBodyMove(&run->bodyInfo, straight_mm_vel, rotate_div16_vel);
    break;

  case MODE_DIRECT_CTRL:
    for (i = 0; i < 2; ++i) {
      set_pwm(i, run->direct[i].pwm);
      if (run->direct[i].mode != DIRECT_MODE_NONE) {
        set_mode(i, run->direct[i].mode);
      }
    }
    break;

  case MODE_DIRECT_WHEEL_CTRL:
    for (i = 0; i < 2; ++i) {
      setWheelMoveVelocity(run->directWhl[i].ref_mm_sec_vel, &run->whl[i]);
    }
    break;

  case MODE_SERVO_FREE:
    for (i = 0; i < 2; ++i) {
      setWheelFree(run->bodyInfo.whl[i]);
    }
    break;
  }
  getRawCount(&run->used_msec);
}


void recv_tRunCtrlPacket(runCtrl_t *run) {
  if (packetRecvHandler(run) > 0) {
    resetWatchDogTimer(run);
  }
}


void init_tRunCtrlState(runCtrl_t *run, long version) {
  int i;

  run->version = version;
  run->msec = 0;
  run->sec = 0;
  run->watch_dog_msec = 0;

  initEnc();
  initMotor();
  for (i = 0; i < 2; ++i) {
    initEncInfo(i, &run->enc[i]);
    initMotorInfo(i, &run->mtr[i]);
    initDirectInfo(&run->direct[i]);
    initWheelInfo(&run->whl[i], &run->mtr[i], &run->enc[i], i);
    initDirectWheelCtrlInfo(&run->directWhl[i]);
  }
  initBodyInfo(&run->bodyInfo, &run->whl[WHL_RIGHT], &run->whl[WHL_LEFT]);
  initBodyPositionInfo(&run->bodyPos, &run->bodyInfo);
  initVelocityCtrlInfo(&run->straight);
  initVelocityCtrlInfo(&run->rotate);
  initVelocityInfo(&run->velInfo);

  initCoordinateInfo(&run->gl_crd);
  initCoordinateInfo(&run->gl_crd_offset);
  initCoordinateInfo(&run->run_crd);
  initCoordinateInfo(&run->run_crd_offset);
  initModeInfo(&run->mode);
}


void reset_tRunCtrlPosition(runCtrl_t *run) {
  initBodyPositionInfo(&run->bodyPos, &run->bodyInfo);
}