main.cpp

#include <Arduino.h>

#include "drivers/indicators.h"
#include "osape/osap/osap.h"

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

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

#include "osape/osap/vport_ucbus_drop.h"

#include "drivers/step_cl.h"
#include "osape/utils/clocks_d51.h"


// -------------------------------------------------------- 0: MAG DIAGNOSTICS 

boolean onMagDiagnosticsPut(uint8_t* data, uint16_t len);
boolean onMagDiagnosticsQuery(void);

Endpoint* magDiagnosticsEP = osap->endpoint(onMagDiagnosticsPut, onMagDiagnosticsQuery);

boolean onMagDiagnosticsPut(uint8_t* data, uint16_t len){
  return true;
}

boolean onMagDiagnosticsQuery(void){
  // put diag. data in the stash 
  uint16_t magField = enc_as5047->read_field_mag(); 
  as5047_diag_t diagnostics = enc_as5047->read_enc_diagnostics(); 
  uint8_t data[7];
  uint16_t wptr = 0;
  ts_writeBoolean(diagnostics.magHi, data, &wptr);
  ts_writeBoolean(diagnostics.magLo, data, &wptr);
  ts_writeBoolean(diagnostics.cordicOverflow, data, &wptr);
  ts_writeBoolean(diagnostics.compensationComplete, data, &wptr);
  ts_writeUint8(diagnostics.angularGainCorrection, data, &wptr);
  ts_writeUint16(magField, data, &wptr);
  magDiagnosticsEP->write(data, 7);
  return true;
}

// -------------------------------------------------------- 1: RUN CALIB

boolean onRunCalibPut(uint8_t* data, uint16_t len);
Endpoint* runCalibEP = osap->endpoint(onRunCalibPut);

boolean onRunCalibPut(uint8_t* data, uint16_t len){
  step_cl->calibrate();
  return true;
}

// -------------------------------------------------------- 2: MODE SWITCH 

boolean onRunModePut(uint8_t* data, uint16_t len){
  step_cl->set_run_mode(data[0]);
  return true;
}

Endpoint* runModeEP = osap->endpoint(onRunModePut);

// -------------------------------------------------------- 3: POSITION TARGET 

boolean onPositionTargetPut(uint8_t* data, uint16_t len);
Endpoint* positionTargetEP = osap->endpoint(onPositionTargetPut);

boolean onPositionTargetPut(uint8_t* data, uint16_t len){
  chunk_float32 tac = { .bytes = { data[0], data[1], data[2], data[3] }};
  step_cl->set_angle_target(tac.f);
  return true;
}

// -------------------------------------------------------- 4: CURRENT POSITION

boolean onPositionPut(uint8_t* data, uint16_t len);
boolean onPositionQuery(void);
Endpoint* positionQueryEP = osap->endpoint(onPositionPut, onPositionQuery);

boolean onPositionPut(uint8_t* data, uint16_t len){
  return true;
}

boolean onPositionQuery(void){
  float posn = step_cl->get_last_pos_est();
  uint8_t data[4];
  uint16_t wptr = 0;
  ts_writeFloat32(posn, data, &wptr);
  positionQueryEP->write(data, 4);
  return true;
}

// -------------------------------------------------------- 5: FULL SPEC

boolean onSpecPut(uint8_t* data, uint16_t len);
boolean onSpecQuery(void);
Endpoint* specQueryEP = osap->endpoint(onSpecPut, onSpecQuery);

boolean onSpecPut(uint8_t* data, uint16_t len){
  return true;
}

boolean onSpecQuery(void){
  // want:
  uint16_t encoder_reading = step_cl->get_last_enc_reading();
  float angle_reading = step_cl->get_last_pos_reading();
  float posn = step_cl->get_last_pos_est();
  float angle_dot = step_cl->get_last_pos_dot();
  float effort = step_cl->get_output_effort();
  // ship,
  uint8_t data[18];
  uint16_t wptr = 0;
  ts_writeUint16(encoder_reading, data, &wptr);
  ts_writeFloat32(angle_reading, data, &wptr);
  ts_writeFloat32(posn, data, &wptr);
  ts_writeFloat32(angle_dot, data, &wptr);
  ts_writeFloat32(effort, data, &wptr);
  specQueryEP->write(data, 18);
  return true;
}

// -------------------------------------------------------- 6: PID Settings

boolean onPIDPut(uint8_t* data, uint16_t len){
  uint16_t ptr = 0;
  chunk_float32 pc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
  chunk_float32 ic = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
  chunk_float32 dc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
  step_cl->set_pid_values(pc.f, ic.f, dc.f);
  return true;
}

Endpoint* pidEP = osap->endpoint(onPIDPut);

// -------------------------------------------------------- 7: Filter Settings

boolean onAlphaPut(uint8_t* data, uint16_t len){
  uint16_t ptr = 0;
  chunk_float32 ac = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
  chunk_float32 adc = { .bytes = { data[ptr ++], data[ptr ++], data[ptr ++], data[ptr ++] }};
  step_cl->set_alpha_values(ac.f, adc.f);
  return true;
}

Endpoint* alphaEP = osap->endpoint(onAlphaPut);

// -------------------------------------------------------- SETUP / RUNTIME 

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

// async loop 
void loop() {
  osap->loop();
  step_a4950->dacRefresh();
} // end loop 

// usb packets 
void OSAP::handleAppPacket(uint8_t* pck, uint16_t ptr, pckm_t* pckm){
  // always clear this 
  pckm->vpa->clear(pckm->location);
}

#ifdef UCBUS_IS_DROP

// on words rx'd from bus, 
void UCBus_Drop::onRxISR(void){

}

// on timed (interrupt) rx of bus packet, channel A 
// this is where we will eventually read-in positions, and target them
void UCBus_Drop::onPacketARx(void){
  
}

#endif 

/*
    case AK_READ_MAG:{
      ptr ++;
      reply[rl ++] = AK_READ_MAG;
      enc_as5047->read_enc_diagnostics();
      uint16_t mag = enc_as5047->read_field_mag();
      ts_writeUint16(mag, reply, &rl);
      break;
    }
    case AK_READ_DIAG:
      ptr ++;
      reply[rl ++] = AK_READ_DIAG;
      ts_writeString(enc_as5047->read_enc_diagnostics(), reply, &rl);
      break;
    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;
    case AK_SET_TC: {
        ptr ++;
        reply[rl ++] = AK_SET_TC;
        chunk_float32 tcs = { .bytes = { pck[ptr ++], pck[ptr ++], pck[ptr ++], pck[ptr ++] }};
        step_cl->set_torque(tcs.f);
        float pos_est = step_cl->get_last_pos_est();
        float last_reading = step_cl->get_last_reading();
        ts_writeFloat32(pos_est, reply, &rl);
        ts_writeFloat32(last_reading, reply, &rl);
        break;
      }
*/