main.cpp

#include <Arduino.h>

#include "drivers/indicators.h"
#include "utils/cobs.h"
#include "osap/osap.h"

OSAP* osap = new OSAP("cl stepper motor drop");

#include "osap/vport_usbserial.h"
VPort_USBSerial* vPortSerial = new VPort_USBSerial();
#include "drivers/ucbus_drop.h"
#include "drivers/step_cl.h"

#include "utils/clocks_d51_module.h"

union chunk_float32 {
  uint8_t bytes[4];
  float f;
};

union chunk_float64 {
  uint8_t bytes[8];
  double f;
};

// adhoc reply 
uint8_t reply[1024];
uint16_t rl = 0;

uint8_t replyBlankPck[1024];
uint16_t replyBlankPl = 0;
uint16_t replyBlankPtr = 0;
uint16_t replyBlankSegsize = 0;
VPort* replyBlankVp;
uint16_t replyBlankVpi = 0;
uint16_t lastQueueSpaceTxd = 0;

#define BUS_DROP 1    // Z: 1, YL: 2, X: 3, YR: 4
#define AXIS_PICK 2   // Z: 2, Y: 1, X: 0
#define AXIS_INVERT false  // Z: false, YL: true, YR: false, X: false
#define SPU 3200.0F // always positive! Z: 3200, XY: 400 
#define C_SCALE 0.2F // 0-1, floating: initial holding current to motor, 0-2.5A 
#define TICKS_PER_PACKET 20.0F // always 20.0F

void setup() {
  ERRLIGHT_SETUP;
  CLKLIGHT_SETUP;
  DEBUG1PIN_SETUP;
  // osap
  osap->description = "cl controller test";
  // serport 
  osap->addVPort(vPortSerial);
  // bus 
  ucBusDrop->init(false, BUS_DROP);
  // cl controller 
  step_cl->init();
}

uint8_t bChPck[64];

volatile float current_floating_pos = 0.0F;
volatile int32_t current_step_pos = 0;
volatile uint32_t delta_steps = 0;

volatile float vel = 0.0F; 
volatile float move_counter = 0.0F;

volatile boolean setBlock = false;

uint16_t tick = 0;

void loop() {
  DEBUG1PIN_TOGGLE;
  osap->loop();
  stepper_hw->dacRefresh();

  tick++;
  if(tick > 500){
    tick = 0;
    enc_as5047->trigger_read();
    while(!enc_as5047->is_read_complete());
    uint16_t reading = 0b0011111111111111 & enc_as5047->get_reading();
    //sysError(String(reading));
  }
  // do packet B grab / handle 
  /*
  if(ucBusDrop->ctr_b()){
    uint16_t len = ucBusDrop->read_b(bChPck);
    uint16_t ptr = 0;
    switch(bChPck[ptr]){
      case AK_SETCURRENT: {
        // get currents 
        ptr ++;
        chunk_float32 currentChunks[3];
        currentChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        currentChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        currentChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        // set current
        stepper_hw->setCurrent(currentChunks[AXIS_PICK].f);
        // bug here 
        // I noticed that setcurrent commands to the z motor seem to fail: the motor drops to a low current no matter the value transmitted
        // I've checked (by inserting a flag to pick y-values) here, and those values work well, so it's nothing with the motor driver itself
        // so, I suspect the b-channel bus packets are coming out of those structures one byte short - makes sense there would be some bug in/around delineation 
        break;
        }
      case AK_SETPOS: {
        // get posns, 
        ptr ++;
        chunk_float32 posChunks[3];
        posChunks[0] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        posChunks[1] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        posChunks[2] = { .bytes = { bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++], bChPck[ptr ++] } };
        float target_current_pos = posChunks[AXIS_PICK].f;
        int32_t target_current_steps = lroundf(target_current_pos * SPU);
        setBlock = true; // don't do step work while these are modified 
        current_floating_pos = target_current_pos;
        current_step_pos = target_current_steps;
        setBlock = false;
      }
      default:
        // noop,
        break;
    }
  }
  */
} // end loop 

void ENC_AS5047::on_read_complete(uint16_t pos){
  // do work 
}

