/*********************************  rvalve.c  *******************************
 * $Source: /home/cvs/ESP/gen2/software/msp430/dwarves/rvalve.c,v $
 *  Copyright (C) 2005 MBARI
 *
 *  MBARI Proprietary Information. All rights reserved.
 * $Id: rvalve.c,v 1.25 2005/11/24 00:21:59 brent Exp $
 *
 * Rotary Valve control algorithm
 *
 *****************************************************************************/

#include "msp430x16x.h"  //register and I/O definitions
#include "dwarfcore.h"
#include "rvalve.h"
#include "parser.h"
#include "debugport.h"
#include "espdwarf.h"

#define maxErr(rv) (rv->maxError & 0x7fff)

static byte drvCtlImage = 0;
static byte drvMtrCtlImage = 0;

static void setDrvCtl (byte mask)
{
  _DINT();
  drvCtlImage = mask;
  writeDWRF (DRVCTL, mask);
  _EINT();
}

static void setDrvMtrCtl (byte mask)
{
  _DINT();
  drvMtrCtlImage = mask;
  writeDWRF (DRVMTRCTL, mask);
  _EINT();
}


static void getShaftPos(RotaryValve* rv)
{
  register uint16 rotaryEnc0, rotaryEnc1, rotaryEnc2, rotaryEnc3;

  register uint16 *in = (uint16 *)&DWRFBUSIN;
  register byte mask = DWRFBUSRD;
  register uint16 temp;
  
#macro readNext(result)
  DWRFSTRBOUT &= ~mask;
/$
   mov.b \@@in, @result
$/
  DWRFSTRBOUT |= mask;
  DWRFSTRBOUT &= ~mask;
/$
   mov.b \@@in, @temp
$/
  DWRFSTRBOUT |= mask;
/$
   swpb  @temp
   bis.w @temp,@result
$/
#endm
  
  _DINT(); _NOP();
  setDWRFadr(DRV0LOW), floatDWRFbus();  
  readNext (rotaryEnc0);
  readNext (rotaryEnc1);
  readNext (rotaryEnc2);
  readNext (rotaryEnc3);
  freezeDWRFadr();
  _EINT();
  
  mask = drvCtlImage;
  if (mask & ENCPWR0)   rv[0].position = rotaryEnc0;
  if (mask & ENCPWR1)   rv[1].position = rotaryEnc1;
  if (mask & ENCPWR2)   rv[2].position = rotaryEnc2;
  if (mask & ENCPWR3)   rv[3].position = rotaryEnc3;
}


static const byte pwrOnx[NROTARY] = {ENCPWR0, ENCPWR1, ENCPWR2, ENCPWR3};
static const byte MxS[NROTARY] = {M0S, M1S, M2S, M3S};
static const byte MxB[NROTARY] = {M0B, M1B, M2B, M3B};
static const byte Mxx[2*NROTARY] = {M0R, M0F, M1R, M1F, M2R, M2F, M3R, M3F};


static int16 delta (RotaryValve *rv, uint16 goal)
//return # of counts added modulo revCounts to current position to reach goal
//may be negative
{
  uint16 current = rv->position;
  uint16 revCounts = rv->revCounts;
  int16 halfRev = revCounts/2;
  int16 distance = goal - current;
  int16 offset = 0;
  if (distance > halfRev) offset = -revCounts;
  else if (distance < -halfRev) offset = revCounts;
  return distance + offset;
}


static uint16 desiredDirection (RotaryValve *rv, int16 delta)
// delta is the result from delta(rv)
// returns 0 for rotating Up (increasing counts), or 1 for rotating down
{
  uint16 direction = 0;
  if (delta < 0) {
    direction = 1;
    delta = -delta;
  }      //direction = the shortest direction toward goal 
  {
    int8 rvDir = rv->direction;
    if (rvDir) {    //a direction was specified so override nearest
      uint16 closest = direction; //remember the closest direction
      direction = rvDir < 0;  //map specified direction to a 0 or 1
      if (direction == closest && delta < rv->revCounts/4) rv->direction = 0;
        //all rotations specified eventually are in the "closest" direction
    }
  }
  return direction;
}


void idleShaft (RotaryValve *rv)
{
  byte mtrMask = drvCtlImage & ~pwrOnx[rv->Valve];
  if (!(mtrMask & (ENCPWR0|ENCPWR1|ENCPWR2|ENCPWR3)))
    mtrMask &= ~MTRPWR;
  setDrvCtl(mtrMask);   //power off motor too with last encoder
  rv->State = SSMoff;
}


