things / stepper-hbridge-xiao

README

H-Bridge Stepper

Software

/*
stepper.js
a "virtual thing" - of course
Jake Read, Leo McElroy and Quentin Bolsee at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2022
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the open systems assembly protocol (OSAP) and modular-things projects.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/

// ---------------------------------------------- serialize, deserialize floats 

import Serializers from "../../../src/lib/osapjs/utils/serializers"
import Thing from "../../../src/lib/thing"

export default class Stepper extends Thing {
  // how many steps-per-unit,
  // this could be included in a machineSpaceToActuatorSpace transform as well,
  private spu = 1;
  // these are stateful for modal functos 
  private lastVel = 100;               // units / sec 
  private lastAccel = 100;             // units / sec / sec
  // and we have an artificial current scaling, 
  private currentMax = 0.33;

  // we have getters and setters for these abs-max terms, 
  // which are also required by the coordinator, 
  private maxAccel = 1000;
  private maxVel = 1000;

  getMaxAccel(){ return this.maxAccel; }
  getMaxVelocity(){ return this.maxVel; }

  setMaxAccel(val: number){
    this.maxAccel = val;
  }

  setMaxVelocity(val: number){
    this.maxVel = val; 
  }


  // reset position (not a move command) 
  async setPosition(pos: number) {
    try {
      // stop AFAP 
      await this.stop();
      // write up a new-position-paquet,
      let datagram = new Uint8Array(4);
      let wptr = 0;
      wptr += Serializers.writeFloat32(datagram, wptr, pos);
      await this.send("setPosition", datagram);
    } catch (err) {
      console.error(err);
    }
  }


  setAccel(accel: number) {
    accel = Math.abs(accel);
    if(accel > this.maxAccel) this.maxAccel = accel;
    this.lastAccel = accel;
  }

  // hidden func for higher power sys, 
  setCurrentMaximum(cmax: number) {
    this.currentMax = cmax;
  }


  async setCurrent(cscale: number) {
    try {
      cscale = cscale * this.currentMax;
      let datagram = new Uint8Array(4);
      let wptr = 0;
      wptr += Serializers.writeFloat32(datagram, wptr, cscale);  // it's 0-1, the firmware checks
      // and we can shippity ship it,
      await this.send("writeSettings", datagram);
    } catch (err) {
      console.error(err);
    }
  }


  // tell me about your steps-per-unit,
  setStepsPerUnit(spu: number) {
    this.spu = spu;
  }

  // -------------------------------------------- Getters


  async getState() {
    try {
      let data = await this.send("getMotionStates", new Uint8Array([]))
      return {
        pos: Serializers.readFloat32(data, 0) / this.spu,
        vel: Serializers.readFloat32(data, 4) / this.spu,
        accel: Serializers.readFloat32(data, 8) / this.spu,
      }
    } catch (err) {
      console.error(err)
    }
  }

  async getPosition() { return (await this.getState()).pos; }

  async getVelocity() { return (await this.getState()).vel; }


  // -------------------------------------------- Operative


  // await no motion,
  async awaitMotionEnd() {
    try {
      return new Promise<void>(async (resolve, reject) => {
        let check = () => {
          this.getState().then((states) => {
            // console.log(`${this.name}\t acc ${states.accel.toFixed(4)},\t vel ${states.vel.toFixed(4)},\t pos ${states.pos.toFixed(4)}`)
            if (states.vel < 0.1 && states.vel > -0.1) {
              resolve()
            } else {
              setTimeout(check, 10)
            }
          }).catch((err) => { throw err })
        }
        check()
      })
    } catch (err) {
      console.error(err)
    }
  }


  // sets the position-target, and delivers rates, accels to use while slewing-to
  async target(pos: number, vel?: number, accel?: number) {
    try {
      // vel, accel are +ve always 
      vel = Math.abs(vel);
      accel = Math.abs(accel);
      // modal vel-and-accels, and guards
      vel ? this.lastVel = vel : vel = this.lastVel;
      if(vel > this.maxVel) this.maxVel = vel;
      accel ? this.lastAccel = accel : accel = this.lastAccel;
      // also, warn against zero-or-negative velocities & accelerations
      if (vel <= 0 || accel <= 0) throw new Error(`y'all are trying to go somewhere, but modal velocity or accel are negative, this won't do...`)
      // stuff a packet,
      let datagram = new Uint8Array(13)
      let wptr = 0
      datagram[wptr++] = 0 // MOTION_MODE_POS
      // write pos, vel, accel *every time* and convert-w-spu on the way out,
      wptr += Serializers.writeFloat32(datagram, wptr, pos * this.spu)  // write posn
      wptr += Serializers.writeFloat32(datagram, wptr, vel * this.spu)  // write max-vel-during
      wptr += Serializers.writeFloat32(datagram, wptr, accel * this.spu)  // write max-accel-during
      // and we can shippity ship it,
      await this.send("setTarget", datagram);
    } catch (err) {
      console.error(err)
    }
  }


  // goto-this-posn, using optional vel, accel, and wait for machine to get there
  async absolute(pos: number, vel: number, accel: number) {
    try {
      await this.target(pos, vel, accel)
      await this.awaitMotionEnd()
    } catch (err) {
      console.error(err)
    }
  } // end absolute


  // goto-relative, also wait,
  async relative(delta: number, vel: number, accel: number) {
    try {
      let state = await this.getState()
      let pos = delta + state.pos
      await this.absolute(pos, vel, accel)
    } catch (err) {
      console.error(err)
    }
  }


  // goto-this-speed, using optional accel,
  async velocity(vel: number, accel?: number) {
    try {
      accel = Math.abs(accel);
      // modal accel, and guards...
      accel ? this.lastAccel = accel : accel = this.lastAccel;
      if(Math.abs(vel) > this.maxVel) this.maxVel = Math.abs(vel);
      // note that we are *not* setting last-vel w/r/t this velocity... esp. since we often call this
      // w/ zero-vel, to stop...
      // now write the paquet,
      let datagram = new Uint8Array(9)
      let wptr = 0
      datagram[wptr++] = 1 // MOTION_MODE_VEL
      wptr += Serializers.writeFloat32(datagram, wptr, vel * this.spu)  // write max-vel-during
      wptr += Serializers.writeFloat32(datagram, wptr, accel * this.spu)  // write max-accel-during
      // mkheeeey
      await this.send("setTarget", datagram);
    } catch (err) {
      console.error(err)
    }
  }


  async stop() {
    try {
      await this.velocity(0)
      await this.awaitMotionEnd()
    } catch (err) {
      console.error(err)
    }
  }


  async getLimitState() {
    try {
      let reply = await this.send("getLimitState", new Uint8Array([0]));
      return reply[0] ? true : false;
    } catch (err) {
      console.error(err);
    }
  }

  // the gd API, we should be able to define 'em inline ? 
  public api = [
    {
      name: "absolute",
      args: [
        "pos: number",
        "vel?: number",
        "accel?: number"
      ]
    }, {
      name: "relative",
      args: [
        "delta: number",
        "vel?: number",
        "accel?: number"
      ]
    }, {
      name: "velocity",
      args: [
        "vel: number",
      ]
    }, {
      name: "setCurrent",
      args: [
        "cscale: number 0 - 1",
      ]
    }, {
      name: "setStepsPerUnit",
      args: [
        "spu: number",
      ]
    }, {
      name: "setAccel",
      args: [
        "accel: number",
      ]
    }, {
      name: "setPosition",
      args: [
        "pos: number"
      ]
    }, {
      name: "setMaxAccel",
      args: [
        "maxAccel: number"
      ]
    }, {
      name: "setMaxVelocity",
      args: [
        "maxVelocity: number"
      ]
    }, {
      name: "stop",
      args: []
    }, {
      name: "awaitMotionEnd",
      args: []
    }, {
      name: "getState",
      args: [],
      return: `
          {
            pos: number,
            vel: number,
            accel: number
          }
        `
    }, {
      name: "getLimitState",
      args: []
    }
  ]
}

Firmware

#include "motionStateMachine.h"
#include "stepperDriver.h"
#include <osap.h>

// using the RP2040 at 200MHz 

#define PIN_LIMIT 26 

// transport layer 
OSAP_Runtime osap;
OSAP_Gateway_USBSerial serLink(&Serial);
OSAP_Port_DeviceNames namePort("stepper");

// ---------------------------------------------- baby needs to serialize fluts 

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

float ts_readFloat32(unsigned char* buf, uint16_t* ptr){
  chunk_float32_stp chunk = { .bytes = { buf[(*ptr)], buf[(*ptr) + 1], buf[(*ptr) + 2], buf[(*ptr) + 3] } };
  (*ptr) += 4;
  return chunk.f;
}

void ts_writeFloat32(float val, volatile unsigned char* buf, uint16_t* ptr){
  chunk_float32_stp chunk;
  chunk.f = val;
  buf[(*ptr)] = chunk.bytes[0]; buf[(*ptr) + 1] = chunk.bytes[1]; buf[(*ptr) + 2] = chunk.bytes[2]; buf[(*ptr) + 3] = chunk.bytes[3];
  (*ptr) += 4;
}

// ---------------------------------------------- set a new target 

void setTarget(uint8_t* data, size_t len){
  uint16_t wptr = 1;
  // there's no value in getting clever here: we have two possible requests...
  if(data[0] == MOTION_MODE_POS){
    float targ = ts_readFloat32(data, &wptr);
    float maxVel = ts_readFloat32(data, &wptr);
    float maxAccel = ts_readFloat32(data, &wptr);
    motion_setPositionTarget(targ, maxVel, maxAccel);
  } else if (data[0] == MOTION_MODE_VEL){
    float targ = ts_readFloat32(data, &wptr);
    float maxAccel = ts_readFloat32(data, &wptr);
    motion_setVelocityTarget(targ, maxAccel);
  }
}

OSAP_Port_Named setTarget_port("setTarget", setTarget);

// ---------------------------------------------- get the current states 

size_t getMotionStates(uint8_t* data, size_t len, uint8_t* reply){
  motionState_t state;
  motion_getCurrentStates(&state);
  uint16_t wptr = 0;
  // in-fill current posn, velocity, and acceleration
  ts_writeFloat32(state.pos, reply, &wptr);
  ts_writeFloat32(state.vel, reply, &wptr);
  ts_writeFloat32(state.accel, reply, &wptr);
  // return the data length 
  return wptr;
}

OSAP_Port_Named getMotionStates_port("getMotionStates", getMotionStates);

// ---------------------------------------------- set a new position 

void setPosition(uint8_t* data, size_t len){
  // should do maxAccel, maxVel, and (optionally) setPosition
  // upstream should've though of this, so,
  uint16_t rptr = 0;
  float pos = ts_readFloat32(data, &rptr);
  motion_setPosition(pos);
}

OSAP_Port_Named setPosition_port("setPosition", setPosition);

// ---------------------------------------------- set..tings 

void writeSettings(uint8_t* data, size_t len){
  // it's just <cscale> for the time being,
  uint16_t rptr = 0;
  float cscale = ts_readFloat32(data, &rptr);
  // we get that as a floating p, 0-1, 
  // driver wants integers 0-1024: 
  uint32_t amp = cscale * 1024;
  stepper_setAmplitude(amp);
}

OSAP_Port_Named writeSettings_port("writeSettings", writeSettings);

// ---------------------------------------------- get the state of the limit switch

// this is lifted here, we set it periodically in the loop (to debounce) 
boolean lastButtonState = false;

size_t getLimitState(uint8_t* data, size_t len, uint8_t* reply){
  lastButtonState ? reply[0] = 1 : reply[0] = 0;
  return 1;
}

OSAP_Port_Named getLimitState_port("getLimitState", getLimitState);

void setup() {
  // startup the stepper hardware 
  stepper_init();
  // setup motion, pick an integration interval (us) 
  motion_init(64);
  // startup the network transporter 
  osap.begin();
  // and our limit pin, is wired (to spec)
  // to a normally-closed switch, from SIG to GND, 
  // meaning that when the switch is "clicked" - it will open, 
  // and the pullup will win, we will have logic high 
  pinMode(PIN_LIMIT, INPUT_PULLUP);
}

uint32_t debounceDelay = 1;
uint32_t lastButtonCheck = 0;

motionState_t states;

void loop() {
  // do transport stuff 
  osap.loop();

  // debounce and set button states,
  if(lastButtonCheck + debounceDelay < millis()){
    lastButtonCheck = millis();
    boolean newState = digitalRead(PIN_LIMIT);
    if(newState != lastButtonState){
      lastButtonState = newState;
    }
  }
}

// test randy set / res velocities, 
// if(lastFlip + flipInterval < millis()){
//   lastFlip = millis();
//   // pick a new flip interval, 
//   flipInterval = random(1000);
//   // and val, 
//   sampleVal = random(-1000, 1000);
//   // and coin-toss for vel or posn, 
//   uint32_t flip = random(0, 2);
//   if(flip == 1){
//     motion_setPositionTarget(sampleVal, 1000.0F, 4000.0F);
//   } else {
//     motion_setVelocityTarget(sampleVal, 5000.0F);
//   }
// }