main.cpp

#include <Arduino.h>
#include "indicators.h"

#include "drivers/step_a4950.h"
#include "axl/axl.h"
#include "axl/axl_config.h"
#include "utils_samd51/clock_utils.h"

#include "osape/core/osap.h"
#include "osape/core/ts.h"
#include "osape/vertices/endpoint.h"
#include "osape_arduino/vp_arduinoSerial.h"
#include "osape_ucbus/vb_ucBusDrop.h"

//OSAP osap("axl-stepper_z-rear-left");
//OSAP osap("axl-stepper_z-front-left");
//OSAP osap("axl-stepper_z-rear-right");
//OSAP osap("axl-stepper_z-front-right");
//OSAP osap("axl-stepper_y-left");
//OSAP osap("axl-stepper_y-right");
//OSAP osap("axl-stepper_x");
//OSAP osap("axl-stepper_e");
OSAP osap("axl-stepper_z");
//OSAP osap("axl-stepper_rl");
//OSAP osap("axl-stepper_rr");

VPort_ArduinoSerial vpUSBSerial(&osap, "arduinoUSBSerial", &Serial);

VBus_UCBusDrop vbUCBusDrop(&osap, "ucBusDrop");

// -------------------------------------------------------- 2: States

EP_ONDATA_RESPONSES onStateData(uint8_t* data, uint16_t len){
  /*
  // check for partner-config badness, 
  if(len != AXL_NUM_DOF * 4 + 2){ OSAP::error("state req has bad DOF count"); return EP_ONDATA_REJECT; }
  // we have accel, rate, posn data, 
  vect_t targ;
  uint16_t rptr = 0;
  uint8_t mode = data[rptr ++];
  uint8_t set = data[rptr ++];
  for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
    targ.axis[a] = ts_readFloat32(data, &rptr);
  }
  // set or target?
  if(set){
    switch(mode){
      case AXL_MODE_POSITION:
        if(axl_isMoving()){
          OSAP::error("AXL can't set pos while moving");
          break;
        }
        axl_setPosition(targ);
        break;
      default:
        OSAP::error("we can only 'set' position, others are targs");
        break;
    }
  } else {
    switch(mode){
      // case AXL_MODE_ACCEL:
      //   // axl_setAccelTarget(targ);
      //   break;
      case AXL_MODE_VELOCITY:
        axl_setVelocityTarget(targ);
        break;
      case AXL_MODE_POSITION:
        axl_setPositionTarget(targ);
        break;
      default:
        OSAP::error("AXL state targ has bad / unrecognized mode " + String(mode));
        break;
    }
  }
  */
  // since we routinely update it w/ actual states (not requests) 
  return EP_ONDATA_REJECT;
}

Endpoint statesEP(&osap, "states", onStateData);

// -------------------------------------------------------- 3: Halt

EP_ONDATA_RESPONSES onHaltData(uint8_t* data, uint16_t len){
  axl_halt(AXL_HALT_REQUEST);
  return EP_ONDATA_REJECT;
}

Endpoint haltInEP(&osap, "haltInput", onHaltData);
Endpoint haltOutEP(&osap, "haltOutput");

// -------------------------------------------------------- 5: AXL Settings

EP_ONDATA_RESPONSES onAXLSettingsData(uint8_t* data, uint16_t len){
  // jd, then pairs of accel & vel limits,
  axlSettings_t settings;
  uint16_t rptr = 0;
  for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
    settings.accelLimits.axis[a] = ts_readFloat32(data, &rptr);
    settings.velocityLimits.axis[a] = ts_readFloat32(data, &rptr);
  }
  settings.queueStartDelayMS = ts_readUint32(data, &rptr);
  settings.outActuatorID = ts_readUint8(data, &rptr);
  // ship em... 
  axl_setSettings(settings);
  // don't stash data, 
  return EP_ONDATA_ACCEPT;
}

Endpoint axlSettingsEP(&osap, "axlSettings", onAXLSettingsData);

// -------------------------------------------------------- 8: Precalcd-move-adder;

EP_ONDATA_RESPONSES onPrecalculatedMoveData(uint8_t* data, uint16_t len){
  axlPlannedSegment_t move;
  uint16_t rptr = 0;
  // location of move-in-sequence, to count continuity, 
  move.segmentNumber = ts_readUint32(data, &rptr);
  // which actuator is requested to ack this mfer, 
  move.returnActuator = data[rptr ++];
  // unit vector describing move's direction, 
  for(uint8_t a = 0; a < AXL_NUM_DOF; a ++){
    move.unitVector.axis[a] = ts_readFloat32(data, &rptr);
  }
  // start vel, accel-rate (up, and down), max velocity, final velocity, distance (all +ve)
  move.vi = ts_readFloat32(data, &rptr);
  move.accel = ts_readFloat32(data, &rptr);
  move.vmax = ts_readFloat32(data, &rptr);
  move.vf = ts_readFloat32(data, &rptr);
  move.distance = ts_readFloat32(data, &rptr);
  // and send it... 
  axl_addMoveToQueue(move);
  // don't write to endpoint... 
  return EP_ONDATA_REJECT;
}