void OSAP::handleAppPacket(uint8_t *pck, uint16_t pl, uint16_t ptr, uint16_t segsize, VPort* vp, uint16_t vpi, uint8_t pwp){
  // track end of header, to reply with 
  uint16_t replyPtr = ptr;
  // (a hack) store one app packet, to format our replies with. do once 
  if(replyBlankPtr == 0){
    for(uint16_t i = 0; i < pl; i++){
      replyBlankPck[i] = pck[i];
    }
    replyBlankPl = pl;
    replyBlankPtr = ptr;
    replyBlankSegsize = segsize;
    replyBlankVp = vp;
    replyBlankVpi = vpi;
  }
  // clear out our reply,   
  rl = 0;
  reply[rl ++] = DK_APP;
  // do the reading:
  ptr ++; // walk appcode DK_APP
  switch(pck[ptr]){
    case AK_RUNCALIB:
      ptr ++; // walk stepcode 
      reply[rl ++] = AK_RUNCALIB;
      if(step_cl->calibrate()){
        reply[rl ++] = AK_OK;
      } else {
        reply[rl ++] = AK_ERR;
      }
      // do step 
      break;
    case AK_READCALIB:
      ptr ++; // walk readcode
      reply[rl ++] = AK_READCALIB;
      step_cl->print_table(); 
      // do work 
      break;
    default:
      break;
  }
  // end pck[ptr] switch 
  if(rl > 1){
    if(vp->cts()){
      appReply(pck, pl, replyPtr, segsize, vp, vpi, reply, rl);
    } else {
      sysError("on reply, not cts, system fails");
    }
  }
  // always clear this 
  vp->clearPacket(pwp);
}

void UCBus_Drop::onRxISR(void){
  /*
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  move_counter += vel;
  boolean move_check = (move_counter > 1.0F);
  //DEBUG2PIN_TOGGLE;
  if(move_check){
    move_counter -= 1.0F;
    if(delta_steps == 0){
      // nothing 
    } else {
      //DEBUG1PIN_TOGGLE;
      stepper_hw->step();
      delta_steps --;
      if(stepper_hw->getDir()){
        current_step_pos ++;
      } else {
        current_step_pos --;
      }
    }
  }
  */
}

void UCBus_Drop::onPacketARx(void){
  /*
  if(setBlock) return;
  //DEBUG2PIN_TOGGLE;
  // last move is done, convert back steps -> float,
  current_floating_pos = current_step_pos / SPU;
  vel = 0.0F; // reset zero in case packet is not move 
  uint8_t bptr = 0;
  // switch bus packet types 
  switch(inBufferA[0]){
    case UB_AK_GOTOPOS:
      {
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        // chunk_float64 target = { 
        //   .bytes = { inBuffer[bptr], inBuffer[bptr + 1], inBuffer[bptr + 2], inBuffer[bptr + 3],
        //             inBuffer[bptr + 4], inBuffer[bptr + 5], inBuffer[bptr + 6], inBuffer[bptr + 7] }};
        float delta = target.f - current_floating_pos;
        // update,
        //move_counter = 0.0F; // shouldn't need this ? try deleting 
        // direction swop,
        if(delta > 0){
          stepper_hw->dir(true);
        } else {
          stepper_hw->dir(false);
        }
        // how many steps, 
        delta_steps = lroundf(abs(delta * SPU));
        // what speed 
        vel = abs(delta * SPU) / TICKS_PER_PACKET;
        // for str8 r8 
        // if(delta_steps == 0){
        //   vel = 0.0F;
        // } else {
        //   vel = abs(delta * SPU) / TICKS_PER_PACKET;
        // }
        break; // end gotopos 
      }
    case UB_AK_SETPOS:
      {
        // reqest is to set position, not go to it... 
        bptr = AXIS_PICK * 4 + 1;
        chunk_float32 target = {
          .bytes = { inBufferA[bptr], inBufferA[bptr + 1], inBufferA[bptr + 2], inBufferA[bptr + 3] }
        };
        float target_current_pos = target.f;
        int32_t target_current_steps = lroundf(target_current_pos * SPU);
        setBlock = true; // don't do step work while these are modified 
        current_floating_pos = target_current_pos;
        current_step_pos = target_current_steps;
        vel = 0;
        delta_steps = 0;
        setBlock = false;
        break;
      }
    default:
      break;
  }  
  //DEBUG2PIN_OFF;
  */
}