Skip to content
Snippets Groups Projects
Commit aeebca46 authored by Jake Read's avatar Jake Read
Browse files

barebones motion / motor code

parent 23f44e5e
Branches
No related tags found
No related merge requests found
Showing
with 1569 additions and 0 deletions
File added
File added
from cobs import cobs
import serial
class CobsUsbSerial:
def __init__(self, port, baudrate=115200):
self.port = port
self.baudrate = baudrate
self.ser = serial.Serial(port, baudrate=baudrate, timeout=1)
def write(self, data: bytes):
data_enc = cobs.encode(data) + b"\x00"
self.ser.write(data_enc)
def read(self):
data_enc = self.ser.read_until(b"\x00")
data = cobs.decode(data_enc[:-1])
return data
from motor import Motor
import random
import time
home_rate = 100
home_accel = 2000
home_backoff_time = 0.5
go_rate = 150
go_accel = 2000
use_current = 0.2
y_motor = Motor("COM25")
print("y_motor: (1): ", y_motor.get_id())
x_bottom_motor = Motor("COM7")
print("x_bottom: (2): ", x_bottom_motor.get_id())
x_top_motor = Motor("COM28")
print("x_top: (3): ", x_top_motor.get_id())
# steps / unit,
y_motor.set_steps_per_unit(200 / 40)
x_bottom_motor.set_steps_per_unit(-200 /40)
x_top_motor.set_steps_per_unit(200 / 40)
# power up
y_motor.set_current(use_current)
x_bottom_motor.set_current(use_current)
x_top_motor.set_current(use_current)
# home Y ....
# if we're on the switch, backoff:
if y_motor.get_states()['limit'] > 0:
y_motor.set_target_velocity(home_rate, home_accel)
time.sleep(home_backoff_time)
y_motor.set_target_velocity(0, home_accel)
time.sleep(home_backoff_time)
# let's home it,
y_motor.set_target_velocity(-home_rate, home_accel)
# then check-wait on states,
while True:
states = y_motor.get_states()
if states['limit'] > 0:
break;
y_motor.set_target_velocity(0, home_accel)
# home X ...
if x_top_motor.get_states()['limit'] > 0:
x_bottom_motor.set_target_velocity(home_rate, home_accel)
x_top_motor.set_target_velocity(home_rate, home_accel)
time.sleep(home_backoff_time)
x_bottom_motor.set_target_velocity(0, home_accel)
x_top_motor.set_target_velocity(0, home_accel)
time.sleep(home_backoff_time)
x_bottom_motor.set_target_velocity(-home_rate, home_accel)
x_top_motor.set_target_velocity(-home_rate, home_accel)
while True:
states = x_top_motor.get_states()
if states['limit'] > 0:
break;
x_bottom_motor.set_target_velocity(0, home_accel)
x_top_motor.set_target_velocity(0, home_accel)
time.sleep(home_backoff_time)
# set zeroes,
x_bottom_motor.set_position(0)
x_top_motor.set_position(0)
y_motor.set_position(0)
# goto points
def goto(x, y, vel, accel):
x_bottom_motor.set_target_position_no_wait(x, vel, accel)
x_top_motor.set_target_position_no_wait(x, vel, accel)
y_motor.set_target_position_no_wait(y, vel, accel)
# # ... wait while not at point:
# while True:
# x_bot_state = x_bottom_motor.get_states()
goto(200, 100, go_rate, go_accel)
\ No newline at end of file
from cobs_usb_serial import CobsUsbSerial
import struct
MSG_GET_ID = 7
MSG_ECHO_TEST = 11
MSG_SET_TARG_VEL = 21
MSG_SET_TARG_POS = 22
MSG_SET_POSITION = 23
MSG_SET_CURRENT = 24
MSG_GET_STATES = 25
MSG_ACK = 31
MSG_NACK = 32
class Motor:
def __init__(self, port: str):
self.cobs = CobsUsbSerial(port)
self.spu = 200
def get_id(self):
msg = struct.pack('=B', MSG_GET_ID)
self.cobs.write(msg)
ack, device_id = struct.unpack('=BB', self.cobs.read())
return device_id
def set_steps_per_unit(self, spu):
self.spu = spu
def test(self) -> None:
test_bytes = bytes([MSG_ECHO_TEST, 14, 16])
print("testing w/ an echo-packet: ", [byte for byte in test_bytes])
self.cobs.write(test_bytes)
reply = self.cobs.read()
print("test echo returns: ", [byte for byte in reply])
def set_target_velocity(self, velocity: float, accel: float):
msg = struct.pack('=Bff', MSG_SET_TARG_VEL, velocity * self.spu, accel * self.spu)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def set_target_position_no_wait(self, position: float, velocity: float, accel: float):
msg = struct.pack('=Bfff', MSG_SET_TARG_POS, position * self.spu, velocity * self.spu, accel * self.spu)
self.cobs.write(msg)
return
def set_target_position(self, position: float, velocity: float, accel: float):
self.set_target_position_no_wait(position, velocity, accel)
return struct.unpack('=B', self.cobs.read())
def set_position(self, position: float):
msg = struct.pack('=Bf', MSG_SET_POSITION, position * self.spu)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def set_current(self, current: float):
msg = struct.pack('=Bf', MSG_SET_CURRENT, current)
self.cobs.write(msg)
return struct.unpack('=B', self.cobs.read())
def get_states(self):
self.cobs.write(bytes([MSG_GET_STATES]))
reply = self.cobs.read()
# print("states returns: ", [byte for byte in reply])
ack, position, velocity, acceleration, limit = struct.unpack('=BfffB', reply)
return {
'ack': ack,
'position': position / self.spu,
'velocity': velocity / self.spu,
'acceleration': acceleration / self.spu,
'limit': limit
}
\ No newline at end of file
import serial.tools.list_ports
def list_serial_ports():
ports = serial.tools.list_ports.comports()
for port in ports:
print(f"Port: {port.device}")
print(f" - Description: {port.description}")
if port.serial_number:
print(f" - Serial Number: {port.serial_number}")
if port.manufacturer:
print(f" - Manufacturer: {port.manufacturer}")
if port.product:
print(f" - Product: {port.product}")
if port.vid is not None:
print(f" - VID: {port.vid:04X}")
if port.pid is not None:
print(f" - PID: {port.pid:04X}")
print()
list_serial_ports()
{
"port": "COM7",
"configuration": "flash=2097152_0,freq=200,opt=Small,rtti=Disabled,stackprotect=Disabled,exceptions=Disabled,dbgport=Disabled,dbglvl=None,usbstack=picosdk,ipbtstack=ipv4only,uploadmethod=default",
"board": "rp2040:rp2040:seeed_xiao_rp2040",
"sketch": "simple-stepper.ino"
}
\ No newline at end of file
This diff is collapsed.
// link !
// TODO: pls use nanocobs, for encode- and decode-in-place, to save big (!) memory
// on new link layer... to fit into D11s...
#include "COBSUSBSerial.h"
#include "cobs.h"
#if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
COBSUSBSerial::COBSUSBSerial(SerialUSB* _usbcdc){
usbcdc = _usbcdc;
}
#else
COBSUSBSerial::COBSUSBSerial(Serial_* _usbcdc){
usbcdc = _usbcdc;
}
#endif
void COBSUSBSerial::begin(void){
usbcdc->begin(9600);
}
void COBSUSBSerial::loop(void){
// check RX side:
// while data & not-full,
while(usbcdc->available() && rxBufferLen == 0){
rxBuffer[rxBufferWp ++] = usbcdc->read();
if(rxBuffer[rxBufferWp - 1] == 0){
// decoding in place should always work: COBS doesn't revisit bytes
// encoding in place would be a different trick, and would require the use of
// nanocobs from this lad: https://github.com/charlesnicholson/nanocobs
size_t len = cobsDecode(rxBuffer, 255, rxBuffer);
// now we are with-packet, set length and reset write pointer
// len includes the trailing 0, rm that...
rxBufferLen = len - 1;
rxBufferWp = 0;
}
}
// check tx side,
while(txBufferLen && usbcdc->availableForWrite()){
// ship a byte,
usbcdc->write(txBuffer[txBufferRp ++]);
// if done, mark empty
if(txBufferRp >= txBufferLen){
txBufferLen = 0;
txBufferRp = 0;
}
}
}
size_t COBSUSBSerial::getPacket(uint8_t* dest){
if(rxBufferLen > 0){
memcpy(dest, rxBuffer, rxBufferLen);
size_t len = rxBufferLen;
rxBufferLen = 0;
return len;
} else {
return 0;
}
}
boolean COBSUSBSerial::clearToRead(void){
return (rxBufferLen > 0);
}
void COBSUSBSerial::send(uint8_t* packet, size_t len){
// ship it! blind!
size_t encodedLen = cobsEncode(packet, len, txBuffer);
// stuff 0 byte,
txBuffer[encodedLen] = 0;
txBufferLen = encodedLen + 1;
txBufferRp = 0;
}
boolean COBSUSBSerial::clearToSend(void){
// we're CTS if we have nothing in the outbuffer,
return (txBufferLen == 0);
}
// we should do some... work with this, i.e.
// keepalives, to detect if other-end is open or not...
boolean COBSUSBSerial::isOpen(void){
if(usbcdc){
return true;
} else {
return false;
}
}
\ No newline at end of file
// example cobs-encoded usb-serial link
#include <Arduino.h>
class COBSUSBSerial {
public:
#if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
COBSUSBSerial(SerialUSB* _usbcdc);
#else
COBSUSBSerial(Serial_* _usbcdc);
#endif
void begin(void);
void loop(void);
// check & read,
boolean clearToRead(void);
size_t getPacket(uint8_t* dest);
// clear ahead?
boolean clearToSend(void);
// open at all?
boolean isOpen(void);
// transmit a packet of this length
void send(uint8_t* packet, size_t len);
private:
#if defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_ARCH_RP2040)
SerialUSB* usbcdc = nullptr;
#else
Serial_* usbcdc = nullptr;
#endif
// buffer, write pointer, length,
uint8_t rxBuffer[255];
uint8_t rxBufferWp = 0;
uint8_t rxBufferLen = 0;
// ibid,
uint8_t txBuffer[255];
uint8_t txBufferRp = 0;
uint8_t txBufferLen = 0;
};
\ No newline at end of file
/*
utils/cobs.cpp
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2019
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the osap project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
#include "cobs.h"
// str8 crib from
// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
/** COBS encode data to buffer
@param data Pointer to input data to encode
@param length Number of bytes to encode
@param buffer Pointer to encoded output buffer
@return Encoded buffer length in bytes
@note doesn't write stop delimiter
*/
size_t cobsEncode(const void *data, size_t length, uint8_t *buffer){
uint8_t *encode = buffer; // Encoded byte pointer
uint8_t *codep = encode++; // Output code pointer
uint8_t code = 1; // Code value
for (const uint8_t *byte = (const uint8_t *)data; length--; ++byte){
if (*byte) // Byte not zero, write it
*encode++ = *byte, ++code;
if (!*byte || code == 0xff){ // Input is zero or block completed, restart
*codep = code, code = 1, codep = encode;
if (!*byte || length)
++encode;
}
}
*codep = code; // Write final code value
return encode - buffer;
}
/** COBS decode data from buffer
@param buffer Pointer to encoded input bytes
@param length Number of bytes to decode
@param data Pointer to decoded output data
@return Number of bytes successfully decoded
@note Stops decoding if delimiter byte is found
*/
size_t cobsDecode(const uint8_t *buffer, size_t length, void *data){
const uint8_t *byte = buffer; // Encoded input byte pointer
uint8_t *decode = (uint8_t *)data; // Decoded output byte pointer
for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block){
if (block) // Decode block byte
*decode++ = *byte++;
else
{
if (code != 0xff) // Encoded zero, write it
*decode++ = 0;
block = code = *byte++; // Next block length
if (code == 0x00) // Delimiter code found
break;
}
}
return decode - (uint8_t *)data;
}
/*
utils/cobs.h
consistent overhead byte stuffing implementation
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2019
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the osap project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
#ifndef UTIL_COBS_H_
#define UTIL_COBS_H_
#include <Arduino.h>
size_t cobsEncode(const void *data, size_t length, uint8_t *buffer);
size_t cobsDecode(const uint8_t *buffer, size_t length, void *data);
#endif
#include "fixedPointUtes.h"
// ---------------------------------------------- Fixed Point Maths
// FP 16.16 puts 65k ticks in front and in behind,
// for i.e. max rate of 32k 'units' (it's signed)
// which correlates to 9830 RPM for a 200-full-step stepper
const int32_t fp_scale = 16;
const float fp_float_max = 32768.0F;
const float fp_float_min = -32768.0F;
const int32_t fp_int_max = 32768;
const int32_t fp_int_min = -32768;
const int64_t fp_32b_max = 2147483648;
const int64_t fp_32b_min = -2147483648;
// hmmm https://www.youtube.com/watch?v=S12qx1DwjVk& at ~ 18:00
float fp_fixed32ToFloat(fpint32_t fixed){
return ((float)fixed / (float)(1 << fp_scale));
}
float fp_fixed64ToFloat(fpint64_t fixed){
return ((float)fixed / (float)(1 << fp_scale));
}
// actually this is unclear to me... https://www.youtube.com/watch?v=S12qx1DwjVk& at 16:57
fpint32_t fp_floatToFixed32(float flt){
if(flt > fp_float_max) flt = fp_float_max;
if(flt < fp_float_min) flt = fp_float_min;
return (flt * (float)(1 << fp_scale));
}
fpint64_t fp_floatToFixed64(float flt){
return (flt * (float)(1 << fp_scale));
}
int32_t fp_fixed32ToInt32(fpint32_t fixed){
return (fixed >> fp_scale);
}
fpint32_t fp_int32ToFixed32(int32_t inty){
if(inty > fp_int_max) inty = fp_int_max;
if(inty < fp_int_min) inty = fp_int_min;
return (inty << fp_scale);
}
fpint32_t fp_mult32x32(fpint32_t a, fpint32_t b){
// the result of this mult can be > our max possible 32b-wide fixedp,
int64_t res = ((int64_t)(a) * (int64_t)(b)) >> fp_scale;
// so we guard against that, since it is cast back to 32b on exit,
if(res > fp_32b_max) res = fp_32b_max;
if(res < fp_32b_min) res = fp_32b_min;
return res;
}
// we could instead do mult w/ some fancy shifting, for speed (no 64b at all)
// but it also makes for tricky overflow / etc; I'm not going to get into this yet
// https://www.youtube.com/watch?v=npQF28g6s_k& 7:40
// fp_int32_t fp_mult(fpint32_t a, fpint32_t b){
// return ((a >> 6) * (b >> 6)) >> 4;
// }
fpint32_t fp_div32x32(fpint32_t num, fpint32_t denum){
return ((int64_t)(num) << fp_scale) / denum;
}
fpint64_t fp_mult32x32_64(fpint32_t a, fpint32_t b){
return ((fpint64_t)(a) * (fpint64_t)(b)) >> fp_scale;
}
fpint64_t fp_div64x64(fpint64_t num, fpint64_t denum){
return (num << 16) / denum;
}
// // big-div,
// fpint64_t fp_calcStopDistance(fpint32_t _vel, fpint32_t _maxAccel){
// // return 0;
// int64_t _velSq = ((int64_t)(_vel) * (int64_t)(_vel)) >> fp_scale;
// int64_t _accelTwo = ((int64_t)(_maxAccel) * (int64_t)(fp_int32ToFixed32(2))) >> fp_scale;
// return (_velSq << fp_scale) / _accelTwo;
// }
\ No newline at end of file
#ifndef FIXEDPOINT_UTES_H_
#define FIXEDPOINT_UTES_H_
#include <Arduino.h>
// ---------------------------------------------- structs and interfaes
// we get explicit about fixed point
typedef int32_t fpint32_t;
typedef int64_t fpint64_t;
// ---------------------------------------------- fixedp conversions
float fp_fixed32ToFloat(fpint32_t fixed);
float fp_fixed64ToFloat(fpint64_t fixed);
fpint32_t fp_floatToFixed32(float flt);
fpint64_t fp_floatToFixed64(float flt);
int32_t fp_fixed32ToInt32(fpint32_t fixed);
fpint32_t fp_int32ToFixed32(int32_t inty);
// ---------------------------------------------- fixedp maths
fpint32_t fp_mult32x32(fpint32_t a, fpint32_t b);
fpint32_t fp_div32x32(fpint32_t num, fpint32_t denum);
// ---------------------------------------------- overflow fixedp maths
fpint64_t fp_mult32x32_64(fpint32_t a, fpint32_t b);
fpint64_t fp_div64x64(fpint64_t num, fpint64_t denum);
#endif
\ No newline at end of file
import fs from 'fs';
import process from 'process';
// Function to generate the sinusoid values
function generateSinusoid(length, amplitude) {
let values = [];
for (let i = 0; i < length; i++) {
let angle = (Math.PI * i) / (length - 1);
let value = Math.round(amplitude * Math.sin(angle));
values.push(value);
}
return values;
}
// Main function to create the lookup table
function createLookupTable(length, amplitude) {
const values = generateSinusoid(length, amplitude);
let lutString = `#ifndef STEPPER_LUT_H_\n#define STEPPER_LUT_H_\n\n`
lutString += `#include <Arduino.h>\n\n`;
lutString += `#define LUT_LENGTH ${length} \n`;
lutString += `#define PWM_PERIOD ${amplitude}\n\n`;
lutString += `const uint16_t LUT[LUT_LENGTH] = {\n`;
// do 'em row by row
let i = 0;
while (i < values.length) {
lutString += ` `;
for (let j = 0; j < 16; j++) {
lutString += `${values[i]}, `;
i++;
if (i >= values.length) break;
}
lutString += `\n`;
}
// lutString += values.join(',') + ',\n';
lutString += '};\n\n';
lutString += `#endif\n`
return lutString;
}
// Read command-line arguments
const length = parseInt(process.argv[2]);
const amplitude = parseInt(process.argv[3]);
// Validate arguments
if (isNaN(length) || isNaN(amplitude)) {
console.error('Usage: node script.js <length> <amplitude>');
process.exit(1);
}
// Generate the lookup table string
const lutString = createLookupTable(length, amplitude);
// Write to file
fs.writeFile('stepperLUT.h', lutString, (err) => {
if (err) {
console.error('Error writing to file:', err);
} else {
console.log('Lookup table written to stepperLUT.h');
}
});
#include "motionStateMachine.h"
#include "fixedPointUtes.h"
#include "stepperDriver.h"
// overlapping with the limit pin !
#define PIN_DEBUG 26
#define ALARM_DT_NUM 1
#define ALARM_DT_IRQ TIMER_IRQ_1
// _delT is re-calculated when we init w/ a new microsecondsPerIntegration
fpint32_t _delT = 0;
uint32_t _delT_us = 0;
// core states (units are full steps)
volatile uint8_t _mode = MOTION_MODE_VEL; // operative _mode
volatile fpint64_t _pos = 0; // current position (big 64)
volatile fpint32_t _vel = 0; // current velocity
volatile fpint32_t _accel = 0; // current acceleration
// init-once values we'll use in the integrator
volatile fpint32_t _delta = 0;
volatile fpint64_t _posTarget = 0; // for position control,
volatile fpint64_t _dist = 0; // for position control,
volatile fpint64_t _stopDistance = 0; // for position control,
// and settings
fpint32_t _maxAccel = 0; // maximum acceleration
fpint32_t _maxVelocity = 0;
// nasty !
fpint32_t _velEpsilon = 0;
void motion_init(uint32_t microsecondsPerIntegration){
// here's our delta-tee for each integration step
_delT = fp_floatToFixed32((float)(microsecondsPerIntegration) / 1000000.0F);
// and (for resetting our timer) the _delta directly in us
_delT_us = microsecondsPerIntegration;
// setup the hardware timer
hw_set_bits(&timer_hw->inte, 1u << ALARM_DT_NUM);
irq_set_exclusive_handler(ALARM_DT_IRQ, alarm_dt_handler);
irq_set_enabled(ALARM_DT_IRQ, true);
timer_hw->alarm[ALARM_DT_NUM] = (uint32_t) (timer_hw->timerawl + _delT_us);
// (optionally) use a debug pin to perf test
// pinMode(PIN_DEBUG, OUTPUT);
_velEpsilon = fp_floatToFixed32(1.0F);
}
void alarm_dt_handler(void){
// setup next call right away
hw_clear_bits(&timer_hw->intr, 1u << ALARM_DT_NUM);
timer_hw->alarm[ALARM_DT_NUM] = (uint32_t) (timer_hw->timerawl + _delT_us);
// do the motion system integration,
// sio_hw->gpio_set = (uint32_t)(1 << PIN_DEBUG);
motion_integrate();
// sio_hw->gpio_clr = (uint32_t)(1 << PIN_DEBUG);
}
// ------------------------------------ integrator codes
fpint64_t motion_calc_stopping_distance(fpint32_t vel, fpint32_t accel){
// our num / denum,
fpint64_t velSqrd = fp_mult32x32_64(vel, vel);
fpint64_t twoAccel = fp_mult32x32_64(fp_int32ToFixed32(2), accel);
// now we div that out,
return fp_div64x64(velSqrd, twoAccel);
}
void motion_calc_mode_velocity(void){
// go fast, or go slo;
if(_vel < _maxVelocity){
_accel = _maxAccel;
} else if (_vel > _maxVelocity){
_accel = -_maxAccel;
}
// using our chosen accel, integrate velocity from previous:
_vel += fp_mult32x32(_accel, _delT);
// and check against targets
// TODO: hack-ey velocity-at-target thing (1st if here)
// should dead-reckon step intervals, checking a delta,
// but requires that we check which direction we are going (towards velocity)
if( _maxVelocity < _velEpsilon && _maxVelocity > -_velEpsilon && // if we are near-zero target velocity,
_vel < _velEpsilon && _vel > -_velEpsilon // and we are also ... near that velocity,
){
_accel = 0;
_vel = 0;
} else if(_vel > 0 && _maxVelocity > 0 && _vel > _maxVelocity){
_accel = 0;
_vel = _maxVelocity;
} else if(_vel < 0 && _maxVelocity < 0 && _vel < _maxVelocity){
_accel = 0;
_vel = _maxVelocity;
}
}
void motion_calc_mode_position(void){
// figure whence we would need to start stopping, and our current dist
_stopDistance = motion_calc_stopping_distance(_vel, _maxAccel);
_dist = _posTarget - _pos;
// check if we're about to make it... bonk if so,
// units are steps, so epsilon is tiny !
if(abs(_dist - _delta) < fp_floatToFixed32(0.001F)){
_vel = 0;
_accel = 0;
return;
}
// now we do a buncha cheques
if(_stopDistance >= abs(_dist)){ // we're going to overshoot,
if(_vel <= 0){ // when -ve vel,
_accel = _maxAccel; // do +ve accel
} else { // when +ve vel,
_accel = -_maxAccel; // do -ve accel
}
} else { // we're not overshooting,
if(_dist > 0){ // if delta is positive,
_accel = _maxAccel; // go forwards,
} else { // if it's negative
_accel = -_maxAccel; // go backwards...
}
}
// using our chosen accel, integrate velocity from previous:
_vel += fp_mult32x32(_accel, _delT);
// cap our _vel based on maximum rates:
if(_vel >= _maxVelocity){
_accel = 0;
_vel = _maxVelocity;
} else if (_vel <= -_maxVelocity){
_accel = 0;
_vel = -_maxVelocity;
}
}
void motion_integrate(void){
// set our _accel based on modal requests,
switch(_mode){
case MOTION_MODE_POS:
motion_calc_mode_position();
break;
case MOTION_MODE_VEL:
motion_calc_mode_velocity();
break;
}
// grab a delta and integrate,
_delta = fp_mult32x32(_vel, _delT);
_pos += _delta;
// position is base 16, we can just shift our way to relative electrical phase
// 0-2048 (11 bits) per electrical phase, is
// 0-512 (9 bits) per step,
// 0b XXXXXXXXXXXXXXXX.XXXXXXXXXXXXXXXX
// 0b XX.XXXXXXXXX
// FS.MicroStep
uint16_t phaseAngle = (_pos >> 7) & 0b11111111111;
stepper_point(phaseAngle);
}
// ------------------------------------ end integrator
void motion_setVelocityTarget(float target, float maxAccel){
noInterrupts();
_maxAccel = fp_floatToFixed32(abs(maxAccel));
_maxVelocity = fp_floatToFixed32(target);
_mode = MOTION_MODE_VEL;
interrupts();
}
void motion_setPosition(float pos){
noInterrupts();
_pos = fp_floatToFixed64(pos);
interrupts();
}
void motion_getCurrentStates(motionState_t* statePtr){
noInterrupts();
statePtr->pos = fp_fixed64ToFloat(_pos);
statePtr->vel = fp_fixed32ToFloat(_vel);
statePtr->accel = fp_fixed32ToFloat(_accel);
interrupts();
}
void motion_setPositionTarget(float target, float maxVel, float maxAccel){
noInterrupts();
_posTarget = fp_floatToFixed64(target);
_maxVelocity = fp_floatToFixed32(abs(maxVel));
_maxAccel = fp_floatToFixed32(abs(maxAccel));
_mode = MOTION_MODE_POS;
interrupts();
}
// void motion_debug(void){
// // ... we could / should snapshot, if this is chunky ?
// noInterrupts();
// Serial.println(String(millis())
// // + "\t_delT: \t" + String(fp_fixed32ToFloat(_delT), 6)
// + "\tm: " + String(_mode)
// + "\tptrg: " + String(fp_fixed64ToFloat(_posTarget), 1)
// + "\tvtrg: " + String(fp_fixed64ToFloat(_maxVelocity), 1)
// + "\tpos: " + String(fp_fixed64ToFloat(_pos), 1)
// + "\tdst: " + String(fp_fixed64ToFloat(_dist), 1)
// + "\tstp: " + String(fp_fixed64ToFloat(_stopDistance), 1)
// + "\tacc: " + String(fp_fixed32ToFloat(_accel), 1)
// + "\tvel: " + String(fp_fixed32ToFloat(_vel), 1)
// // + "\tvtrg: \t" + String(fp_fixed32ToFloat(_maxVelocity), 4)
// );
// interrupts();
// }
#ifndef MOTION_STATE_MACHINE_H_
#define MOTION_STATE_MACHINE_H_
#include <Arduino.h>
#include <hardware/timer.h>
#include <hardware/irq.h>
#define MOTION_MODE_POS 0
#define MOTION_MODE_VEL 1
typedef struct motionState_t {
float pos;
float vel;
float accel;
} motionState_t;
void motion_init(uint32_t microsecondsPerIntegration);
void motion_integrate(void);
void alarm_dt_handler(void);
void motion_setPositionTarget(float target, float maxVel, float maxAccel);
void motion_setVelocityTarget(float target, float maxAccel);
void motion_setPosition(float pos);
void motion_getCurrentStates(motionState_t* statePtr);
#endif
#include "serializationUtes.h"
float ts_readFloat32(unsigned char* buf, uint16_t* ptr){
chunk_float32 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 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;
}
\ No newline at end of file
#ifndef SERIALIZATION_UTES_H_
#define SERIALIZATION_UTES_H_
#include <Arduino.h>
union chunk_float32 {
uint8_t bytes[4];
float f;
};
float ts_readFloat32(unsigned char* buf, uint16_t* ptr);
void ts_writeFloat32(float val, volatile unsigned char* buf, uint16_t* ptr);
#endif
\ No newline at end of file
#include "COBSUSBSerial.h"
#include "serializationUtes.h"
#include "motionStateMachine.h"
#include "stepperDriver.h"
// no-transport-layer stepper motor
// using the RP2040 at 200MHz
// using https://modular-things.github.io/modular-things/things/stepper-hbridge-xiao/
#define PIN_LIMIT 26
#define MSG_GET_ID 7
#define MSG_ECHO_TEST 11
#define MSG_SET_TARG_VEL 21
#define MSG_SET_TARG_POS 22
#define MSG_SET_POSITION 23
#define MSG_SET_CURRENT 24
#define MSG_GET_STATES 25
#define MSG_ACK 31
#define MSG_NACK 32
// y_motor: 1, x_bottom_motor: 2, x_top_motor: 3
#define MOTOR_ID 3
COBSUSBSerial cobs(&Serial);
void setTargetVelocity(uint8_t* data, size_t len){
uint16_t wptr = 0;
float targ = ts_readFloat32(data, &wptr);
float maxAccel = ts_readFloat32(data, &wptr);
motion_setVelocityTarget(targ, maxAccel);
}
void setTargetPosition(uint8_t* data, size_t len){
uint16_t wptr = 0;
float targ = ts_readFloat32(data, &wptr);
float maxVel = ts_readFloat32(data, &wptr);
float maxAccel = ts_readFloat32(data, &wptr);
motion_setPositionTarget(targ, maxVel, maxAccel);
}
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);
}
void setCurrent(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);
}
boolean lastButtonState = false;
size_t getStates(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);
// and limit state,
reply[wptr ++] = (lastButtonState ? 1 : 0);
// return the data length
return wptr;
}
void setup() {
// startup the stepper hardware
stepper_init();
// setup motion, pick an integration interval (us)
motion_init(64);
// 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);
// and our comms
cobs.begin();
}
uint32_t debounceDelay = 1;
uint32_t lastButtonCheck = 0;
motionState_t states;
uint8_t msg[256];
uint8_t reply[256];
void loop() {
// run comms
cobs.loop();
// and check for paquiats: condition is if we can
// get a packet, and tx the reply
if(cobs.clearToRead() && cobs.clearToSend()){
size_t len = cobs.getPacket(msg);
// start a reply,
size_t wptr = 0;
reply[wptr ++] = MSG_ACK;
// switch on grams,
switch(msg[0]){
case MSG_GET_ID:
reply[wptr ++] = MOTOR_ID;
break;
case MSG_ECHO_TEST:
memcpy(reply, msg, len);
wptr = len;
break;
case MSG_SET_TARG_VEL:
setTargetVelocity(msg + 1, len - 1);
break;
case MSG_SET_TARG_POS:
setTargetPosition(msg + 1, len - 1);
break;
case MSG_SET_POSITION:
setPosition(msg + 1, len - 1);
break;
case MSG_SET_CURRENT:
setCurrent(msg + 1, len - 1);
break;
case MSG_GET_STATES:
wptr += getStates(msg + 1, len - 1, reply + 1);
break;
default:
reply[0] = MSG_NACK;
break;
}
// send the reply,
cobs.send(reply, wptr);
}
// debounce and set button states,
if(lastButtonCheck + debounceDelay < millis()){
lastButtonCheck = millis();
boolean newState = digitalRead(PIN_LIMIT);
if(newState != lastButtonState){
lastButtonState = newState;
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment