Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • jakeread/displacementexercise
  • rcastro/ussm
  • palomagr/displacementexercise
3 results
Show changes
Showing
with 4387 additions and 0 deletions
/*
cobsserial.h
COBS delineated serial packets
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 squidworks and ponyo projects.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
#ifndef COBSSERIAL_H_
#define COBSSERIAL_H_
#include <arduino.h>
#include "cobs.h"
#include "syserror.h"
#include "./drivers/indicators.h"
#define VPUSB_SPACE_SIZE 1028
class COBSSerial {
private:
// unfortunately, looks like we need to write-in to temp,
// and decode out of that
uint8_t _encodedPacket[VPUSB_SPACE_SIZE];
uint8_t _packet[VPUSB_SPACE_SIZE];
uint16_t _pl = 0;
uint16_t _bwp = 0; // byte write pointer,
uint8_t _lastPacket = 0; // last packet written into
// outgoing cobs-copy-in,
uint8_t _encodedOut[VPUSB_SPACE_SIZE];
// this is just for debug,
uint8_t _ringPacket[VPUSB_SPACE_SIZE];
public:
COBSSerial();
// props
uint16_t maxSegLength = VPUSB_SPACE_SIZE - 6;
// code
void init(void);
void loop(void);
// handle incoming frames
boolean hasPacket(void); // check existence
void getPacket(uint8_t** pck, uint16_t* pl);
void clearPacket(void);
// dish outgoing frames, and check if cts
void sendPacket(uint8_t *pck, uint16_t pl);
};
#endif
#include "syserror.h"
uint8_t errBuf[1028];
uint8_t errEncoded[1028];
/*
boolean writeString(unsigned char* dest, uint16_t* dptr, String msg){
uint16_t len = msg.length();
dest[(*dptr) ++] = TS_STRING_KEY;
writeLenBytes(dest, dptr, len);
msg.getBytes(dest, len + 1);
return true;
}
boolean writeLenBytes(unsigned char* dest, uint16_t* dptr, uint16_t len){
dest[(*dptr) ++] = len;
dest[(*dptr) ++] = (len >> 8) & 255;
return true;
}
*/
// config-your-own-ll-escape-hatch
void debugmsg(String msg){
uint32_t len = msg.length();
errBuf[0] = DEXKEY_DEBUGMSG; // the ll-errmsg-key
errBuf[1] = len & 255;
errBuf[2] = (len >> 8) & 255;
errBuf[3] = (len >> 16) & 255;
errBuf[4] = (len >> 24) & 255;
msg.getBytes(&errBuf[5], len + 1);
size_t ecl = cobsEncode(errBuf, len + 5, errEncoded);
if(Serial.availableForWrite() > (int64_t)ecl){
Serial.write(errEncoded, ecl);
Serial.flush();
}
}
#ifndef SYSERROR_H_
#define SYSERROR_H_
#include <arduino.h>
#include "./drivers/indicators.h"
#include "cobs.h"
#include "ts.h"
#define DEXKEY_DEBUGMSG 11
void debugmsg(String msg);
#endif
/*
osap/ts.cpp
typeset / keys / writing / reading
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 squidworks and ponyo projects.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
#include "ts.h"
void ts_writeBoolean(boolean val, unsigned char *buf, uint16_t *ptr){
if(val){
buf[(*ptr) ++] = 1;
} else {
buf[(*ptr) ++] = 0;
}
}
void ts_readUint16(uint16_t *val, unsigned char *buf, uint16_t *ptr){
*val = buf[(*ptr) + 1] << 8 | buf[(*ptr)];
*ptr += 2;
}
void ts_writeUint16(uint16_t val, unsigned char *buf, uint16_t *ptr){
buf[(*ptr) ++] = val & 255;
buf[(*ptr) ++] = (val >> 8) & 255;
}
void ts_writeUint32(uint32_t val, unsigned char *buf, uint16_t *ptr){
buf[(*ptr) ++] = val & 255;
buf[(*ptr) ++] = (val >> 8) & 255;
buf[(*ptr) ++] = (val >> 16) & 255;
buf[(*ptr) ++] = (val >> 24) & 255;
}
// pls recall the union,
union chunk_float32 {
uint8_t bytes[4];
float f;
};
/*
chunk_float32 dur_chunk = { .bytes = { bPck[ptr ++], bPck[ptr ++], bPck[ptr ++], bPck[ptr ++] } } ;
float duration = dur_chunk.f;
*/
union chunk_int32 {
uint8_t bytes[4];
int32_t num;
};
void ts_readInt32(int32_t *val, unsigned char *buf, uint16_t *ptr){
chunk_int32 wc = { .bytes = { buf[(*ptr) ++], buf[(*ptr) ++], buf[(*ptr) ++], buf[(*ptr) ++] }};
*val = wc.num;
}
void ts_writeInt32(int32_t val, unsigned char *buf, uint16_t *ptr){
chunk_int32 wc = { .num = val };
buf[(*ptr) ++] = wc.bytes[0];
buf[(*ptr) ++] = wc.bytes[1];
buf[(*ptr) ++] = wc.bytes[2];
buf[(*ptr) ++] = wc.bytes[3];
}
void ts_writeString(String val, unsigned char *buf, uint16_t *ptr){
uint32_t len = val.length();
buf[(*ptr) ++] = len & 255;
buf[(*ptr) ++] = (len >> 8) & 255;
buf[(*ptr) ++] = (len >> 16) & 255;
buf[(*ptr) ++] = (len >> 24) & 255;
val.getBytes(&buf[*ptr], len + 1);
*ptr += len;
}
/*
osap/ts.h
typeset / keys / writing / reading
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 squidworks and ponyo projects.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
#include <arduino.h>
// -------------------------------------------------------- Reading and Writing
void ts_writeBoolean(boolean val, unsigned char *buf, uint16_t *ptr);
void ts_readUint16(uint16_t *val, uint8_t *buf, uint16_t *ptr);
void ts_writeUint16(uint16_t val, unsigned char *buf, uint16_t *ptr);
void ts_writeUint32(uint32_t val, unsigned char *buf, uint16_t *ptr);
void ts_readInt32(int32_t *val, unsigned char *buf, uint16_t *ptr);
void ts_writeInt32(int32_t val, unsigned char *buf, uint16_t *ptr);
void ts_writeString(String val, unsigned char *buf, uint16_t *ptr);
// for feather M4 express
#define CLKLIGHT_BM (uint32_t)(1 << 0)
#define CLKLIGHT_ON digitalWrite(13, HIGH)
#define CLKLIGHT_OFF digitalWrite(13, LOW)
#define CLKLIGHT_TOGGLE digitalWrite(13, !digitalRead(13))
#define CLKLIGHT_SETUP pinMode(13, OUTPUT)
//
// there's a neopixel on the board that could be used for this,
// maybe later
#define ERRLIGHT_BM (uint32_t)(1 << 0)
#define ERRLIGHT_ON
#define ERRLIGHT_OFF
#define ERRLIGHT_TOGGLE
#define ERRLIGHT_SETUP
#ifndef PERIPHERAL_NUMS_H_
#define PERIPHERAL_NUMS_H_
#define PERIPHERAL_A 0
#define PERIPHERAL_B 1
#define PERIPHERAL_C 2
#define PERIPHERAL_D 3
#define PERIPHERAL_E 4
#define PERIPHERAL_F 5
#define PERIPHERAL_G 6
#define PERIPHERAL_H 7
#define PERIPHERAL_I 8
#define PERIPHERAL_K 9
#define PERIPHERAL_L 10
#define PERIPHERAL_M 11
#define PERIPHERAL_N 12
#endif
\ No newline at end of file
// AccelStepper.cpp
//
// Copyright (C) 2009-2013 Mike McCauley
// $Id: AccelStepper.cpp,v 1.24 2020/04/20 00:15:03 mikem Exp mikem $
#include "AccelStepper.h"
#if 0
// Some debugging assistance
void dump(uint8_t* p, int l)
{
int i;
for (i = 0; i < l; i++)
{
Serial.print(p[i], HEX);
Serial.print(" ");
}
Serial.println("");
}
#endif
void AccelStepper::moveTo(long absolute)
{
if (_targetPos != absolute)
{
_targetPos = absolute;
computeNewSpeed();
// compute new n?
}
}
void AccelStepper::move(long relative)
{
moveTo(_currentPos + relative);
}
// Implements steps according to the current step interval
// You must call this at least once per step
// returns true if a step occurred
boolean AccelStepper::runSpeed()
{
// Dont do anything unless we actually have a step interval
if (!_stepInterval)
return false;
unsigned long time = micros();
if (time - _lastStepTime >= _stepInterval)
{
if (_direction == DIRECTION_CW)
{
// Clockwise
_currentPos += 1;
}
else
{
// Anticlockwise
_currentPos -= 1;
}
step(_currentPos);
_lastStepTime = time; // Caution: does not account for costs in step()
return true;
}
else
{
return false;
}
}
long AccelStepper::distanceToGo()
{
return _targetPos - _currentPos;
}
long AccelStepper::targetPosition()
{
return _targetPos;
}
long AccelStepper::currentPosition()
{
return _currentPos;
}
// Useful during initialisations or after initial positioning
// Sets speed to 0
void AccelStepper::setCurrentPosition(long position)
{
_targetPos = _currentPos = position;
_n = 0;
_stepInterval = 0;
_speed = 0.0;
}
void AccelStepper::computeNewSpeed()
{
long distanceTo = distanceToGo(); // +ve is clockwise from curent location
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
if (distanceTo == 0 && stepsToStop <= 1)
{
// We are at the target and its time to stop
_stepInterval = 0;
_speed = 0.0;
_n = 0;
return;
}
if (distanceTo > 0)
{
// We are anticlockwise from the target
// Need to go clockwise from here, maybe decelerate now
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
_n = -_n; // Start accceleration
}
}
else if (distanceTo < 0)
{
// We are clockwise from the target
// Need to go anticlockwise from here, maybe decelerate
if (_n > 0)
{
// Currently accelerating, need to decel now? Or maybe going the wrong way?
if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
_n = -stepsToStop; // Start deceleration
}
else if (_n < 0)
{
// Currently decelerating, need to accel again?
if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
_n = -_n; // Start accceleration
}
}
// Need to accelerate or decelerate
if (_n == 0)
{
// First step from stopped
_cn = _c0;
_direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
}
else
{
// Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
_cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
_cn = max(_cn, _cmin);
}
_n++;
_stepInterval = _cn;
_speed = 1000000.0 / _cn;
if (_direction == DIRECTION_CCW)
_speed = -_speed;
#if 0
Serial.println(_speed);
Serial.println(_acceleration);
Serial.println(_cn);
Serial.println(_c0);
Serial.println(_n);
Serial.println(_stepInterval);
Serial.println(distanceTo);
Serial.println(stepsToStop);
Serial.println("-----");
#endif
}
// Run the motor to implement speed and acceleration in order to proceed to the target position
// You must call this at least once per step, preferably in your main loop
// If the motor is in the desired position, the cost is very small
// returns true if the motor is still running to the target position.
boolean AccelStepper::run()
{
if (runSpeed())
computeNewSpeed();
return _speed != 0.0 || distanceToGo() != 0;
}
AccelStepper::AccelStepper(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable)
{
_interface = interface;
_currentPos = 0;
_targetPos = 0;
_speed = 0.0;
_maxSpeed = 1.0;
_acceleration = 0.0;
_sqrt_twoa = 1.0;
_stepInterval = 0;
_minPulseWidth = 1;
_enablePin = 0xff;
_lastStepTime = 0;
_pin[0] = pin1;
_pin[1] = pin2;
_pin[2] = pin3;
_pin[3] = pin4;
_enableInverted = false;
// NEW
_n = 0;
_c0 = 0.0;
_cn = 0.0;
_cmin = 1.0;
_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
_pinInverted[i] = 0;
if (enable)
enableOutputs();
// Some reasonable default
setAcceleration(1);
}
AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
{
_interface = 0;
_currentPos = 0;
_targetPos = 0;
_speed = 0.0;
_maxSpeed = 1.0;
_acceleration = 0.0;
_sqrt_twoa = 1.0;
_stepInterval = 0;
_minPulseWidth = 1;
_enablePin = 0xff;
_lastStepTime = 0;
_pin[0] = 0;
_pin[1] = 0;
_pin[2] = 0;
_pin[3] = 0;
_forward = forward;
_backward = backward;
// NEW
_n = 0;
_c0 = 0.0;
_cn = 0.0;
_cmin = 1.0;
_direction = DIRECTION_CCW;
int i;
for (i = 0; i < 4; i++)
_pinInverted[i] = 0;
// Some reasonable default
setAcceleration(1);
}
void AccelStepper::setMaxSpeed(float speed)
{
if (speed < 0.0)
speed = -speed;
if (_maxSpeed != speed)
{
_maxSpeed = speed;
_cmin = 1000000.0 / speed;
// Recompute _n from current speed and adjust speed if accelerating or cruising
if (_n > 0)
{
_n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
computeNewSpeed();
}
}
}
float AccelStepper::maxSpeed()
{
return _maxSpeed;
}
void AccelStepper::setAcceleration(float acceleration)
{
if (acceleration == 0.0)
return;
if (acceleration < 0.0)
acceleration = -acceleration;
if (_acceleration != acceleration)
{
// Recompute _n per Equation 17
_n = _n * (_acceleration / acceleration);
// New c0 per Equation 7, with correction per Equation 15
_c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15
_acceleration = acceleration;
computeNewSpeed();
}
}
void AccelStepper::setSpeed(float speed)
{
if (speed == _speed)
return;
speed = constrain(speed, -_maxSpeed, _maxSpeed);
if (speed == 0.0)
_stepInterval = 0;
else
{
_stepInterval = fabs(1000000.0 / speed);
_direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
}
_speed = speed;
}
float AccelStepper::speed()
{
return _speed;
}
// Subclasses can override
void AccelStepper::step(long step)
{
switch (_interface)
{
case FUNCTION:
step0(step);
break;
case DRIVER:
step1(step);
break;
case FULL2WIRE:
step2(step);
break;
case FULL3WIRE:
step3(step);
break;
case FULL4WIRE:
step4(step);
break;
case HALF3WIRE:
step6(step);
break;
case HALF4WIRE:
step8(step);
break;
}
}
// You might want to override this to implement eg serial output
// bit 0 of the mask corresponds to _pin[0]
// bit 1 of the mask corresponds to _pin[1]
// ....
void AccelStepper::setOutputPins(uint8_t mask)
{
uint8_t numpins = 2;
if (_interface == FULL4WIRE || _interface == HALF4WIRE)
numpins = 4;
else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
numpins = 3;
uint8_t i;
for (i = 0; i < numpins; i++)
digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i]));
}
// 0 pin step function (ie for functional usage)
void AccelStepper::step0(long step)
{
(void)(step); // Unused
if (_speed > 0)
_forward();
else
_backward();
}
// 1 pin step function (ie for stepper drivers)
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step1(long step)
{
(void)(step); // Unused
// _pin[0] is step, _pin[1] is direction
setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses
setOutputPins(_direction ? 0b11 : 0b01); // step HIGH
// Caution 200ns setup time
// Delay the minimum allowed pulse width
delayMicroseconds(_minPulseWidth);
setOutputPins(_direction ? 0b10 : 0b00); // step LOW
}
// 2 pin step function
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step2(long step)
{
switch (step & 0x3)
{
case 0: /* 01 */
setOutputPins(0b10);
break;
case 1: /* 11 */
setOutputPins(0b11);
break;
case 2: /* 10 */
setOutputPins(0b01);
break;
case 3: /* 00 */
setOutputPins(0b00);
break;
}
}
// 3 pin step function
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step3(long step)
{
switch (step % 3)
{
case 0: // 100
setOutputPins(0b100);
break;
case 1: // 001
setOutputPins(0b001);
break;
case 2: //010
setOutputPins(0b010);
break;
}
}
// 4 pin step function for half stepper
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step4(long step)
{
switch (step & 0x3)
{
case 0: // 1010
setOutputPins(0b0101);
break;
case 1: // 0110
setOutputPins(0b0110);
break;
case 2: //0101
setOutputPins(0b1010);
break;
case 3: //1001
setOutputPins(0b1001);
break;
}
}
// 3 pin half step function
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step6(long step)
{
switch (step % 6)
{
case 0: // 100
setOutputPins(0b100);
break;
case 1: // 101
setOutputPins(0b101);
break;
case 2: // 001
setOutputPins(0b001);
break;
case 3: // 011
setOutputPins(0b011);
break;
case 4: // 010
setOutputPins(0b010);
break;
case 5: // 011
setOutputPins(0b110);
break;
}
}
// 4 pin half step function
// This is passed the current step number (0 to 7)
// Subclasses can override
void AccelStepper::step8(long step)
{
switch (step & 0x7)
{
case 0: // 1000
setOutputPins(0b0001);
break;
case 1: // 1010
setOutputPins(0b0101);
break;
case 2: // 0010
setOutputPins(0b0100);
break;
case 3: // 0110
setOutputPins(0b0110);
break;
case 4: // 0100
setOutputPins(0b0010);
break;
case 5: //0101
setOutputPins(0b1010);
break;
case 6: // 0001
setOutputPins(0b1000);
break;
case 7: //1001
setOutputPins(0b1001);
break;
}
}
// Prevents power consumption on the outputs
void AccelStepper::disableOutputs()
{
if (! _interface) return;
setOutputPins(0); // Handles inversion automatically
if (_enablePin != 0xff)
{
pinMode(_enablePin, OUTPUT);
digitalWrite(_enablePin, LOW ^ _enableInverted);
}
}
void AccelStepper::enableOutputs()
{
if (! _interface)
return;
pinMode(_pin[0], OUTPUT);
pinMode(_pin[1], OUTPUT);
if (_interface == FULL4WIRE || _interface == HALF4WIRE)
{
pinMode(_pin[2], OUTPUT);
pinMode(_pin[3], OUTPUT);
}
else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
{
pinMode(_pin[2], OUTPUT);
}
if (_enablePin != 0xff)
{
pinMode(_enablePin, OUTPUT);
digitalWrite(_enablePin, HIGH ^ _enableInverted);
}
}
void AccelStepper::setMinPulseWidth(unsigned int minWidth)
{
_minPulseWidth = minWidth;
}
void AccelStepper::setEnablePin(uint8_t enablePin)
{
_enablePin = enablePin;
// This happens after construction, so init pin now.
if (_enablePin != 0xff)
{
pinMode(_enablePin, OUTPUT);
digitalWrite(_enablePin, HIGH ^ _enableInverted);
}
}
void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
{
_pinInverted[0] = stepInvert;
_pinInverted[1] = directionInvert;
_enableInverted = enableInvert;
}
void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
{
_pinInverted[0] = pin1Invert;
_pinInverted[1] = pin2Invert;
_pinInverted[2] = pin3Invert;
_pinInverted[3] = pin4Invert;
_enableInverted = enableInvert;
}
// Blocks until the target position is reached and stopped
void AccelStepper::runToPosition()
{
while (run())
YIELD; // Let system housekeeping occur
}
boolean AccelStepper::runSpeedToPosition()
{
if (_targetPos == _currentPos)
return false;
if (_targetPos >_currentPos)
_direction = DIRECTION_CW;
else
_direction = DIRECTION_CCW;
return runSpeed();
}
// Blocks until the new target position is reached
void AccelStepper::runToNewPosition(long position)
{
moveTo(position);
runToPosition();
}
void AccelStepper::stop()
{
if (_speed != 0.0)
{
long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding)
if (_speed > 0)
move(stepsToStop);
else
move(-stepsToStop);
}
}
bool AccelStepper::isRunning()
{
return !(_speed == 0.0 && _targetPos == _currentPos);
}
This diff is collapsed.
/**
*
* HX711 library for Arduino
* https://github.com/bogde/HX711
*
* MIT License
* (c) 2018 Bogdan Necula
*
**/
#include <arduino.h>
#include "HX711.h"
// TEENSYDUINO has a port of Dean Camera's ATOMIC_BLOCK macros for AVR to ARM Cortex M3.
#define HAS_ATOMIC_BLOCK (defined(ARDUINO_ARCH_AVR) || defined(TEENSYDUINO))
// Whether we are running on either the ESP8266 or the ESP32.
#define ARCH_ESPRESSIF (defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32))
// Whether we are actually running on FreeRTOS.
#define IS_FREE_RTOS defined(ARDUINO_ARCH_ESP32)
// Define macro designating whether we're running on a reasonable
// fast CPU and so should slow down sampling from GPIO.
#define FAST_CPU \
( \
ARCH_ESPRESSIF || \
defined(ARDUINO_ARCH_SAM) || defined(ARDUINO_ARCH_SAMD) || \
defined(ARDUINO_ARCH_STM32) || defined(TEENSYDUINO) \
)
#if HAS_ATOMIC_BLOCK
// Acquire AVR-specific ATOMIC_BLOCK(ATOMIC_RESTORESTATE) macro.
#include <util/atomic.h>
#endif
#if FAST_CPU
// Make shiftIn() be aware of clockspeed for
// faster CPUs like ESP32, Teensy 3.x and friends.
// See also:
// - https://github.com/bogde/HX711/issues/75
// - https://github.com/arduino/Arduino/issues/6561
// - https://community.hiveeyes.org/t/using-bogdans-canonical-hx711-library-on-the-esp32/539
uint8_t shiftInSlow(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder) {
uint8_t value = 0;
uint8_t i;
for(i = 0; i < 8; ++i) {
digitalWrite(clockPin, HIGH);
delayMicroseconds(1);
if(bitOrder == LSBFIRST)
value |= digitalRead(dataPin) << i;
else
value |= digitalRead(dataPin) << (7 - i);
digitalWrite(clockPin, LOW);
delayMicroseconds(1);
}
return value;
}
#define SHIFTIN_WITH_SPEED_SUPPORT(data,clock,order) shiftInSlow(data,clock,order)
#else
#define SHIFTIN_WITH_SPEED_SUPPORT(data,clock,order) shiftIn(data,clock,order)
#endif
HX711::HX711() {
}
HX711::~HX711() {
}
void HX711::begin(byte dout, byte pd_sck, byte gain) {
PD_SCK = pd_sck;
DOUT = dout;
pinMode(PD_SCK, OUTPUT);
pinMode(DOUT, INPUT);
set_gain(gain);
}
bool HX711::is_ready() {
return digitalRead(DOUT) == LOW;
}
void HX711::set_gain(byte gain) {
switch (gain) {
case 128: // channel A, gain factor 128
GAIN = 1;
break;
case 64: // channel A, gain factor 64
GAIN = 3;
break;
case 32: // channel B, gain factor 32
GAIN = 2;
break;
}
digitalWrite(PD_SCK, LOW);
read();
}
long HX711::read() {
// Wait for the chip to become ready.
wait_ready();
// Define structures for reading data into.
unsigned long value = 0;
uint8_t data[3] = { 0 };
uint8_t filler = 0x00;
// Protect the read sequence from system interrupts. If an interrupt occurs during
// the time the PD_SCK signal is high it will stretch the length of the clock pulse.
// If the total pulse time exceeds 60 uSec this will cause the HX711 to enter
// power down mode during the middle of the read sequence. While the device will
// wake up when PD_SCK goes low again, the reset starts a new conversion cycle which
// forces DOUT high until that cycle is completed.
//
// The result is that all subsequent bits read by shiftIn() will read back as 1,
// corrupting the value returned by read(). The ATOMIC_BLOCK macro disables
// interrupts during the sequence and then restores the interrupt mask to its previous
// state after the sequence completes, insuring that the entire read-and-gain-set
// sequence is not interrupted. The macro has a few minor advantages over bracketing
// the sequence between `noInterrupts()` and `interrupts()` calls.
#if HAS_ATOMIC_BLOCK
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
#elif IS_FREE_RTOS
// Begin of critical section.
// Critical sections are used as a valid protection method
// against simultaneous access in vanilla FreeRTOS.
// Disable the scheduler and call portDISABLE_INTERRUPTS. This prevents
// context switches and servicing of ISRs during a critical section.
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
portENTER_CRITICAL(&mux);
#else
// Disable interrupts.
noInterrupts();
#endif
// Pulse the clock pin 24 times to read the data.
data[2] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST);
data[1] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST);
data[0] = SHIFTIN_WITH_SPEED_SUPPORT(DOUT, PD_SCK, MSBFIRST);
// Set the channel and the gain factor for the next reading using the clock pin.
for (unsigned int i = 0; i < GAIN; i++) {
digitalWrite(PD_SCK, HIGH);
#if ARCH_ESPRESSIF
delayMicroseconds(1);
#endif
digitalWrite(PD_SCK, LOW);
#if ARCH_ESPRESSIF
delayMicroseconds(1);
#endif
}
#if IS_FREE_RTOS
// End of critical section.
portEXIT_CRITICAL(&mux);
#elif HAS_ATOMIC_BLOCK
}
#else
// Enable interrupts again.
interrupts();
#endif
// Replicate the most significant bit to pad out a 32-bit signed integer
if (data[2] & 0x80) {
filler = 0xFF;
} else {
filler = 0x00;
}
// Construct a 32-bit signed integer
value = ( static_cast<unsigned long>(filler) << 24
| static_cast<unsigned long>(data[2]) << 16
| static_cast<unsigned long>(data[1]) << 8
| static_cast<unsigned long>(data[0]) );
return static_cast<long>(value);
}
void HX711::wait_ready(unsigned long delay_ms) {
// Wait for the chip to become ready.
// This is a blocking implementation and will
// halt the sketch until a load cell is connected.
while (!is_ready()) {
// Probably will do no harm on AVR but will feed the Watchdog Timer (WDT) on ESP.
// https://github.com/bogde/HX711/issues/73
delay(delay_ms);
}
}
bool HX711::wait_ready_retry(int retries, unsigned long delay_ms) {
// Wait for the chip to become ready by
// retrying for a specified amount of attempts.
// https://github.com/bogde/HX711/issues/76
int count = 0;
while (count < retries) {
if (is_ready()) {
return true;
}
delay(delay_ms);
count++;
}
return false;
}
bool HX711::wait_ready_timeout(unsigned long timeout, unsigned long delay_ms) {
// Wait for the chip to become ready until timeout.
// https://github.com/bogde/HX711/pull/96
unsigned long millisStarted = millis();
while (millis() - millisStarted < timeout) {
if (is_ready()) {
return true;
}
delay(delay_ms);
}
return false;
}
long HX711::read_average(byte times) {
long sum = 0;
for (byte i = 0; i < times; i++) {
sum += read();
// Probably will do no harm on AVR but will feed the Watchdog Timer (WDT) on ESP.
// https://github.com/bogde/HX711/issues/73
delay(0);
}
return sum / times;
}
double HX711::get_value(byte times) {
return read_average(times) - OFFSET;
}
float HX711::get_units(byte times) {
return get_value(times) / SCALE;
}
void HX711::tare(byte times) {
double sum = read_average(times);
set_offset(sum);
}
void HX711::set_scale(float scale) {
SCALE = scale;
}
float HX711::get_scale() {
return SCALE;
}
void HX711::set_offset(long offset) {
OFFSET = offset;
}
long HX711::get_offset() {
return OFFSET;
}
void HX711::power_down() {
digitalWrite(PD_SCK, LOW);
digitalWrite(PD_SCK, HIGH);
}
void HX711::power_up() {
digitalWrite(PD_SCK, LOW);
}
/**
*
* HX711 library for Arduino
* https://github.com/bogde/HX711
*
* MIT License
* (c) 2018 Bogdan Necula
*
**/
#ifndef HX711_h
#define HX711_h
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class HX711
{
private:
byte PD_SCK; // Power Down and Serial Clock Input Pin
byte DOUT; // Serial Data Output Pin
byte GAIN; // amplification factor
long OFFSET = 0; // used for tare weight
float SCALE = 1; // used to return weight in grams, kg, ounces, whatever
public:
HX711();
virtual ~HX711();
// Initialize library with data output pin, clock input pin and gain factor.
// Channel selection is made by passing the appropriate gain:
// - With a gain factor of 64 or 128, channel A is selected
// - With a gain factor of 32, channel B is selected
// The library default is "128" (Channel A).
void begin(byte dout, byte pd_sck, byte gain = 128);
// Check if HX711 is ready
// from the datasheet: When output data is not ready for retrieval, digital output pin DOUT is high. Serial clock
// input PD_SCK should be low. When DOUT goes to low, it indicates data is ready for retrieval.
bool is_ready();
// Wait for the HX711 to become ready
void wait_ready(unsigned long delay_ms = 0);
bool wait_ready_retry(int retries = 3, unsigned long delay_ms = 0);
bool wait_ready_timeout(unsigned long timeout = 1000, unsigned long delay_ms = 0);
// set the gain factor; takes effect only after a call to read()
// channel A can be set for a 128 or 64 gain; channel B has a fixed 32 gain
// depending on the parameter, the channel is also set to either A or B
void set_gain(byte gain = 128);
// waits for the chip to be ready and returns a reading
long read();
// returns an average reading; times = how many times to read
long read_average(byte times = 10);
// returns (read_average() - OFFSET), that is the current value without the tare weight; times = how many readings to do
double get_value(byte times = 1);
// returns get_value() divided by SCALE, that is the raw value divided by a value obtained via calibration
// times = how many readings to do
float get_units(byte times = 1);
// set the OFFSET value for tare weight; times = how many times to read the tare value
void tare(byte times = 10);
// set the SCALE value; this value is used to convert the raw data to "human readable" data (measure units)
void set_scale(float scale = 1.f);
// get the current SCALE
float get_scale();
// set OFFSET, the value that's subtracted from the actual reading (tare weight)
void set_offset(long offset = 0);
// get the current OFFSET
long get_offset();
// puts the chip into power down mode
void power_down();
// wakes up the chip after power down mode
void power_up();
};
#endif /* HX711_h */
#include <Arduino.h>
// system
#include "drivers/indicators.h"
#include "comm/cobs.h"
#include "comm/cobsserial.h"
// loadcell / stepper
#include "lib/HX711.h"
#include "lib/AccelStepper.h"
#define LOADCELL_DOUT_PIN 12
#define LOADCELL_SCK_PIN 11
#define LOADCELL_OFFSET 50682624
#define LOADCELL_DIVIDER 5895655
#define STEPPER_STEP_PIN 9
#define STEPPER_DIR_PIN 10
#define STEPPER_ENABLE_PIN 5
HX711 loadcell;
AccelStepper stepper(AccelStepper::DRIVER, 9, 10);
// using A4988 step driver,
// want to limit to 1A current for thermal safety,
// vref = 8 * I_max * 0.050 // is 8 * 1A * 50mOhm
// so, target vref at 0.4v
void enableStepDriver(void){
digitalWrite(STEPPER_ENABLE_PIN, LOW);
}
void disableStepDriver(void){
digitalWrite(STEPPER_ENABLE_PIN, HIGH);
}
COBSSerial* cobsSerial = new COBSSerial();
void setup() {
// do osap setup,
ERRLIGHT_SETUP;
CLKLIGHT_SETUP;
cobsSerial->init();
// do application setup,
// EN pin on step driver
pinMode(5, OUTPUT);
disableStepDriver();
// stepper basics hello
stepper.setMaxSpeed(2500);
stepper.setAcceleration(5000);
// loacell
loadcell.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
loadcell.set_scale(LOADCELL_DIVIDER);
loadcell.set_offset(LOADCELL_OFFSET);
loadcell.tare();
}
uint8_t* pck;
uint16_t pl = 0;
uint8_t res[1024];
uint16_t rl = 0;
#define DEXKEY_LOADCELLREADING 12
#define DEXKEY_LOADCELLTARE 14
#define DEXKEY_STEPS 15
#define DEXKEY_MOTORENABLE 16
uint16_t one = 1;
int32_t stepsToTake = 0;
int32_t position = 0;
uint8_t retries = 0;
void loop() {
stepper.run();
cobsSerial->loop();
if(cobsSerial->hasPacket()){
CLKLIGHT_TOGGLE;
cobsSerial->getPacket(&pck, &pl);
switch(pck[0]){
case DEXKEY_LOADCELLREADING:
while(!loadcell.is_ready()){
delay(100);
retries ++;
if(retries > 10) break;
}
if(loadcell.is_ready()){
retries = 0;
int32_t reading = loadcell.get_value(5);
rl = 0;
res[rl ++] = DEXKEY_LOADCELLREADING;
ts_writeInt32(reading, res, &rl);
//debugmsg("reads: " + String(reading));
} else {
debugmsg("loadcell not ready");
}
break;
case DEXKEY_LOADCELLTARE:
loadcell.tare();
rl = 0;
res[rl ++] = DEXKEY_LOADCELLTARE;
break;
case DEXKEY_STEPS:
stepsToTake = 0;
one = 1;
ts_readInt32(&stepsToTake, pck, &one);
//debugmsg("to take steps: " + String(stepsToTake));
stepper.runToNewPosition(position + stepsToTake);
position += stepsToTake;
rl = 0;
res[rl ++] = DEXKEY_STEPS;
ts_writeInt32(stepsToTake, res, &rl);
break;
case DEXKEY_MOTORENABLE:
rl = 0;
res[rl ++] = DEXKEY_MOTORENABLE;
if(pck[1] > 0){
enableStepDriver();
res[rl ++] = 1;
} else {
disableStepDriver();
res[rl ++] = 0;
}
default:
// no-op
break;
}
if(rl > 0){
cobsSerial->sendPacket(res, rl);
}
cobsSerial->clearPacket();
}
} // end loop
This directory is intended for PIO Unit Testing and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PIO Unit Testing:
- https://docs.platformio.org/page/plus/unit-testing.html
/*
dex-client.js
dex tool client side
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2020
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the open systems assembly protocol (OSAP) project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
'use strict'
import { TS } from '../core/ts.js'
import LeastSquares from './utes/lsq.js'
import DEXUI from './dex-ui.js'
import * as dat from './libs/dat.gui.module.js'
console.log("hello-dex-tools")
// -------------------------------------------------------- KEYS / TOPLEVEL UI
document.addEventListener('keydown', (evt) => {
console.log(`keycode ${evt.keyCode}`)
switch (evt.keyCode) {
case 80: // 'p'
if(wscPort.send){
console.log('pinging...')
wscPort.send(Uint8Array.from([0,1,2]))
} else {
console.log('port closed')
}
break;
case 82: // 'r'
// read loadcell,
console.log('reading cell...')
dex.readLoadcell().then((count) => {
console.log('resolves count', count)
}).catch((err) => {
console.error(err)
})
break;
case 69: // 'e'
dex.setMotorEnable(true).then((val) => {
console.log('motor is ', val)
}).catch((err) => {
console.error(err)
})
break;
case 68: // 'd'
dex.setMotorEnable(false).then((val) => {
console.log('motor is ', val)
}).catch((err) => {
console.error(err)
})
break;
case 83: // 's'
dex.step(-10000).then((increment) => {
console.log('motor stepped', increment)
}).catch((err) => {
console.error(err)
})
break;
}
})
// -------------------------------------------------------- Button Smashing
let UI = new DEXUI()
// -------------------------------------------------------- DEX Virtual Machine
let TIMEOUT_MS = 5000
let DEXKEY_DEBUGMSG = 11
let DEXKEY_LOADCELLREADING = 12
let DEXKEY_LOADCELLTARE = 14
let DEXKEY_STEPS = 15
let DEXKEY_MOTORENABLE = 16
let dex = {}
let lsq = new LeastSquares()
let oldReadings = [[25, 14854, 29649, 44453, 74061, 103695],
[0, -0.100, -0.200, -0.300, -0.500, -0.700]]
let calibReadings = [[1,2,3], [1,2,3]]
let newReadings = [
[42, 255872, 209341, 171922, 141727, 108767, 79558, 48830, 30461],
[0, -16.8854, -13.7983, -11.3386, -9.3394, -7.1638, -5.243, -3.2144, -2.009]]
//[0, 1723g, 1408g, 1157, 953, 731, 535, 328, 205]] // (gf)
// donate some x, y readings to calibrate the least squares
// here this was observed integer outputs from the amp, and load applied during those readings
lsq.setObservations(newReadings)
// loadcells need to be calibrated,
dex.readLoadcell = () => {
return new Promise((resolve, reject) => {
if(!wscPort.send){
reject('port to dex is closed')
return
}
wscPort.send((Uint8Array.from([DEXKEY_LOADCELLREADING])))
let rejected = false
dex.recv = (data) => {
if(rejected) return // if already timed out... don't get confused
if(data[0] != DEXKEY_LOADCELLREADING){
reject('oddball key after loadcell request')
} else {
let counts = TS.read('int32', data, 1, true)
resolve(lsq.predict(counts))
}
}
setTimeout(() => {
rejected = true
reject('timeout')
}, TIMEOUT_MS)
})
}
dex.tareLoadcell = () => {
return new Promise((resolve, reject) => {
if(!wscPort.send){
reject('port to dex is closed')
return
}
wscPort.send(Uint8Array.from([DEXKEY_LOADCELLTARE]))
let rejected = false
dex.recv = (data) => {
if(rejected) return
if(data[0] != DEXKEY_LOADCELLTARE){
reject('oddball key after loadcell tare request')
} else {
resolve()
}
}
setTimeout(() => {
rejected = true
reject('timeout')
}, TIMEOUT_MS)
})
}
dex.setMotorEnable = (val) => {
return new Promise((resolve, reject) => {
if(!wscPort.send){
reject('port to dex is closed')
return
}
let ob = 0
if(val) ob = 1
wscPort.send(Uint8Array.from([DEXKEY_MOTORENABLE, ob]))
let rejected = false
dex.recv = (data) => {
if(rejected) return
if(data[0] != DEXKEY_MOTORENABLE){
reject('oddball key after motor enable request')
} else {
if(data[1] > 0){
resolve(true)
} else {
resolve(false)
}
}
} // end recv
setTimeout(() => {
rejected = true
reject('timeout')
}, TIMEOUT_MS)
})
}
// mm -> steps,
// microsteps are 16
// pinion is 20T, driven is 124
// lead screw is 1204: 4mm / revolution,
dex.spmm = (200 * 16 / (20/124)) / 4
dex.maxspeed = 0.5 //mm/s it's slow !
console.log('SPMM', dex.spmm)
dex.step = (mm) => {
// assert maximum move increment here,
if(mm > 20) mm = 20
if(mm < -20) mm = -20
return new Promise((resolve, reject) => {
if(!wscPort.send){
reject('port to dex is closed')
return
}
// calculate for spmm,
let steps = Math.floor(- dex.spmm * mm)
// console.log('STEPS', steps)
let req = new Uint8Array(5)
req[0] = DEXKEY_STEPS
TS.write('int32', steps, req, 1, true)
wscPort.send(req)
let rejected = false
dex.recv = (data) => {
if(rejected) return
if(data[0] != DEXKEY_STEPS){
reject('oddball key after step request')
} else {
let increment = TS.read('int32', data, 1, true)
resolve(- increment / dex.spmm)
}
} // end recv
let moveTime = (Math.abs(mm / dex.maxspeed) + 5) * 1000
//console.log('TIME', moveTime)
setTimeout(() => {
rejected = true
reject('timeout')
}, moveTime)
})
}
// -------------------------------------------------------- SPAWNING TEST SUBS
let wscPort = {}
wscPort.send = null
wscPort.onReceive = (data) => {
// put ll-msg catch flag here,
if(data[0] == DEXKEY_DEBUGMSG){
let msg = TS.read('string', data, 1, true)
console.warn('DEX DEBUGMSG: ', msg.value)
} else {
dex.recv(data)
}
}
let LOGPHY = false
// to test these systems, the client (us) will kickstart a new process
// on the server, and try to establish connection to it.
console.log("making client-to-server request to start remote process,")
console.log("and connecting to it w/ new websocket")
// ok, let's ask to kick a process on the server,
// in response, we'll get it's IP and Port,
// then we can start a websocket client to connect there,
// automated remote-proc. w/ vPort & wss medium,
// for args, do '/processName.js?args=arg1,arg2'
jQuery.get('/startLocal/dex-usb-bridge.js', (res) => {
if (res.includes('OSAP-wss-addr:')) {
let addr = res.substring(res.indexOf(':') + 2)
if (addr.includes('ws://')) {
console.log('starting socket to remote at', addr)
let ws = new WebSocket(addr)
ws.onopen = (evt) => {
wscPort.send = (buffer) => {
if (LOGPHY) console.log('PHY WSC Send', buffer)
ws.send(buffer)
}
ws.onmessage = (msg) => {
msg.data.arrayBuffer().then((buffer) => {
let uint = new Uint8Array(buffer)
if (LOGPHY) console.log('PHY WSC Recv')
if (LOGPHY) TS.logPacket(uint)
wscPort.onReceive(uint)
}).catch((err) => {
console.error(err)
})
}
UI.start(dex)
}
ws.onerror = (err) => {
console.error('sckt err', err)
UI.connectionError('socket closed')
}
ws.onclose = (evt) => {
console.error('sckt closed', evt)
wscPort.send = null
UI.connectionError('socket closed')
}
}
} else {
console.error('remote OSAP not established', res)
}
})
\ No newline at end of file
/*
dex-ui.js
dex interface client side
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2020
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the open systems assembly protocol (OSAP) project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
import LineChart from "./utes/lineChart.js"
export default function DEXUI(){
// the machine object,
let dex = null
this.start = (machine) => {
console.warn('alive')
dex = machine
clear()
}
// when lower levels bail, refreshing is the move
this.connectionError = (msg) => {
console.error('HALT')
}
// elements
let dom = $('#wrapper').get(0)
// UI util
let setPosition = (div, x, y) => {
$(dom).append(div)
$(div).css('left', `${x}px`).css('top', `${y}px`)
}
// hold flag
let waitUi = $('<div>').addClass('flag').get(0)
setPosition(waitUi, 180, 50)
let waitStatus = false
let waiting = () => {
waitStatus = true
$(waitUi).css('background-color', '#f7bbb7')
}
let clear = () => {
waitStatus = false
$(waitUi).css('background-color', '#b7f7c3')
}
let isWaiting = () =>{
return waitStatus
}
clear()
// incremental stacks
let ypos = 50
let yinc = 30
// position / zero
let position = 0
let posDisplay = $('<div>').addClass('button').get(0)
setPosition(posDisplay, 50, ypos)
posDisplay.update = (num) => {
position = num
$(posDisplay).text(`${num.toFixed(5)}mm`)
}
posDisplay.update(position)
$(posDisplay).click((evt) => {
position = 0
posDisplay.update(position)
})
let enableButton = $('<div>').addClass('button').get(0)
setPosition(enableButton, 50, ypos += yinc)
let motorEnabled = false
enableButton.update = (state) => {
if(state){
motorEnabled = true
$(enableButton).text('disable motor')
$(enableButton).css('background-color', '#b7f7c3')
} else {
motorEnabled = false
$(enableButton).text('enable motor')
$(enableButton).css('background-color', '#f7bbb7')
}
}
enableButton.update(motorEnabled)
$(enableButton).click((evt) => {
if(isWaiting()) return
waiting()
let req = !motorEnabled
dex.setMotorEnable(req).then((state) => {
clear()
enableButton.update(state)
}).catch((err) => {
clear()
console.error(err)
})
})
// increment-by
let incBy = $('<input type="text" value="5">').addClass('inputwrap').get(0)
setPosition(incBy, 50, ypos += yinc)
//console.log(incBy.value)
// up-button
let incUp = $('<div>').addClass('button').get(0)
setPosition(incUp, 50, ypos += yinc)
$(incUp).text('jog up')
$(incUp).click((evt) => {
if(isWaiting()) return
waiting()
dex.step(parseFloat(incBy.value)).then((increment) => {
clear()
posDisplay.update(position += increment)
}).catch((err) => {
clear()
console.error(err)
})
})
// down-button
let incDown = $('<div>').addClass('button').get(0)
setPosition(incDown, 50, ypos += yinc)
$(incDown).text('jog down')
$(incDown).click((evt) => {
if(isWaiting()) return
waiting()
dex.step(parseFloat(incBy.value) * -1).then((increment) => {
clear()
posDisplay.update(position += increment)
}).catch((err) => {
clear()
console.error(err)
})
})
// reading / zero
let load = 0
let loadDisplay = $('<div>').addClass('button').get(0)
setPosition(loadDisplay, 50, ypos += yinc + 10)
loadDisplay.update = (num) => {
load = num
$(loadDisplay).text(`${num.toFixed(3)}N`)
}
loadDisplay.update(load)
$(loadDisplay).click((evt) => {
if(isWaiting()) return
waiting()
dex.tareLoadcell().then(() => {
clear()
loadDisplay.update(0)
}).catch((err) => {
clear()
console.error(err)
})
})
// read request
let readReq = $('<div>').addClass('button').get(0)
setPosition(readReq, 50, ypos += yinc)
$(readReq).text('read loadcell')
$(readReq).click((evt) => {
if(isWaiting()) return
waiting()
dex.readLoadcell().then((load) => {
clear()
loadDisplay.update(load)
}).catch((err) => {
clear()
console.error(err)
})
})
// testing
// test-by
let testStep = $('<input type="text" value="0.005">').addClass('inputwrap').get(0)
setPosition(testStep, 50, ypos += yinc + 10)
// start / stop testing
let testButton = $('<div>').addClass('button').get(0)
setPosition(testButton, 50, ypos += yinc)
let testing = false
testButton.update = (state) => {
if(state){
testing = true
$(testButton).text('stop testing')
$(testButton).css('background-color', '#f7bbb7')
} else {
testing = false
$(testButton).text('start testing')
$(testButton).css('background-color', '#b7f7c3')
}
}
testButton.update(testing)
$(testButton).click((evt) => {
if(testing){
// stop
testButton.update(false)
} else {
// stop testing
runTest()
}
})
// util
let stringDate = () => {
let now = new Date()
return `${now.getFullYear()}-${now.getMonth()}-${now.getDate()}_${now.getHours()}-${now.getMinutes()}`
}
// testing
let testData = []
let runTest = async () => {
if(isWaiting()) return
position = 0
let startTime = performance.now()
testData = []
let drawData = []
testButton.update(true)
waiting()
while(testing){
try {
let time = (performance.now() - startTime) / 1000
// read 1st, then move,
let reading = await dex.readLoadcell()
loadDisplay.update(reading)
// store it, time, extension, load
testData.push([time, position, reading])
drawData.push([position, reading])
// and now,
let increment = await dex.step(testStep.value)
posDisplay.update(position += increment)
chart.draw(drawData)
} catch (err) {
console.error(err)
testButton.update(false)
}
}
clear()
}
// and finally, the doodling
let chart = new LineChart()
setPosition(chart.de, 220, 50)
// read request
let saveReq = $('<div>').addClass('button').get(0)
setPosition(saveReq, 50, ypos += yinc)
$(saveReq).text('save data')
$(saveReq).click((evt) => {
console.log()
dish(testData, 'csv', `${stringDate()}_dextest`)
})
let dish = (obj, format, name) => {
// serialize the thing
let url = null
if(format == 'json'){
url = URL.createObjectURL(new Blob([JSON.stringify(obj, null, 2)], {
type: "application/json"
}))
} else if (format == 'csv') {
let csvContent = "data:text/csv;charset=utf-8," //+ obj.map(e => e.join(',')).join('\n')
csvContent += "Time,Extension,Load\n"
csvContent += "(sec),(mm),(N)\n"
for(let line of obj){
csvContent += `${line[0].toFixed(3)},${line[1].toFixed(4)},${line[2].toFixed(4)}\n`
}
console.log(csvContent)
url = encodeURI(csvContent)
}
// hack to trigger the download,
let anchor = $('<a>ok</a>').attr('href', url).attr('download', name + `.${format}`).get(0)
$(document.body).append(anchor)
anchor.click()
}
}
File added
controller/js/client/drawing/bg.png

346 B

/*
domtools.js
osap tool drawing utility
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2020
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the open systems assembly protocol (OSAP) project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
// -------------------------------------------------------- TRANSFORM
// move things,
let writeTransform = (div, tf) => {
//console.log('vname, div, tf', view.name, div, tf)
if (tf.s) {
div.style.transform = `scale(${tf.s})`
} else {
div.style.transform = `scale(1)`
}
//div.style.transformOrigin = `${tf.ox}px ${tf.oy}px`
div.style.left = `${parseInt(tf.x)}px`
div.style.top = `${parseInt(tf.y)}px`
}
// a utility to do the same, for the background, for *the illusion of movement*,
// as a note: something is wrongo with this, background doth not zoom at the same rate...
let writeBackgroundTransform = (div, tf) => {
div.style.backgroundSize = `${tf.s * 10}px ${tf.s * 10}px`
div.style.backgroundPosition = `${tf.x + 50*(1-tf.s)}px ${tf.y + 50*(1-tf.s)}px`
}
// a uility to read those transforms out of elements,
// herein lays ancient mods code, probably a better implementation exists
let readTransform = (div) => {
// transform, for scale
let transform = div.style.transform
let index = transform.indexOf('scale')
let left = transform.indexOf('(', index)
let right = transform.indexOf(')', index)
let s = parseFloat(transform.slice(left + 1, right))
// left and right, position
let x = parseFloat(div.style.left)
let y = parseFloat(div.style.top)
return ({
s: s,
x: x,
y: y
})
}
// -------------------------------------------------------- DRAG Attach / Detach
let dragTool = (dragHandler, upHandler) => {
let onUp = (evt) => {
if (upHandler) upHandler(evt)
window.removeEventListener('mousemove', dragHandler)
window.removeEventListener('mouseup', onUp)
}
window.addEventListener('mousemove', dragHandler)
window.addEventListener('mouseup', onUp)
}
// -------------------------------------------------------- SVG
// return in an absolute-positioned wrapper at ax, ay, with dx / dy endpoint
let svgLine = (ax, ay, dx, dy, width) => {
let cont = document.createElementNS('http://www.w3.org/2000/svg', 'svg')
$(cont).addClass('svgcont').css('left', ax).css('top', ay)
let path = document.createElementNS('http://www.w3.org/2000/svg', 'line')
$(cont).append(path)
path.style.stroke = '#1a1a1a'
path.style.fill = 'none'
path.style.strokeWidth = `${width}px`
path.setAttribute('x1', 0)
path.setAttribute('y1', 0)
path.setAttribute('x2', dx)
path.setAttribute('y2', dy)
return cont
}
export default {
writeTransform,
writeBackgroundTransform,
readTransform,
dragTool,
svgLine
}
/*
gridsquid.js
osap tool drawing set
Jake Read at the Center for Bits and Atoms
(c) Massachusetts Institute of Technology 2020
This work may be reproduced, modified, distributed, performed, and
displayed for any purpose, but must acknowledge the open systems assembly protocol (OSAP) project.
Copyright is retained and must be preserved. The work is provided as is;
no warranty is provided, and users accept all liability.
*/
'use strict'
import dt from './domtools.js'
export default function GRIDSQUID() {
// ------------------------------------------------------ PLANE / ZOOM / PAN
let plane = $('<div>').addClass('plane').get(0)
let wrapper = $('#wrapper').get(0)
// odd, but w/o this, scaling the plane & the background together introduces some numerical errs,
// probably because the DOM is scaling a zero-size plane, or somesuch.
$(plane).css('background', 'url("/client/drawing/bg.png")').css('width', '100px').css('height', '100px')
let cs = 1 // current scale,
let dft = { s: cs, x: 600, y: 600, ox: 0, oy: 0 } // default transform
// zoom on wheel
wrapper.addEventListener('wheel', (evt) => {
evt.preventDefault()
evt.stopPropagation()
let ox = evt.clientX
let oy = evt.clientY
let ds
if (evt.deltaY > 0) {
ds = 0.025
} else {
ds = -0.025
}
let ct = dt.readTransform(plane)
ct.s *= 1 + ds
ct.x += (ct.x - ox) * ds
ct.y += (ct.y - oy) * ds
// max zoom pls thx
if (ct.s > 1.5) ct.s = 1.5
if (ct.s < 0.05) ct.s = 0.05
cs = ct.s
dt.writeTransform(plane, ct)
dt.writeBackgroundTransform(wrapper, ct)
})
// pan on drag,
wrapper.addEventListener('mousedown', (evt) => {
evt.preventDefault()
evt.stopPropagation()
dt.dragTool((drag) => {
drag.preventDefault()
drag.stopPropagation()
let ct = dt.readTransform(plane)
ct.x += drag.movementX
ct.y += drag.movementY
dt.writeTransform(plane, ct)
dt.writeBackgroundTransform(wrapper, ct)
})
})
// init w/ defaults,
dt.writeTransform(plane, dft)
dt.writeBackgroundTransform(wrapper, dft)
$(wrapper).append(plane)
// ------------------------------------------------------ RENDER / RERENDER
// all nodes render into the plane, for now into the wrapper
// once ready to render, heights etc should be set,
let renderNode = (node) => {
let nel = $(`<div>`).addClass('node').get(0) // node class is position:absolute
nel.style.width = `${parseInt(node.width)}px`
nel.style.height = `${parseInt(node.height)}px`
nel.style.left = `${parseInt(node.pos.x)}px`
nel.style.top = `${parseInt(node.pos.y)}px`
$(nel).append($(`<div>${node.name}</div>`).addClass('nodename'))
if (node.el) $(node.el).remove()
node.el = nel
$(plane).append(node.el)
}
let renderVPort = (vPort, outgoing) => {
let nel = $('<div>').addClass('vPort').get(0)
nel.style.width = `${parseInt(vPort.parent.width) - 4}px`
let ph = perPortHeight - heightPadding
nel.style.height = `${parseInt(ph)}px`
nel.style.left = `${parseInt(vPort.parent.pos.x)}px`
let ptop = vPort.parent.pos.y + heightPadding + vPort.indice * perPortHeight + heightPadding / 2
nel.style.top = `${parseInt(ptop)}px`
$(nel).append($(`<div>${vPort.name}</div>`).addClass('vPortname'))
// draw outgoing svg,
if(outgoing){
// anchor position (absolute, within attached-to), delx, dely
let line = dt.svgLine(perNodeWidth - 2, ph / 2, linkWidth, 0, 2)
$(nel).append(line) // appended, so that can be rm'd w/ the .remove() call
}
if (vPort.el) $(vPort.el).remove()
vPort.el = nel
$(plane).append(vPort.el)
}
// draw vals,
let perNodeWidth = 60
let linkWidth = 30
let perPortHeight = 120
let heightPadding = 10
// for now, this'll look a lot like thar recursor,
// and we'll just do it once, assuming nice and easy trees ...
this.draw = (root) => {
let start = performance.now()
$('.node').remove()
$('.vPort').remove()
// alright binches lets recurse,
// node-to-draw, vPort-entered-on, depth of recursion
let recursor = (node, entry, entryTop, depth) => {
node.width = perNodeWidth // time being, we are all this wide
node.height = heightPadding * 2 + node.vPorts.length * perPortHeight // 10px top / bottom, 50 per port
node.pos = {}
node.pos.x = depth * (perNodeWidth + linkWidth) // our x-pos is related to our depth,
// and the 'top' - now, if entry has an .el / etc data - if ports have this data as well, less calc. here
if (entry) {
node.pos.y = entryTop - entry.indice * perPortHeight - heightPadding
} else {
node.pos.y = 0
}
// draw ready?
renderNode(node)
// traverse,
for (let vp of node.vPorts) {
if (vp == entry) {
renderVPort(vp)
continue
} else if (vp.reciprocal) {
renderVPort(vp, true)
recursor(vp.reciprocal.parent, vp.reciprocal, node.pos.y + heightPadding + vp.indice * perPortHeight, depth + 1)
} else {
renderVPort(vp)
}
}
}
recursor(root, null, 0, 0)
//console.warn('draw was', performance.now() - start)
// root.pos = {
// x: 10,
// y: 10
// }
// root.width = 100
// root.height = 100
// renderNode(root)
// console.warn('next call')
// root.pos.x = 50
// renderNode(root)
}
}
<!DOCTYPE html>
<html>
<head>
<title>dex-tools</title>
<!-- these three disable caching, or like, should? -->
<meta http-equiv="Cache-Control" content="no-cache, no-store, must-revalidate" />
<meta http-equiv="Pragma" content="no-cache" />
<meta http-equiv="Expires" content="-1" />
</head>
<body>
<link href="style.css" rel="stylesheet">
<script src="libs/jquery.min.js"></script>
<script src="libs/math.js" type="text/javascript"></script>
<script src="libs/d3.js"></script>
<script type="module" src="dex-client.js"></script>
<div id="wrapper">
<!-- bootloop puts first view inside of this div -->
</div>
</body>
</html>