static uint16 ShaftStateMachine(RotaryValve *rv)
{
//this routine runs a state machine for one of the shafts
//returns fullPWM if far from the goal or
//  the shaft's configured 'slowPWM' if near it

  switch (rv->State) { 
    case SSMread:    //update position without moving
      if (--rv->timer) break;
      setDrvCtl (drvCtlImage | pwrOnx[rv->Valve]);
      rv->State = SSMreading;
      break;

    case SSMstart:  // start a move to specified goal angle
      //this valve has been asked to seek a postion
      //first turn on the encoder and motor power
      setDrvCtl (drvCtlImage | pwrOnx[rv->Valve] | MTRPWR);
      rv->lashGoal = rv->goal;
      rv->State = SSMcheck;
      break;   

    case SSMcheck:
    {
      int16 distance = delta(rv, rv->lashGoal);
      if (abs(distance) > maxErr(rv)) {
        uint16 direction = desiredDirection(rv, distance);
        uint16 lash = rv->lash;
        uint16 lashGoal;
        if (direction) {    //offset goal for expected backlash
          lashGoal = rv->goal - lash;
          if (lashGoal >= rv->revCounts) lashGoal += rv->revCounts;
          distance = lashGoal - rv->position;
          rv->direction = -1;  //avoid ambiguity oscillation on half rotations
        }else{
          lashGoal = rv->goal + lash;
          if (lashGoal >= rv->revCounts) lashGoal -= rv->revCounts;
          distance = rv->position - lashGoal;
          rv->direction = 1;   //avoid ambiguity oscillation on half rotations
        }
        rv->lashGoal = lashGoal;
        rv->timer = rv->stuckTics;
        rv->lastDist = 0;
        
        rv->State = SSMrotatingUp + direction;
        if (rv->maxError & 0x8000) direction ^= 1;
        {
          unsigned v = rv->Valve;
          setDrvMtrCtl (drvMtrCtlImage & MxS[v] | Mxx[(v<<1)|direction]);
        }
        if (distance < rv->near) return rv->slowPWM;
        break;
      }  //else, fall into SSMreading case to report final position
    }
      
    case SSMreading:
      idleShaft(rv);
      set16(opComplete,rvalve0 << rv->Valve);//Signal not sent yet so send it!
      signalEV();
    case SSMoff:         //channel inactive
      break;    


    case SSMrotatingUp:
    case SSMrotatingDown:
    {  //continue while not at goal AND not wanting to change direction...
      int16 distance = delta(rv, rv->lashGoal);
      uint16 magnitude = abs(distance);
      if (magnitude > maxErr(rv) && 
          desiredDirection(rv,distance) == rv->State-SSMrotatingUp) {
        if (rv->timer) {  //try to detect if we're stuck
          uint16 closingRate = abs(distance - rv->lastDist);
          if (closingRate > 1) {  //closing in on goal
            rv->lastDist = distance;
            rv->timer = rv->stuckTics;
          }else if (!--rv->timer) { //we're stuck, so request...
            rv->timer=1;
            break;  //full power regardless of how close we are to the goal!
          }
        }
        if (magnitude < rv->near) return rv->slowPWM;
        break;
      } //else, we overshot the goal
      setDrvMtrCtl (drvMtrCtlImage | MxB[rv->Valve]);  //so, put on the brakes!
      rv->State = SSMbraking;
      rv->timer = rv->brakeTics;
    }
    case SSMbraking: //wait for motor to stop
      if (--rv->timer) break;
      rv->State = SSMcheck;
    default:
      setDrvMtrCtl (drvMtrCtlImage & MxS[rv->Valve]);
  }//end of state switch

  return fullPWM;
}//End of ShaftStateMachine


void valveISR(RotaryValve* rv)
{
  uint16 lowestPWM;
  int32  voltComp;
  RotaryValve *end;
  
//start the reading of the flags
  _DINT(); _NOP();
  writeDWRF (DRVCTL, drvCtlImage | START);
  _EINT();
  
/* start to compute low speed PWM with voltage compensation
   while waiting for absolute angle encoders to be read
   (we rely on this taking at least 10 microseconds)
   ADC12MEM2 is 205 counts per battery volt
   Note that the physics of the compensation is parabolic
   This is a linear approximation.
*/
  voltComp = 1400L*(int16)(16*205-ADC12MEM2); //16V = maximum operating voltage
  end = rv+NROTARY;
  lowestPWM = fullPWM;
  
//now read the results
  getShaftPos(rv);
  do {  //update control state of each shaft
    uint16 thisShaftsPWM = ShaftStateMachine(rv);
    if (thisShaftsPWM < lowestPWM) lowestPWM = thisShaftsPWM;
  } while (++rv < end);
/* compute low speed PWM with voltage compensation
   while waiting for absolute angle encoders to be read
   (this MUST TAKE at least 10 microseconds so encoders can shift out data)
   ADC12MEM2 is 205 counts per battery volt
   Note that the physics of the compensation is parabolic
   This is a linear approximation roughly valid for voltages from 10 to 16.
*/
  {
    int pwm = (((int32)lowestPWM<<16) + voltComp)>>16;
    if (pwm >= fullPWM)
      TBCCR5 = fullPWM;
    else if (pwm <= 0)
      TBCCR5 = 0;  //this is an error condition (linear approximation failed)
    else
      TBCCR5 = pwm;
  }
}



void configRotaryValves(RotaryValve* rv)
{
  unsigned valveNum = 0;
  while (valveNum < NROTARY) {
    rv->Valve=valveNum;	 	
    rv->revCounts = 512;
    rv->maxError = 2;
    rv->brakeTics = 4;
    rv->stuckTics = 0;  //disable stuck detection by default
    rv->near = 0;
    rv->lash = 0;
    rv->timer = 1;
    rv->slowPWM=6*fullPWM/10; //default to 60% power output when "near" goal
    rv->State = SSMread;      //do an initial read of position
    rv++;
    valveNum++;
  }
}