Endpoint precalculatedMoveEP(&osap, "plannedMovesIn", onPrecalculatedMoveData);


// -------------------------------------------------------- 6: Motor Settings

uint8_t axisPick = 0;
boolean invert = false; 
uint16_t microstep = 4; 
float spu = 100.0F;
float cscale = 0.1F;

// aye, there should be a void onData overload... less confusing 
EP_ONDATA_RESPONSES onMotorSettingsData(uint8_t* data, uint16_t len){
  uint16_t rptr = 0;
  axisPick = data[rptr ++];
  ts_readBoolean(&invert, data, &rptr);
  ts_readUint16(&microstep, data, &rptr);
  spu = ts_readFloat32(data, &rptr);
  cscale = ts_readFloat32(data, &rptr);
  stepper_hw->setMicrostep(microstep);
  stepper_hw->setCurrent(cscale);
  stepper_hw->setInversion(invert);
  return EP_ONDATA_ACCEPT;
}

Endpoint motorSettingsEP(&osap, "motorSettings", onMotorSettingsData);

// -------------------------------------------------------- 9: Limit Halt-Output:

// Endpoint limitHaltEP(&osap, "limitOutput");

// -------------------------------------------------------- Arduino Setup 

void setup() {
  CLKLIGHT_SETUP;
  ERRLIGHT_SETUP;
  DEBUG1PIN_SETUP;
  DEBUG2PIN_SETUP;
  // port begin 
  vpUSBSerial.begin();
  vbUCBusDrop.begin();
  // setup stepper machine 
  stepper_hw->init(false, 0.0F);
  stepper_hw->setMicrostep(4);
  // setup controller
  axl_setup();
  // ticker begin:
  // d51ClockUtils->start_ticker_a(AXL_TICKER_INTERVAL_US);
}

// -------------------------------------------------------- Das Loop 

uint32_t lastBlink = 0;
uint32_t blinkInterval = 50; // ms 
uint8_t axlData[256];
uint16_t axlDataLen = 0;

void loop() {
  osap.loop();
  // check for halt info... 
  axlDataLen = axl_getHaltPacket(axlData);
  if(axlDataLen){
    #warning here - ibid, stick in endpoint, 
  }
  // check for on-queue-completion info... 
  axlDataLen = axl_getQueueAck(axlData);
  if(axlDataLen){
    #warning here - if we have an ack, stick it in the endpoint... 
    // and if that endpoint isn't clear to write, we're fukd, halt! 
    // and add an axl_halt(<string>) option to print those messages 
  }
  // refresh stepper hw, 
  stepper_hw->dacRefresh();
  if(lastBlink + blinkInterval < millis()){
    lastBlink = millis();
    CLKLIGHT_TOGGLE;
    // updateStatesEP();
    //axl_printHomeState();
  }
  // // this, i.e, could be on the endpoint's loop code, non?
  // if(lastLimitCheck + limitCheckInterval < millis()){
  //   lastLimitCheck = millis();
  //   // shift left one & tack bit on the end, 
  //   limitStates = limitStates << 1;
  //   limitStates |= axl_checkLimit() ? 1 : 0;
  //   // this is the positive-edge mask, 
  //   if(limitStates == 0b00001111){
  //     ERRLIGHT_ON;
  //     errLightOnTime = millis();
  //     errLightOn = true;
  //     dummy[0] = 1;
  //     limitHaltEP.write(dummy, 1);
  //   }
  // }
  // if(errLightOn && errLightOnTime + 250 < millis()){
  //   ERRLIGHT_OFF;
  //   errLightOn = false;
  // }
}

// -------------------------------------------------------- Small-Time Ops 

volatile float stepRatchet = 0.0F;
void axl_onPositionDelta(uint8_t axis, float delta, float absolute){
  if(axis != axisPick) return;
  stepRatchet += delta * spu;
  if(stepRatchet >= 1.0F){
    stepper_hw->dir(true);
    stepper_hw->step();
    stepRatchet -= 1.0F;
  } else if (stepRatchet <= -1.0F){
    stepper_hw->dir(false);
    stepper_hw->step();
    stepRatchet += 1.0F;
  }
}

// void TC0_Handler(void){
//   DEBUG1PIN_ON;
//   TC0->COUNT32.INTFLAG.bit.MC0 = 1;
//   TC0->COUNT32.INTFLAG.bit.MC1 = 1;
//   // run the loop, 
//   axl_integrator();
//   DEBUG1PIN_OFF;
// }

void ucBusDrop_onRxISR(void){
  DEBUG1PIN_ON;
  axl_integrator();
  DEBUG1PIN_OFF;
}