From 6b3f9ab64292388b2e776059e38cb3032b1e3276 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Mon, 9 Dec 2024 14:57:17 +0100 Subject: [PATCH 01/16] Remove arduino example --- examples/arduino/client/README.rst | 51 --- examples/arduino/client/connection.py | 15 - examples/arduino/client/default.sreg | 23 - examples/arduino/client/main.py | 20 - examples/arduino/client/testbench.py | 54 --- examples/arduino/client/trajectory.py | 51 --- examples/arduino/client/visualization.py | 90 ---- examples/arduino/server/avr/Min.cpp | 530 ---------------------- examples/arduino/server/avr/Min.h | 216 --------- examples/arduino/server/avr/README.rst | 45 -- examples/arduino/server/avr/TimerOne.cpp | 209 --------- examples/arduino/server/avr/TimerOne.h | 70 --- examples/arduino/server/avr/Transport.cpp | 198 -------- examples/arduino/server/avr/Transport.h | 203 --------- examples/arduino/server/avr/makefile | 16 - examples/arduino/server/avr/server.ino | 80 ---- 16 files changed, 1871 deletions(-) delete mode 100644 examples/arduino/client/README.rst delete mode 100644 examples/arduino/client/connection.py delete mode 100644 examples/arduino/client/default.sreg delete mode 100644 examples/arduino/client/main.py delete mode 100644 examples/arduino/client/testbench.py delete mode 100644 examples/arduino/client/trajectory.py delete mode 100644 examples/arduino/client/visualization.py delete mode 100644 examples/arduino/server/avr/Min.cpp delete mode 100644 examples/arduino/server/avr/Min.h delete mode 100644 examples/arduino/server/avr/README.rst delete mode 100644 examples/arduino/server/avr/TimerOne.cpp delete mode 100644 examples/arduino/server/avr/TimerOne.h delete mode 100644 examples/arduino/server/avr/Transport.cpp delete mode 100644 examples/arduino/server/avr/Transport.h delete mode 100644 examples/arduino/server/avr/makefile delete mode 100644 examples/arduino/server/avr/server.ino diff --git a/examples/arduino/client/README.rst b/examples/arduino/client/README.rst deleted file mode 100644 index 0b65149..0000000 --- a/examples/arduino/client/README.rst +++ /dev/null @@ -1,51 +0,0 @@ -===================================== -Visualization for the Arduino example -===================================== - -This `pyWisp` example can be used for the Arduino example. -The following features are included: - -* Heartbeat - -.. sphinx-marker - -Communication -^^^^^^^^^^^^^ - -For the communication the following frames are used: - - -.. list-table:: - :widths: 20 20 20 20 20 - :header-rows: 1 - - * - ID - - Datentyp - - Verwendung - - Richtung - - Klasse - * - 1 - - 1x byte - - Experiment starten/beenden - - zum Server - - - * - 10 - - unsigned long + 1x byte + 1x long + 1x float + 1x real - - Zeit + Wert1 + Wert2 + Wert3 + Wert4 - - vom Server - - TestTCP (TestBench) - * - 11 - - unsigned long + 1x double - - Zeit + Traj. Ausgang - - vom Server - - RampTrajectory (Trajectory) - * - 12 - - 1x byte + 1x int + 1x float + 1x double - - Zeit + Value1 + Value2 + Value3 + Value4 - - zum Server - - TestTCP (TestBench) - * - 13 - - 2x long + 2x double - - StartValue, StartTime, EndValue, EndTime - - zum Server - - RampTrajectory (Trajectory) diff --git a/examples/arduino/client/connection.py b/examples/arduino/client/connection.py deleted file mode 100644 index 288681c..0000000 --- a/examples/arduino/client/connection.py +++ /dev/null @@ -1,15 +0,0 @@ -# -*- coding: utf-8 -*- -from collections import OrderedDict - -from pywisp.connection import SerialConnection - - -class ConnTestSerial(SerialConnection): - settings = OrderedDict([("port", '/dev/cu.usbmodem1451'), - ("baud", 115200), - ]) - - def __init__(self): - SerialConnection.__init__(self, - self.settings['port'], - self.settings['baud']) diff --git a/examples/arduino/client/default.sreg b/examples/arduino/client/default.sreg deleted file mode 100644 index 906274e..0000000 --- a/examples/arduino/client/default.sreg +++ /dev/null @@ -1,23 +0,0 @@ -# default experiment file that is loaded when the gui starts up - -- Name: RunSystem - - RampTrajectory: - StartTime: 5 - StartValue: 28.0 - EndTime: 70 - EndValue: 5.0 - - Test: - Value1: 11.2 - Value2: 22.1 - Value3: 4 - Value4: 1 - - Config: - MovingWindowSize: 5 - MovingWindowEnable: True - HeartbeatTime: 100 - - Visu: - MplExampleVisualizer: diff --git a/examples/arduino/client/main.py b/examples/arduino/client/main.py deleted file mode 100644 index 48c191b..0000000 --- a/examples/arduino/client/main.py +++ /dev/null @@ -1,20 +0,0 @@ -# -*- coding: utf-8 -*- -import sys - -import testbench -import trajectory -from PyQt5.QtWidgets import QApplication -from connection import ConnTestSerial -from visualization import MplExampleVisualizer - -import pywisp as pw - -if __name__ == '__main__': - pw.registerConnection(ConnTestSerial) - pw.registerExperimentModule(testbench.Test) - pw.registerExperimentModule(trajectory.RampTrajectory) - pw.registerVisualizer(MplExampleVisualizer) - app = QApplication(sys.argv) - form = pw.MainGui() - form.show() - app.exec_() diff --git a/examples/arduino/client/testbench.py b/examples/arduino/client/testbench.py deleted file mode 100644 index 45f4633..0000000 --- a/examples/arduino/client/testbench.py +++ /dev/null @@ -1,54 +0,0 @@ -# -*- coding: utf-8 -*- -from collections import OrderedDict - -import struct -from connection import ConnTestSerial - -from pywisp.experimentModules import ExperimentModule - - -class Test(ExperimentModule): - dataPoints = ['Value1', - 'Value2', - 'Value3', - 'Value4', - ] - - publicSettings = OrderedDict([("Value1", 0.0), - ("Value2", 10.0), - ("Value3", 320), - ("Value4", 10)]) - - ids = [10, 12] - - connection = ConnTestSerial.__name__ - - def __init__(self): - ExperimentModule.__init__(self) - - def getParams(self, data): - payload = struct.pack('>ffhB', - float(data[0]), - float(data[1]), - int(data[2]), - int(data[3]) % 256) - dataPoint = {'id': self.ids[1], - 'msg': payload - } - return dataPoint - - def handleFrame(self, frame): - dataPoints = {} - fid = frame.min_id - if fid == self.ids[0]: - data = struct.unpack('>LffhB', frame.payload) - dataPoints['Time'] = data[0] - dataPoints['DataPoints'] = {'Value1': data[1], - 'Value2': data[2], - 'Value3': data[3], - 'Value4': data[4], - } - else: - dataPoints = None - - return dataPoints diff --git a/examples/arduino/client/trajectory.py b/examples/arduino/client/trajectory.py deleted file mode 100644 index a7abb21..0000000 --- a/examples/arduino/client/trajectory.py +++ /dev/null @@ -1,51 +0,0 @@ -# -*- coding: utf-8 -*- -from collections import OrderedDict - -import struct -from connection import ConnTestSerial -from pywisp.experimentModules import ExperimentModule - - -class RampTrajectory(ExperimentModule): - dataPoints = ['TrajOutput'] - - publicSettings = OrderedDict([("StartValue", 0.0), - ("StartTime", 10), - ("EndValue", 0.7), - ("EndTime", 15)]) - - ids = [11, 13] - - connection = ConnTestSerial.__name__ - - def __init__(self): - ExperimentModule.__init__(self) - - def getStartParams(self, data): - pass - - def getStopParams(self, data): - pass - - def getParams(self, data): - payload = struct.pack('>fLfL', - float(data[0]), - int(float(data[1]) * 1000), - float(data[2]), - int(float(data[3]) * 1000)) - dataPoint = {'id': self.ids[1], - 'msg': payload - } - return dataPoint - - def handleFrame(self, frame): - dataPoints = {} - fid = frame.min_id - if fid == self.ids[0]: - data = struct.unpack('>Lf', frame.payload) - dataPoints['Time'] = data[0] - dataPoints['DataPoints'] = {'TrajOutput': data[1]} - else: - dataPoints = None - - return dataPoints diff --git a/examples/arduino/client/visualization.py b/examples/arduino/client/visualization.py deleted file mode 100644 index 4cc510d..0000000 --- a/examples/arduino/client/visualization.py +++ /dev/null @@ -1,90 +0,0 @@ -# -*- coding: utf-8 -*- - -from matplotlib.offsetbox import * -from pywisp.visualization import MplVisualizer - - -class MplExampleVisualizer(MplVisualizer): - def __init__(self, qWidget, qLayout): - MplVisualizer.__init__(self, qWidget, qLayout) - self.axes.set_axis_off() - self.axes.set_aspect(1.5) - self.axes.set_xlim(0, 1) - self.axes.set_ylim(0, 1) - - top = 0.98 - mid = 0.5 - - def makeValue(x, y, dir, text, textpos): - arrLen = 30 - if dir == 'below': - bx = 0 - by = arrLen - elif dir == 'above': - bx = 0 - by = -arrLen - elif dir == 'left': - bx = arrLen - by = 0 - elif dir == 'right': - bx = -arrLen - by = 0 - elif dir is None: - bx = 0 - by = 0 - - tax, tay = 0.5, 0.5 - if textpos == 'below': - tay = 1 - tx = 0 - ty = -15 - elif textpos == 'above': - tay = 0 - tx = 0 - ty = 15 - elif textpos == 'left': - tax = 1 - tx = -15 - ty = 0 - elif textpos == 'right': - tax = 0 - tx = 15 - ty = 0 - # variable text - textObject = TextArea('tmp', textprops=dict(size=8)) - ab = AnnotationBbox(textObject, xy=(x, y), xycoords=self.axes.transAxes, - xybox=(bx, by), boxcoords='offset points', - bboxprops=dict(boxstyle='circle'), - arrowprops=dict(arrowstyle="-")) - self.axes.add_artist(ab) - # annotating text - tx = AnnotationBbox(HPacker(sep=0, pad=0, children=[TextArea(text, textprops=dict(size=8))]), - xy=(x, y), xycoords=self.axes.transAxes, box_alignment=(tax, tay), - xybox=(bx + tx, by + ty), boxcoords='offset points', - frameon=False) - self.axes.add_artist(tx) - return textObject - - self.Value1 = makeValue(0.1, mid, 'below', 'V$_{1}$', 'left') - self.Value2 = makeValue(0.35, mid, None, 'V$_{2}$', 'below') - self.Value3 = makeValue(0.65, mid, None, 'V$_{3}$', 'below') - self.Value4 = makeValue(0.9, mid, 'below', 'V$_{4}$', 'right') - self.ValueTraj = makeValue(mid, top, None, 'V$_{Traj}$', 'above') - - self.canvas.draw_idle() - - def update(self, dataPoints): - dps = {'TrajOutput': [self.ValueTraj, '1'], - 'Value1': [self.Value1, '1'], - 'Value2': [self.Value2, '1'], - 'Value3': [self.Value3, '1'], - 'Value4': [self.Value4, '1'], - } - - for key, value in dps.items(): - if dataPoints[key].values: - formatStr = "%." + value[1] + "f" - value[0].set_text(formatStr % dataPoints[key].values[-1]) - - self.canvas.draw_idle() - self.saveIfChecked() diff --git a/examples/arduino/server/avr/Min.cpp b/examples/arduino/server/avr/Min.cpp deleted file mode 100644 index c8d54db..0000000 --- a/examples/arduino/server/avr/Min.cpp +++ /dev/null @@ -1,530 +0,0 @@ -#include "Min.h" - - -#if DEBUG_PRINT -void debug_print(const char *msg, ...) { - static uint8_t init = 1; - if (init) { - Serial2.begin(115200); - init = 0; - } - char s[256]; - va_list args; - va_start(args, msg); - vsprintf(s, msg, args); - va_end(args); - Serial2.print(s); -} -#endif - -void Min::crc32_init_context(uint32_t &checksum) { - checksum = 0xffffffffU; -} - -void Min::crc32_step(uint32_t &checksum, uint8_t byte) { - checksum ^= byte; - for (uint32_t j = 0; j < 8; j++) { - uint32_t mask = (uint32_t) - (checksum & 1U); - checksum = (checksum >> 1) ^ (0xedb88320U & mask); - } -} - -uint32_t Min::crc32_finalize(uint32_t &checksum) { - return ~checksum; -} - -void Min::stuffed_tx_byte(uint8_t byte) { - // Transmit the byte - tx_byte(byte); - crc32_step(this->tx_checksum, byte); - - // See if an additional stuff byte is needed - if (byte == HEADER_BYTE) { - if (--this->tx_header_byte_countdown == 0) { - tx_byte(STUFF_BYTE); // Stuff byte - this->tx_header_byte_countdown = 2U; - } - } else { - this->tx_header_byte_countdown = 2U; - } -} - -void Min::on_wire_bytes(uint8_t id_control, uint8_t seq, uint8_t *payload_base, uint16_t payload_offset, - uint16_t payload_mask, uint8_t payload_len) { - uint8_t n; - uint32_t checksum; - - this->tx_header_byte_countdown = 2U; - crc32_init_context(this->tx_checksum); - - // Header is 3 bytes; because unstuffed will reset receiver immediately - tx_byte(HEADER_BYTE); - tx_byte(HEADER_BYTE); - tx_byte(HEADER_BYTE); - - stuffed_tx_byte(id_control); - if (id_control & TRANSPORT_FRAME) { - // Send the sequence number if it is a transport frame - stuffed_tx_byte(seq); - } - - stuffed_tx_byte(payload_len); - - for (n = payload_len; n > 0; n--) { - stuffed_tx_byte(payload_base[payload_offset]); - payload_offset++; - payload_offset &= payload_mask; - } - - checksum = crc32_finalize(this->tx_checksum); - - // Network order is big-endian. A decent C compiler will spot that this - // is extracting bytes and will use efficient instructions. - stuffed_tx_byte((uint8_t)((checksum >> 24) & 0xffU)); - stuffed_tx_byte((uint8_t)((checksum >> 16) & 0xffU)); - stuffed_tx_byte((uint8_t)((checksum >> 8) & 0xffU)); - stuffed_tx_byte((uint8_t)((checksum >> 0) & 0xffU)); - - // Ensure end-of-frame doesn't contain 0xaa and confuse search for start-of-frame - tx_byte(EOF_BYTE); -} - -// Pops frame from front of queue, reclaims its ring buffer space -void Min::transport_fifo_pop() { - struct TransportFrame *frame = &this->transport_fifo.frames[this->transport_fifo.head_idx]; - debug_print("Popping frame id=%d seq=%d\n", frame->min_id, frame->seq); - - this->transport_fifo.n_frames--; - this->transport_fifo.head_idx++; - this->transport_fifo.head_idx &= transport_fifo_max_frames_mask; - this->transport_fifo.n_ring_buffer_bytes -= frame->payload_len; -} - -// Claim a buffer slot from the FIFO. Returns 0 if there is no space. -struct TransportFrame *Min::transport_fifo_push(uint8_t data_size) { - // A frame is only queued if there aren't too many frames in the FIFO and there is space in the - // data ring buffer. - struct TransportFrame *ret = 0; - if (this->transport_fifo.n_frames < transport_fifo_max_frames) { - // Is there space in the ring buffer for the frame payload? - if (this->transport_fifo.n_ring_buffer_bytes + data_size <= transport_fifo_max_frame_data) { - this->transport_fifo.n_frames++; - // Create FIFO entry - ret = &(this->transport_fifo.frames[this->transport_fifo.tail_idx]); - ret->payload_offset = this->transport_fifo.ring_buffer_tail_offset; - - // Claim ring buffer space - this->transport_fifo.n_ring_buffer_bytes += data_size; - this->transport_fifo.ring_buffer_tail_offset += data_size; - this->transport_fifo.ring_buffer_tail_offset &= transport_fifo_max_frame_data_mask; - - // Claim FIFO space - this->transport_fifo.tail_idx++; - this->transport_fifo.tail_idx &= transport_fifo_max_frames_mask; - } else { - debug_print("No FIFO payload space: data_size=%d, n_ring_buffer_bytes=%d\n", - data_size, this->transport_fifo.n_ring_buffer_bytes); - } - } else { - debug_print("No FIFO frame slots\n"); - } - return ret; -} - -// Return the nth frame in the FIFO -struct TransportFrame *Min::transport_fifo_get(uint8_t n) { - uint8_t idx = this->transport_fifo.head_idx; - return &this->transport_fifo.frames[(idx + n) & transport_fifo_max_frames_mask]; -} - -// Sends the given frame to the serial line -void Min::transport_fifo_send(struct TransportFrame *frame) { - debug_print("transport_fifo_send: min_id=%d, seq=%d, payload_len=%d\n", - frame->min_id, frame->seq, frame->payload_len); - on_wire_bytes(frame->min_id | (uint8_t) TRANSPORT_FRAME, frame->seq, payloads_ring_buffer, frame->payload_offset, - transport_fifo_max_frame_data_mask, frame->payload_len); - frame->last_sent_time_ms = transport_fifo.now; -} - -void Min::tx_byte(uint8_t byte) { - serial->write(&byte, 1); -} - -uint32_t Min::time_ms() { - return millis(); -} - -uint16_t Min::tx_space() { - return serial->availableForWrite(); -} - - -// We don't queue an ACK frame - we send it straight away (if there's space to do so) -void Min::send_ack() { - // In the embedded end we don't reassemble out-of-order frames and so never ask for retransmits. Payload is - // always the same as the sequence number. - debug_print("send ACK: seq=%d\n", this->transport_fifo.rn); - if (on_wire_size(0) <= tx_space()) { - on_wire_bytes(ACK, this->transport_fifo.rn, &this->transport_fifo.rn, 0, 0, 1U); - this->transport_fifo.last_sent_ack_time_ms = transport_fifo.now; - } -} - -// We don't queue an RESET frame - we send it straight away (if there's space to do so) -void Min::send_reset() { - debug_print("send RESET\n"); - if (on_wire_size(0) <= tx_space()) { - on_wire_bytes(RESET, 0, 0, 0, 0, 0); - } -} - -void TransportFIFO::reset() { - // Clear down the transmission FIFO queue - this->n_frames = 0; - this->head_idx = 0; - this->tail_idx = 0; - this->n_ring_buffer_bytes = 0; - this->ring_buffer_tail_offset = 0; - this->sn_max = 0; - this->sn_min = 0; - this->rn = 0; - - // Reset the timers - this->last_received_anything_ms = now; - this->last_sent_ack_time_ms = now; - this->last_received_frame_ms = 0; -} - -void Min::transport_reset(bool inform_other_side) { - if (inform_other_side) { - // Tell the other end we have gone away - send_reset(); - } - - // Throw our frames away - transport_fifo.reset(); -} - -// Queues a MIN ID / payload frame into the outgoing FIFO -// Returns true if the frame was queued OK. -bool Min::queue_frame(uint8_t min_id, uint8_t *payload, uint8_t payload_len) { - struct TransportFrame *frame = transport_fifo_push(payload_len); // Claim a FIFO slot, reserve space for payload - - // We are just queueing here: the poll() function puts the frame into the window and on to the wire - if (frame != 0) { - // Copy frame details into frame slot, copy payload into ring buffer - frame->min_id = min_id & (uint8_t) 0x3fU; - frame->payload_len = payload_len; - - uint16_t payload_offset = frame->payload_offset; - for (uint32_t i = 0; i < payload_len; i++) { - payloads_ring_buffer[payload_offset] = payload[i]; - payload_offset++; - payload_offset &= transport_fifo_max_frame_data_mask; - } - debug_print("Queued ID=%d, len=%d\n", min_id, payload_len); - return true; - } else { - debug_print("Dropping frame ID=%d, len=%d\n", min_id, payload_len); - this->transport_fifo.dropped_frames++; - return false; - } -} - -// Finds the frame in the window that was sent least recently -struct TransportFrame *Min::find_retransmit_frame() { - uint8_t window_size = this->transport_fifo.sn_max - this->transport_fifo.sn_min; - - // Start with the head of the queue and call this the oldest - struct TransportFrame *oldest_frame = &this->transport_fifo.frames[this->transport_fifo.head_idx]; - uint32_t oldest_elapsed_time = transport_fifo.now - oldest_frame->last_sent_time_ms; - - uint8_t idx = this->transport_fifo.head_idx; - for (uint8_t i = 0; i < window_size; i++) { - uint32_t elapsed = transport_fifo.now - this->transport_fifo.frames[idx].last_sent_time_ms; - if (elapsed > oldest_elapsed_time) { // Strictly older only; otherwise the earlier frame is deemed the older - oldest_elapsed_time = elapsed; - oldest_frame = &this->transport_fifo.frames[idx]; - } - idx++; - idx &= transport_fifo_max_frames_mask; - } - - return oldest_frame; -} - -// This runs the receiving half of the transport protocol, acknowledging frames received, discarding -// duplicates received, and handling RESET requests. -void Min::valid_frame_received() { - uint8_t id_control = this->rx_frame_id_control; - uint8_t *payload = this->rx_frame_payload_buf; - uint8_t payload_len = this->rx_control; - - uint8_t seq = this->rx_frame_seq; - - uint8_t num_acked; - uint8_t num_nacked; - uint8_t num_in_window; - - // When we receive anything we know the other end is still active and won't shut down - this->transport_fifo.last_received_anything_ms = transport_fifo.now; - - switch (id_control) { - case ACK: - // If we get an ACK then we remove all the acknowledged frames with seq < rn - // The payload byte specifies the number of NACKed frames: how many we want retransmitted because - // they have gone missing. - // But we need to make sure we don't accidentally ACK too many because of a stale ACK from an old session - num_acked = seq - this->transport_fifo.sn_min; - num_nacked = payload[0] - seq; - num_in_window = this->transport_fifo.sn_max - this->transport_fifo.sn_min; - - if (num_acked <= num_in_window && this->transport_fifo.n_frames) { - this->transport_fifo.sn_min = seq; - // Now pop off all the frames up to (but not including) rn - // The ACK contains Rn; all frames before Rn are ACKed and can be removed from the window - debug_print("Received ACK seq=%d, num_acked=%d, num_nacked=%d\n", seq, num_acked, num_nacked); - for (uint8_t i = 0; i < num_acked; i++) { - transport_fifo_pop(); - } - uint8_t idx = this->transport_fifo.head_idx; - // Now retransmit the number of frames that were requested - for (uint8_t i = 0; i < num_nacked; i++) { - struct TransportFrame *retransmit_frame = &this->transport_fifo.frames[idx]; - if (on_wire_size(retransmit_frame->payload_len) <= tx_space()) { - transport_fifo_send(retransmit_frame); - } - idx++; - idx &= transport_fifo_max_frames_mask; - } - } else { - debug_print("Received spurious ACK seq=%d\n", seq); - this->transport_fifo.spurious_acks++; - } - break; - case RESET: - // If we get a RESET demand then we reset the transport protocol (empty the FIFO, reset the - // sequence numbers, etc.) - // We don't send anything, we just do it. The other end can send frames to see if this end is - // alive (pings, etc.) or just wait to get application frames. - this->transport_fifo.resets_received++; - transport_reset(true); - break; - default: - if (id_control & TRANSPORT_FRAME) { - // Incoming application frames - - // Reset the activity time (an idle connection will be stalled) - this->transport_fifo.last_received_frame_ms = transport_fifo.now; - - if (seq == this->transport_fifo.rn) { - // Accept this frame as matching the sequence number we were looking for - - // Now looking for the next one in the sequence - this->transport_fifo.rn++; - - // Always send an ACK back for the frame we received - // ACKs are short (should be about 9 microseconds to send on the wire) and - // this will cut the latency down. - // We also periodically send an ACK in case the ACK was lost, and in any case - // frames are re-sent. - send_ack(); - - // Now ready to pass this up to the application handlers - - // Pass frame up to application handler to deal with - debug_print("Incoming app frame seq=%d, id=%d, payload len=%d\n", - seq, id_control & (uint8_t) 0x3fU, payload_len); - application_handler(id_control & (uint8_t) 0x3fU, payload, payload_len); - } else { - // Discard this frame because we aren't looking for it: it's either a dupe because it was - // retransmitted when our ACK didn't get through in time, or else it's further on in the - // sequence and others got dropped. - this->transport_fifo.sequence_mismatch_drop++; - } - } else { - // Not a transport frame - application_handler(id_control & (uint8_t) 0x3fU, payload, payload_len); - } - break; - } -} - -void Min::rx_byte(uint8_t byte) { - // Regardless of state, three header bytes means "start of frame" and - // should reset the frame buffer and be ready to receive frame data - // - // Two in a row in over the frame means to expect a stuff byte. - uint32_t crc; - - if (this->rx_header_bytes_seen == 2) { - this->rx_header_bytes_seen = 0; - if (byte == HEADER_BYTE) { - debug_print("H"); - this->rx_frame_state = RECEIVING_ID_CONTROL; - return; - } - if (byte == STUFF_BYTE) { - debug_print("S"); - /* Discard this byte; carry on receiving on the next character */ - return; - } else { - /* Something has gone wrong, give up on this frame and look for header again */ - this->rx_frame_state = SEARCHING_FOR_SOF; - return; - } - } - - if (byte == HEADER_BYTE) { - debug_print("H"); - this->rx_header_bytes_seen++; - } else { - this->rx_header_bytes_seen = 0; - } - - switch (this->rx_frame_state) { - case SEARCHING_FOR_SOF: - break; - case RECEIVING_ID_CONTROL: - this->rx_frame_id_control = byte; - this->rx_frame_payload_bytes = 0; - crc32_init_context(this->rx_checksum); - crc32_step(this->rx_checksum, byte); - if (byte & TRANSPORT_FRAME) { - this->rx_frame_state = RECEIVING_SEQ; - } else { - this->rx_frame_seq = 0; - this->rx_frame_state = RECEIVING_LENGTH; - } - break; - case RECEIVING_SEQ: - this->rx_frame_seq = byte; - crc32_step(this->rx_checksum, byte); - this->rx_frame_state = RECEIVING_LENGTH; - break; - case RECEIVING_LENGTH: - this->rx_frame_length = byte; - debug_print("l=%d ", this->rx_frame_length); - this->rx_control = byte; - crc32_step(this->rx_checksum, byte); - if (this->rx_frame_length > 0) { - // Can reduce the RAM size by compiling limits to frame sizes - if (this->rx_frame_length <= max_payload) { - this->rx_frame_state = RECEIVING_PAYLOAD; - } else { - // Frame dropped because it's longer than any frame we can buffer - this->rx_frame_state = SEARCHING_FOR_SOF; - } - } else { - this->rx_frame_state = RECEIVING_CHECKSUM_3; - } - break; - case RECEIVING_PAYLOAD: - this->rx_frame_payload_buf[this->rx_frame_payload_bytes++] = byte; - crc32_step(this->rx_checksum, byte); - if (--this->rx_frame_length == 0) { - this->rx_frame_state = RECEIVING_CHECKSUM_3; - } - break; - case RECEIVING_CHECKSUM_3: - debug_print("C4C"); - this->rx_frame_checksum = ((uint32_t) byte) << 24; - this->rx_frame_state = RECEIVING_CHECKSUM_2; - break; - case RECEIVING_CHECKSUM_2: - debug_print("C3C"); - this->rx_frame_checksum |= ((uint32_t) byte) << 16; - this->rx_frame_state = RECEIVING_CHECKSUM_1; - break; - case RECEIVING_CHECKSUM_1: - debug_print("C2C"); - this->rx_frame_checksum |= ((uint32_t) byte) << 8; - this->rx_frame_state = RECEIVING_CHECKSUM_0; - break; - case RECEIVING_CHECKSUM_0: - debug_print("C1C"); - this->rx_frame_checksum |= byte; - crc = crc32_finalize(this->rx_checksum); - if (this->rx_frame_checksum != crc) { - debug_print("frame fails crc\n"); - // Frame fails the checksum and so is dropped - this->rx_frame_state = SEARCHING_FOR_SOF; - } else { - debug_print("frame passes crc\n"); - // Checksum passes, go on to check for the end-of-frame marker - this->rx_frame_state = RECEIVING_EOF; - } - break; - case RECEIVING_EOF: - if (byte == 0x55u) { - // Frame received OK, pass up data to handler - debug_print("valid frame\n"); - valid_frame_received(); - } - // else discard - // Look for next frame */ - this->rx_frame_state = SEARCHING_FOR_SOF; - break; - default: - // Should never get here but in case we do then reset to a safe state - this->rx_frame_state = SEARCHING_FOR_SOF; - break; - } -} - -// Sends received bytes into a MIN context and runs the transport timeouts -void Min::poll() { - int avl = 0; - uint32_t len = 0; - if ((avl = serial->available())) { - len = serial->readBytes(serialBuf, avl); - debug_print("received %d bytes of data: \n", len); - for (uint32_t i = 0; i < len; i++) { - debug_print("%x ", serialBuf[i]); - rx_byte(serialBuf[i]); - } - debug_print("\n"); - - } - - transport_fifo.now = time_ms(); - - bool remote_connected = (transport_fifo.now - this->transport_fifo.last_received_anything_ms < idle_timeout_ms); - bool remote_active = (transport_fifo.now - this->transport_fifo.last_received_frame_ms < idle_timeout_ms); - - // This sends one new frame or resends one old frame - uint8_t window_size = this->transport_fifo.sn_max - this->transport_fifo.sn_min; // Window size - if ((window_size < max_window_size) && (this->transport_fifo.n_frames > window_size)) { - // There are new frames we can send; but don't even bother if there's no buffer space for them - struct TransportFrame *frame = transport_fifo_get(window_size); - if (on_wire_size(frame->payload_len) < tx_space()) { - frame->seq = this->transport_fifo.sn_max; - transport_fifo_send(frame); - - // Move window on - this->transport_fifo.sn_max++; - } - } else { - // Sender cannot send new frames so resend old ones (if there's anyone there) - if ((window_size > 0) && remote_connected && this->transport_fifo.n_frames) { - // There are unacknowledged frames. Can re-send an old frame. Pick the least recently sent one. - struct TransportFrame *oldest_frame = find_retransmit_frame(); - if (transport_fifo.now - oldest_frame->last_sent_time_ms >= frame_retransmit_timeout_ms) { - // Resending oldest frame if there's a chance there's enough space to send it - if (on_wire_size(oldest_frame->payload_len) <= tx_space()) { - transport_fifo_send(oldest_frame); - } - } - } - } - // Periodically transmit the ACK with the rn value, unless the line has gone idle - if (transport_fifo.now - this->transport_fifo.last_sent_ack_time_ms > ack_retransmit_timeout_ms) { - if (remote_active) { - send_ack(); - } - } -} - -void Min::application_handler(uint8_t min_id, uint8_t *min_payload, uint8_t len_payload) { - application_function(min_id, min_payload, len_payload); -} diff --git a/examples/arduino/server/avr/Min.h b/examples/arduino/server/avr/Min.h deleted file mode 100644 index 815b73a..0000000 --- a/examples/arduino/server/avr/Min.h +++ /dev/null @@ -1,216 +0,0 @@ -// MIN Protocol v3.0. -// -// MIN is a lightweight reliable protocol for exchanging information from a microcontroller (MCU) to a host. -// It is designed to run on an 8-bit MCU but also scale up to more powerful devices. A typical use case is to -// send data from a UART on a small MCU over a UART-USB converter plugged into a PC host. A Python implementation -// of host code is provided (or this code could be compiled for a PC). -// -// MIN supports frames of 0-255 bytes (with a lower limit selectable at compile time to reduce RAM). MIN frames -// have identifier values between 0 and 63. -// -// A transport layer is compiled in. This provides sliding window reliable transmission of frames. - -#ifndef MIN_H -#define MIN_H - -#include -#include - -#define DEBUG_PRINT 0 -#if DEBUG_PRINT - -void debug_print(const char *msg, ...); - -#else -#define debug_print(...) -#endif - -struct TransportFrame { - uint32_t last_sent_time_ms; // When frame was last sent (used for re-send timeouts) - uint16_t payload_offset; // Where in the ring buffer the payload is - uint8_t payload_len; // How big the payload is - uint8_t min_id; // ID of frame - uint8_t seq; // Sequence number of frame -}; - -class TransportFIFO { -public: - TransportFIFO(uint8_t size) { - frames = new struct TransportFrame[size]; - now = 0; - } - - void reset(); - - - struct TransportFrame *frames; - uint32_t last_sent_ack_time_ms; - uint32_t last_received_anything_ms; - uint32_t last_received_frame_ms; - uint32_t dropped_frames; // Diagnostic counters - uint32_t spurious_acks; - uint32_t sequence_mismatch_drop; - uint32_t resets_received; - uint16_t n_ring_buffer_bytes; // Number of bytes used in the payload ring buffer - uint16_t ring_buffer_tail_offset; // Tail of the payload ring buffer - uint8_t n_frames; // Number of frames in the FIFO - uint8_t head_idx; // Where frames are taken from in the FIFO - uint8_t tail_idx; // Where new frames are added - uint8_t sn_min; // Sequence numbers for transport protocol - uint8_t sn_max; - uint8_t rn; - uint32_t now; -}; - -class Min { -public: - Min(uint8_t frames, uint16_t frame_data, uint8_t max_payload, uint8_t serialBufLen) : transport_fifo(frames) { - // Initialize context - rx_header_bytes_seen = 0; - rx_frame_state = SEARCHING_FOR_SOF; - - // Counters for diagnosis purposes - transport_fifo.reset(); - - this->max_payload = max_payload; - payloads_ring_buffer = new uint8_t[frame_data]; - rx_frame_payload_buf = new uint8_t[max_payload]; // Payload received so far - transport_fifo_max_frames = frames; - transport_fifo_max_frames_mask = frames - 1; - transport_fifo_max_frame_data = frame_data; - transport_fifo_max_frame_data_mask = frame_data - 1; - - serialBuf = new uint8_t[serialBufLen]; - } - - void initSerial(HardwareSerial &serial) { - this->serial = &serial; - - this->serial->begin(115200); - } - - -private: - TransportFIFO transport_fifo; // T-MIN queue of outgoing frames - uint32_t ack_retransmit_timeout_ms = 25; - uint32_t frame_retransmit_timeout_ms = 400; - uint32_t max_window_size = 16; - uint32_t idle_timeout_ms = 3000; - uint32_t transport_fifo_max_frames_mask; - uint32_t transport_fifo_max_frame_data_mask; - uint32_t transport_fifo_max_frames = 16; - uint32_t transport_fifo_max_frame_data; - uint8_t max_payload = 40; - - uint8_t *serialBuf; - uint8_t *payloads_ring_buffer; - - HardwareSerial *serial; - - uint8_t *rx_frame_payload_buf; // Payload received so far - uint32_t rx_frame_checksum; // Checksum received over the wire - uint32_t rx_checksum; // Calculated checksum for receiving frame - uint32_t tx_checksum; // Calculated checksum for sending frame - uint8_t rx_header_bytes_seen; // Countdown of header bytes to reset state - uint8_t rx_frame_state; // State of receiver - uint8_t rx_frame_payload_bytes; // Length of payload received so far - uint8_t rx_frame_id_control; // ID and control bit of frame being received - uint8_t rx_frame_seq; // Sequence number of frame being received - uint8_t rx_frame_length; // Length of frame - uint8_t rx_control; // Control byte - uint8_t tx_header_byte_countdown; // Count out the header bytes - - // Special protocol bytes - enum { - HEADER_BYTE = 0xaaU, - STUFF_BYTE = 0x55U, - EOF_BYTE = 0x55U, - }; - - // Receiving state machine - enum { - SEARCHING_FOR_SOF, - RECEIVING_ID_CONTROL, - RECEIVING_SEQ, - RECEIVING_LENGTH, - RECEIVING_PAYLOAD, - RECEIVING_CHECKSUM_3, - RECEIVING_CHECKSUM_2, - RECEIVING_CHECKSUM_1, - RECEIVING_CHECKSUM_0, - RECEIVING_EOF, - }; - - enum { - // Top bit must be set: these are for the transport protocol to use - // 0x7f and 0x7e are reserved MIN identifiers. - ACK = 0xffU, - RESET = 0xfeU, - TRANSPORT_FRAME = 0x80U, - }; - - - void crc32_init_context(uint32_t &checksum); - - void crc32_step(uint32_t &checksum, uint8_t byte); - - uint32_t crc32_finalize(uint32_t &checksum); - - void stuffed_tx_byte(uint8_t byte); - - void on_wire_bytes(uint8_t id_control, uint8_t seq, uint8_t *payload_base, uint16_t payload_offset, - uint16_t payload_mask, uint8_t payload_len); - - void transport_fifo_pop(); - - TransportFrame *transport_fifo_push(uint8_t data_size); - - TransportFrame *transport_fifo_get(uint8_t n); - - void transport_fifo_send(TransportFrame *frame); - - void send_ack(); - - void send_reset(); - - void transport_fifo_reset(); - - void transport_reset(bool inform_other_side); - -public: - bool queue_frame(uint8_t min_id, uint8_t *payload, uint8_t payload_len); - - void poll(); - -private: - TransportFrame *find_retransmit_frame(); - - void valid_frame_received(); - - void rx_byte(uint8_t byte); - - - uint32_t on_wire_size(uint32_t p) { - return p + 11U; - } - - void tx_byte(uint8_t byte); - - uint32_t time_ms(); - - uint16_t tx_space(); - - void application_handler(uint8_t min_id, uint8_t *min_payload, uint8_t len_payload); - -public: - void add_application_function( - void(*application_function)(uint8_t min_id, uint8_t *min_payload, uint8_t len_payload)) { - this->application_function = application_function; - } - -private: - void (*application_function)(uint8_t min_id, uint8_t *min_payload, uint8_t len_payload); - -}; - -#endif //MIN_H diff --git a/examples/arduino/server/avr/README.rst b/examples/arduino/server/avr/README.rst deleted file mode 100644 index ecd2ca6..0000000 --- a/examples/arduino/server/avr/README.rst +++ /dev/null @@ -1,45 +0,0 @@ -============================== -Server for the Arduino example -============================== - -.. sphinx-marker - -Requirements -^^^^^^^^^^^^ - -For the usage of the Arduino code the following programs are needed: - -- Arduino IDE -- Python -- make - -Additionally to this the following repository must be cloned: - -- `Arduino-Makefile `_ - -Useage -^^^^^^ - -In the makefile you have to correct the path to your arduino installation - -.. code:: bash - - ARDUINO_DIR = - -and to the Arduino-Makefile - -.. code:: bash - - include /Arduino.mk - -To compile the code: - -.. code:: bash - - make - -To upload the code to Arduino: - -.. code:: bash - - make upload diff --git a/examples/arduino/server/avr/TimerOne.cpp b/examples/arduino/server/avr/TimerOne.cpp deleted file mode 100644 index efede8d..0000000 --- a/examples/arduino/server/avr/TimerOne.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/* - * Interrupt and PWM utilities for 16 bit Timer1 on ATmega168/328 - * Original code by Jesse Tane for http://labs.ideo.com August 2008 - * Modified March 2009 by Jérôme Despatis and Jesse Tane for ATmega328 support - * Modified June 2009 by Michael Polli and Jesse Tane to fix a bug in setPeriod() which caused the timer to stop - * Modified June 2011 by Lex Talionis to add a function to read the timer - * Modified Oct 2011 by Andrew Richards to avoid certain problems: - * - Add (long) assignments and casts to TimerOne::read() to ensure calculations involving tmp, ICR1 and TCNT1 aren't truncated - * - Ensure 16 bit registers accesses are atomic - run with interrupts disabled when accessing - * - Remove global enable of interrupts (sei())- could be running within an interrupt routine) - * - Disable interrupts whilst TCTN1 == 0. Datasheet vague on this, but experiment shows that overflow interrupt - * flag gets set whilst TCNT1 == 0, resulting in a phantom interrupt. Could just set to 1, but gets inaccurate - * at very short durations - * - startBottom() added to start counter at 0 and handle all interrupt enabling. - * - start() amended to enable interrupts - * - restart() amended to point at startBottom() - * Modiied 7:26 PM Sunday, October 09, 2011 by Lex Talionis - * - renamed start() to resume() to reflect it's actual role - * - renamed startBottom() to start(). This breaks some old code that expects start to continue counting where it left off - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - * See Google Code project http://code.google.com/p/arduino-timerone/ for latest - */ -#ifndef TIMERONE_cpp -#define TIMERONE_cpp - -#include "TimerOne.h" - -TimerOne Timer1; // preinstatiate - -ISR(TIMER1_OVF_vect) // interrupt service routine that wraps a user defined function supplied by attachInterrupt -{ - Timer1.isrCallback(); -} - - -void TimerOne::initialize(long microseconds) -{ - TCCR1A = 0; // clear control register A - TCCR1B = _BV(WGM13); // set mode 8: phase and frequency correct pwm, stop the timer - setPeriod(microseconds); -} - - -void TimerOne::setPeriod(long microseconds) // AR modified for atomic access -{ - - long cycles = (F_CPU / 2000000) * microseconds; // the counter runs backwards after TOP, interrupt is at BOTTOM so divide microseconds by 2 - if(cycles < RESOLUTION) clockSelectBits = _BV(CS10); // no prescale, full xtal - else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11); // prescale by /8 - else if((cycles >>= 3) < RESOLUTION) clockSelectBits = _BV(CS11) | _BV(CS10); // prescale by /64 - else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12); // prescale by /256 - else if((cycles >>= 2) < RESOLUTION) clockSelectBits = _BV(CS12) | _BV(CS10); // prescale by /1024 - else cycles = RESOLUTION - 1, clockSelectBits = _BV(CS12) | _BV(CS10); // request was out of bounds, set as maximum - - oldSREG = SREG; - cli(); // Disable interrupts for 16 bit register access - ICR1 = pwmPeriod = cycles; // ICR1 is TOP in p & f correct pwm mode - SREG = oldSREG; - - TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); - TCCR1B |= clockSelectBits; // reset clock select register, and starts the clock -} - -void TimerOne::setPwmDuty(char pin, int duty) -{ - unsigned long dutyCycle = pwmPeriod; - - dutyCycle *= duty; - dutyCycle >>= 10; - - oldSREG = SREG; - cli(); - if(pin == 1 || pin == 9) OCR1A = dutyCycle; - else if(pin == 2 || pin == 10) OCR1B = dutyCycle; - SREG = oldSREG; -} - -void TimerOne::pwm(char pin, int duty, long microseconds) // expects duty cycle to be 10 bit (1024) -{ - if(microseconds > 0) setPeriod(microseconds); - if(pin == 1 || pin == 9) { - DDRB |= _BV(PORTB1); // sets data direction register for pwm output pin - TCCR1A |= _BV(COM1A1); // activates the output pin - } - else if(pin == 2 || pin == 10) { - DDRB |= _BV(PORTB2); - TCCR1A |= _BV(COM1B1); - } - setPwmDuty(pin, duty); - resume(); // Lex - make sure the clock is running. We don't want to restart the count, in case we are starting the second WGM - // and the first one is in the middle of a cycle -} - -void TimerOne::disablePwm(char pin) -{ - if(pin == 1 || pin == 9) TCCR1A &= ~_BV(COM1A1); // clear the bit that enables pwm on PB1 - else if(pin == 2 || pin == 10) TCCR1A &= ~_BV(COM1B1); // clear the bit that enables pwm on PB2 -} - -void TimerOne::attachInterrupt(void (*isr)(), long microseconds) -{ - if(microseconds > 0) setPeriod(microseconds); - isrCallback = isr; // register the user's callback with the real ISR - TIMSK1 = _BV(TOIE1); // sets the timer overflow interrupt enable bit - // might be running with interrupts disabled (eg inside an ISR), so don't touch the global state -// sei(); - resume(); -} - -void TimerOne::detachInterrupt() -{ - TIMSK1 &= ~_BV(TOIE1); // clears the timer overflow interrupt enable bit - // timer continues to count without calling the isr -} - -void TimerOne::resume() // AR suggested -{ - TCCR1B |= clockSelectBits; -} - -void TimerOne::restart() // Depricated - Public interface to start at zero - Lex 10/9/2011 -{ - start(); -} - -void TimerOne::start() // AR addition, renamed by Lex to reflect it's actual role -{ - unsigned int tcnt1; - - TIMSK1 &= ~_BV(TOIE1); // AR added - GTCCR |= _BV(PSRSYNC); // AR added - reset prescaler (NB: shared with all 16 bit timers); - - oldSREG = SREG; // AR - save status register - cli(); // AR - Disable interrupts - TCNT1 = 0; - SREG = oldSREG; // AR - Restore status register - resume(); - do { // Nothing -- wait until timer moved on from zero - otherwise get a phantom interrupt - oldSREG = SREG; - cli(); - tcnt1 = TCNT1; - SREG = oldSREG; - } while (tcnt1==0); - -// TIFR1 = 0xff; // AR - Clear interrupt flags -// TIMSK1 = _BV(TOIE1); // sets the timer overflow interrupt enable bit -} - -void TimerOne::stop() -{ - TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12)); // clears all clock selects bits -} - -unsigned long TimerOne::read() //returns the value of the timer in microseconds -{ //rember! phase and freq correct mode counts up to then down again - unsigned long tmp; // AR amended to hold more than 65536 (could be nearly double this) - unsigned int tcnt1; // AR added - - oldSREG= SREG; - cli(); - tmp=TCNT1; - SREG = oldSREG; - - char scale=0; - switch (clockSelectBits) - { - case 1:// no prescalse - scale=0; - break; - case 2:// x8 prescale - scale=3; - break; - case 3:// x64 - scale=6; - break; - case 4:// x256 - scale=8; - break; - case 5:// x1024 - scale=10; - break; - } - - do { // Nothing -- max delay here is ~1023 cycles. AR modified - oldSREG = SREG; - cli(); - tcnt1 = TCNT1; - SREG = oldSREG; - } while (tcnt1==tmp); //if the timer has not ticked yet - - //if we are counting down add the top value to how far we have counted down - tmp = ( (tcnt1>tmp) ? (tmp) : (long)(ICR1-tcnt1)+(long)ICR1 ); // AR amended to add casts and reuse previous TCNT1 - return ((tmp*1000L)/(F_CPU /1000L))<. - * - * See Google Code project http://code.google.com/p/arduino-timerone/ for latest - */ -#ifndef TIMERONE_h -#define TIMERONE_h - -#include -#include - -#define RESOLUTION 65536 // Timer1 is 16 bit - -class TimerOne -{ - public: - - // properties - unsigned int pwmPeriod; - unsigned char clockSelectBits; - char oldSREG; // To hold Status Register while ints disabled - - // methods - void initialize(long microseconds=1000000); - void start(); - void stop(); - void restart(); - void resume(); - unsigned long read(); - void pwm(char pin, int duty, long microseconds=-1); - void disablePwm(char pin); - void attachInterrupt(void (*isr)(), long microseconds=-1); - void detachInterrupt(); - void setPeriod(long microseconds); - void setPwmDuty(char pin, int duty); - void (*isrCallback)(); -}; - -extern TimerOne Timer1; -#endif \ No newline at end of file diff --git a/examples/arduino/server/avr/Transport.cpp b/examples/arduino/server/avr/Transport.cpp deleted file mode 100644 index 6ffbb58..0000000 --- a/examples/arduino/server/avr/Transport.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/** @file Transport.cpp - * - */ -#ifndef TRANSPORT_CPP -#define TRANSPORT_CPP - -#include "Transport.h" - -void Transport::run() { - cMin.poll(); -} -//---------------------------------------------------------------------- - -void Transport::sendData() { - - startFrame(15U); - pack(this->_benchData.lTime); - pack(this->_benchData.dValue1); - pack(this->_benchData.fValue2); - pack(this->_benchData.iValue3); - pack(this->_benchData.cValue4); - sendFrame(10); - - startFrame(8U); - pack(this->_benchData.lTime); - pack(this->_trajData.dOutput); - sendFrame(11); -} -//---------------------------------------------------------------------- - -void Transport::handleFrame(uint8_t id, uint8_t *payload, uint8_t payloadLen) { - switch (id) { - case 1: - unpackExp(payload); - break; - case 12: - unpackBenchData(payload); - break; - case 13: - unpackTrajRampData(payload); - break; - default:; - } -} -//---------------------------------------------------------------------- - -void Transport::unpackExp(uint8_t *payload) { - uint8_t iData = 0; - startFrame(payload); - unPack(iData); - if (iData & 2) { - this->keepaliveTime = this->_benchData.lTime; - } else { - this->bActivateExperiment = iData & 1; - this->_benchData.lTime = 0; - } - -} -//---------------------------------------------------------------------- - -void Transport::unpackBenchData(uint8_t *payload) { - startFrame(payload); - unPack(this->_benchData.dValue1); - unPack(this->_benchData.fValue2); - unPack(this->_benchData.iValue3); - unPack(this->_benchData.cValue4); -} -//---------------------------------------------------------------------- - -void Transport::unpackTrajRampData(uint8_t *payload) { - startFrame(payload); - unPack(this->_trajData.dStartValue); - unPack(this->_trajData.lStartTime); - unPack(this->_trajData.dEndValue); - unPack(this->_trajData.lEndTime); -} -//---------------------------------------------------------------------- - -void Transport::pack32(uint32_t data, uint8_t payload[]) { - payload[0] = (uint8_t)((data & 0xff000000UL) >> 24); - payload[1] = (uint8_t)((data & 0x00ff0000UL) >> 16); - payload[2] = (uint8_t)((data & 0x0000ff00UL) >> 8); - payload[3] = (uint8_t)(data & 0x000000ffUL); -} -//---------------------------------------------------------------------- - -void Transport::pack(double dValue) { - pack32(*((uint32_t * ) & dValue), payload + cCursor); - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::pack(unsigned long lValue) { - pack32(*((uint32_t * ) & lValue), payload + cCursor); - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::pack(float fValue) { - pack32(*((uint32_t * ) & fValue), payload + cCursor); - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::pack(int iValue) { - payload[cCursor + 0] = (uint8_t)(((uint16_t) iValue & 0x0000ff00UL) >> 8); - payload[cCursor + 1] = (uint8_t)((uint16_t) iValue & 0x000000ffUL); - - cCursor += 2U; -} -//---------------------------------------------------------------------- - -void Transport::pack(unsigned char cValue) { - payload[cCursor + 0] = cValue; - - cCursor++; -} -//---------------------------------------------------------------------- - -uint32_t Transport::unPack32() { - uint32_t res = ((uint32_t)(payload[cCursor + 0]) << 24) | - ((uint32_t)(payload[cCursor + 1]) << 16) | - ((uint32_t)(payload[cCursor + 2]) << 8) | - (uint32_t)(payload[cCursor + 3]); - - return res; -} -//---------------------------------------------------------------------- - -void Transport::unPack(double &dValue) { - uint32_t res = unPack32(); - dValue = *(double *) &res; - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::unPack(unsigned long &lValue) { - uint32_t res = unPack32(); - lValue = *(unsigned long *) &res; - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::unPack(float &fValue) { - uint32_t res = unPack32(); - fValue = *(float *) &res; - - cCursor += 4U; -} -//---------------------------------------------------------------------- - -void Transport::unPack(int &iValue) { - iValue = ((uint32_t)(payload[cCursor + 0]) << 8) | - (uint32_t)(payload[cCursor + 1]); - - cCursor += 2U; -} -//---------------------------------------------------------------------- - -void Transport::unPack(unsigned char &cValue) { - cValue = payload[cCursor + 0]; - - cCursor++; -} -//---------------------------------------------------------------------- - -void Transport::unPack(bool &bValue) { - bValue = payload[cCursor + 0]; - - cCursor++; -} -//---------------------------------------------------------------------- - -void Transport::startFrame(unsigned char cSize) { - this->payload = new uint8_t[cSize]; - this->payloadSize = cSize; - cCursor = 0; -} -//---------------------------------------------------------------------- - -void Transport::startFrame(uint8_t *payload) { - this->payload = payload; - cCursor = 0; -} -//---------------------------------------------------------------------- - -void Transport::sendFrame(unsigned char id) { - cMin.queue_frame(id, payload, payloadSize); - delete[] payload; -} -//---------------------------------------------------------------------- - -#endif // TRANSPORT_CPP diff --git a/examples/arduino/server/avr/Transport.h b/examples/arduino/server/avr/Transport.h deleted file mode 100644 index f8c05f3..0000000 --- a/examples/arduino/server/avr/Transport.h +++ /dev/null @@ -1,203 +0,0 @@ -/** @file Transport.h - * - */ -#ifndef TRANSPORT_H -#define TRANSPORT_H - -#if defined(ARDUINO) && ARDUINO >= 100 -#include "Arduino.h" -#else - -#include "WProgram.h" - -#endif - -#define MAX_PAYLOAD (20) -#define TRANSPORT_MAX_WINDOW_SIZE (40U) -#define MIN_MAX_FRAMES (16) -#define MIN_MAX_FRAME_DATA (1<<8) -#define BUFLEN (64) - -#include "Min.h" - -/** - * @brief Layer class for the min protocol communication between the host the micro controller - */ -class Transport { -public: - Transport() : cMin(MIN_MAX_FRAMES, MIN_MAX_FRAME_DATA, MAX_PAYLOAD, BUFLEN) { - - static Transport *transportObj = this; - - cMin.add_application_function( - [](uint8_t id, uint8_t *payload, uint8_t len) { - transportObj->handleFrame(id, payload, len); - } - ); - } - - void init() { - cMin.initSerial(Serial); - } - - void run(); ///< must be called periodically, to ensure correct - ///< functionality of the class - - /** - * @brief Function for reading activity status of the experiment - * @return Experiment active - */ - bool runExp() { return this->bActivateExperiment; } - - /** - * @brief reset transport layer, whole rig - */ - void reset() { - this->bActivateExperiment = 0; - this->_benchData.lTime = 0; - } - - /** - * Handles incoming data frames. - * @param id frame identifier - * @param payload frame data as pointer - * @param payloadLen length of frame data - */ - void handleFrame(uint8_t id, uint8_t *payload, uint8_t payloadLen); - - /** - * @brief Function for sending the \ref benchData to the Host - */ - void sendData(); - - /** - * @brief struct of test rig data - */ - struct benchData { - unsigned long lTime = 0; ///< seconds since start of experiment - double dValue1 = 21454.23123; - float fValue2 = 556.2; - int iValue3 = 244; - unsigned char cValue4 = 12; - } _benchData; - - /** - * @brief struct of trajectory data - */ - struct trajData { - double dStartValue = 0.0; ///< start value of the trajectory - unsigned long lStartTime = 0; ///< start time of the trajectory - double dEndValue = 127.34; ///< end value of the trajectory - unsigned long lEndTime = 0; ///< end time of the trajectory - double dOutput = 85.0; ///< output value of the trajectory - } _trajData; - - unsigned long keepaliveTime = 0; - -private: - bool bActivateExperiment = false; - unsigned char cCursor = 0; - uint8_t *payload; - uint8_t payloadSize; - - void startFrame(unsigned char cSize); - - void startFrame(uint8_t *payload); - - void sendFrame(unsigned char id); - - void unpackExp(uint8_t *payload); - - void unpackBenchData(uint8_t *payload); - - void unpackTrajRampData(uint8_t *payload); - - Min cMin; - - /** - * Adds a double value to payload - * @param dValue value that is packed in payload - */ - void pack(double dValue); - - /** - * Adds an unsigned long value to payload - * @param lValue value that is packed in payload - */ - void pack(unsigned long lValue); - - /** - * Adds a float value to payload - * @param fValue value that is packed in payload - */ - void pack(float fValue); - - /** - * Adds an integer value to payload - * @param iValue value that is packed in payload - */ - void pack(int iValue); - - /** - * Adds an unsigned char value to payload - * @param cValue value that is packed in payload - */ - void pack(unsigned char cValue); - - /** - * Convert a unsigned 32 bit unsigned integer in a byte array - * @param data the 32 bit integer - * @param payload byte array - */ - void pack32(uint32_t data, uint8_t payload[]); - - /** - * Convert a 4 byte array to a 32 bit unsigned integer - * @return the 32 bit unsigned integer - */ - uint32_t unPack32(); - - /** - * Return a double value from payload - * @param dValue value with unpacked data - */ - void unPack(double &dValue); - - /** - * Return an unsigned long value from payload - * @param lValue value with unpacked data - */ - void unPack(unsigned long &lValue); - - /** - * Return a float value from payload - * @param fValue value with unpacked data - */ - void unPack(float &fValue); - - /** - * Return an integer value from payload - * @param iValue value with unpacked data - */ - void unPack(int &iValue); - - /** - * Return an unsigned char value from payload - * @param cValue value with unpacked data - */ - void unPack(unsigned char &cValue); - - /** - * Return a bool value from payload - * @param bValue value with unpacked data - */ - void unPack(bool &bValue); - -}; - -/** - * @brief external instance of the transport layer class must be declared in main - */ -extern Transport transport; - -#endif // TRANSPORT_H \ No newline at end of file diff --git a/examples/arduino/server/avr/makefile b/examples/arduino/server/avr/makefile deleted file mode 100644 index ab1978b..0000000 --- a/examples/arduino/server/avr/makefile +++ /dev/null @@ -1,16 +0,0 @@ -# Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile - -BOARD_TAG = uno - -ifeq ($(OS),Windows_NT) -ARDUINO_DIR = /c/Arduino/ -MONITOR_PORT = com4 - -include $(USERPROFILE)/Documents/Arduino-Makefile/Arduino.mk - -else -ARDUINO_DIR = /usr/share/arduino - -include $(HOME)/git/Arduino-Makefile/Arduino.mk - -endif diff --git a/examples/arduino/server/avr/server.ino b/examples/arduino/server/avr/server.ino deleted file mode 100644 index d7e4ef4..0000000 --- a/examples/arduino/server/avr/server.ino +++ /dev/null @@ -1,80 +0,0 @@ -/** @file server.ino - * - */ - -// Includes -#include "TimerOne.h" -#include "Transport.h" - -//---------------------------------------------------------------------- - -// System parameter -const unsigned long lDt = 100; ///< Sampling step [ms] -const unsigned long lKeepalive = 500; ///< keepalive time [ms] -//---------------------------------------------------------------------- - -// Communication -Transport transport; -//---------------------------------------------------------------------- - -/** - * @brief Method that calculates a trajectory value and writes the return value in _trajData->dOutput - * @param _benchData pointer to test rig data struct - * @param _trajData pointer to trajectory struct - */ -void fTrajectory(struct Transport::benchData *_benchData, struct Transport::trajData *_trajData) { - if (_benchData->lTime < _trajData->lStartTime) { - _trajData->dOutput = _trajData->dStartValue; - } else { - if (_benchData->lTime < _trajData->lEndTime) { - double dM = (_trajData->dEndValue - _trajData->dStartValue) / (_trajData->lEndTime - _trajData->lStartTime); - double dN = _trajData->dEndValue - dM * _trajData->lEndTime; - _trajData->dOutput = dM * _benchData->lTime + dN; - } else { - _trajData->dOutput = _trajData->dEndValue; - } - } -} -//---------------------------------------------------------------------- - -/** - * @brief continuous loop. Is called by Timer1 every \ref lDt milliseconds. - */ -void fContLoop() { - sei(); - if (transport.runExp()) { - transport._benchData.lTime += lDt; - - fTrajectory(&transport._benchData, &transport._trajData); - - transport.sendData(); - - // handle keepalive signal - if (lKeepalive != 0 && transport._benchData.lTime > transport.keepaliveTime + lKeepalive) { - transport.reset(); - } - } -} -//---------------------------------------------------------------------- - -/** - * @brief (arduino function) - * Initializes Transport protocol, the Timer, and the sensors. - */ -void setup() { - transport.init(); - - // initialize Timer - Timer1.initialize(lDt * 1000); - Timer1.attachInterrupt(fContLoop); -} -//---------------------------------------------------------------------- - -/** - * @brief (arduino function) - * Main Loop - */ -void loop() { - transport.run(); -} -//---------------------------------------------------------------------- From a514f7c626b9f4ff8e96d6700961a496f5b83e83 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Mon, 9 Dec 2024 16:06:23 +0100 Subject: [PATCH 02/16] Start restrucutre --- docs/examples/serial.rst | 11 ---- examples/generic/{server => }/CMakeLists.txt | 0 examples/generic/{server => }/README.md | 0 examples/generic/{server => }/codegen.py | 0 examples/generic/inc/doublePendulum.h | 28 ++++++++++ examples/generic/{server => }/inc/model.h | 0 examples/generic/server/local.cmake | 27 --------- examples/generic/{server => }/settings.py | 0 .../generic/{server/src => sim}/server.cpp | 0 examples/generic/stm/server.cpp | 56 +++++++++++++++++++ examples/generic/{client => visu}/README.rst | 0 .../generic/{client => visu}/connection.py | 0 .../generic/{client => visu}/default.sreg | 0 examples/generic/{client => visu}/main.py | 0 examples/generic/{client => visu}/settings.py | 0 .../generic/{client => visu}/testbench.py | 0 .../generic/{client => visu}/visualization.py | 0 17 files changed, 84 insertions(+), 38 deletions(-) rename examples/generic/{server => }/CMakeLists.txt (100%) rename examples/generic/{server => }/README.md (100%) rename examples/generic/{server => }/codegen.py (100%) create mode 100644 examples/generic/inc/doublePendulum.h rename examples/generic/{server => }/inc/model.h (100%) delete mode 100644 examples/generic/server/local.cmake rename examples/generic/{server => }/settings.py (100%) rename examples/generic/{server/src => sim}/server.cpp (100%) create mode 100644 examples/generic/stm/server.cpp rename examples/generic/{client => visu}/README.rst (100%) rename examples/generic/{client => visu}/connection.py (100%) rename examples/generic/{client => visu}/default.sreg (100%) rename examples/generic/{client => visu}/main.py (100%) rename examples/generic/{client => visu}/settings.py (100%) rename examples/generic/{client => visu}/testbench.py (100%) rename examples/generic/{client => visu}/visualization.py (100%) diff --git a/docs/examples/serial.rst b/docs/examples/serial.rst index e76d328..95685aa 100644 --- a/docs/examples/serial.rst +++ b/docs/examples/serial.rst @@ -6,14 +6,3 @@ Currently one implementation variant is available: .. contents:: :local: -Arduino -------- - -The example shows a `pyWisp` implementation for an UART communication with a µC (Arduino) and consists of two parts. - -The server part includes the whole Arduino files with the `Min`-Protocol. - -.. include:: ../../examples/arduino/server/README.rst - :start-after: sphinx-marker - -The visualization part includes the `pyWisp` project with all needed files. \ No newline at end of file diff --git a/examples/generic/server/CMakeLists.txt b/examples/generic/CMakeLists.txt similarity index 100% rename from examples/generic/server/CMakeLists.txt rename to examples/generic/CMakeLists.txt diff --git a/examples/generic/server/README.md b/examples/generic/README.md similarity index 100% rename from examples/generic/server/README.md rename to examples/generic/README.md diff --git a/examples/generic/server/codegen.py b/examples/generic/codegen.py similarity index 100% rename from examples/generic/server/codegen.py rename to examples/generic/codegen.py diff --git a/examples/generic/inc/doublePendulum.h b/examples/generic/inc/doublePendulum.h new file mode 100644 index 0000000..b353ca0 --- /dev/null +++ b/examples/generic/inc/doublePendulum.h @@ -0,0 +1,28 @@ +#pragma once +#include +#include +using Eigen::Matrix; +struct DoublePendulum { + using Input = Matrix; + using State = Matrix; + Input u{}; + State state{}; + void setInput(const Input &input) { u = input; } + void compute(uint32_t dt) { + auto x = state(0); + auto dx = state(1); + auto phi1 = state(2); + auto dphi1 = state(3); + auto phi2 = state(4); + auto dphi2 = state(5); + auto F = u(0); + State tmp{}; + tmp[0] = dx; + tmp[1] = (9.1441406250000024e-5*F*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00033782519531250004*F*pow(cos(phi1), 2) + 2.7105054312137611e-20*F*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 2.7105054312137611e-20*F*pow(cos(phi2), 2) - 0.0065634609374999988*F*pow(cos(phi1 - phi2), 2) + 0.024248341796874993*F + 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 2.5336889648437505e-5*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2) + 1.6940658945086007e-21*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00049225957031250002*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.0018186256347656245*pow(dphi1, 2)*sin(phi1) - 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 7.2391113281250003e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.00049225957031250002*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi1 - phi2) - 0.00051960732421874988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi2) + 4.0498762790596242e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) - 4.2748694056740464e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 8.4456298828125018e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2) - 8.4703294725430034e-22*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1 - phi2), 2) + 0.00060620854492187477*pow(dphi2, 2)*sin(phi2) - 8.0011230468750031e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*pow(cos(phi2), 2) + 0.00057430283203124995*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi2)*cos(phi1 - phi2) + 0.00047094610253906272*sin(phi1)*pow(cos(phi1), 3) - 0.00013455602929687502*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.033803464693359375*sin(phi1)*cos(phi1) + 0.0096581327695312493*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00013455602929687504*sin(phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 0.00014203136425781251*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.0096581327695312493*sin(phi2)*cos(phi1)*cos(phi1 - phi2) - 0.010194695701171871*sin(phi2)*cos(phi2))/(4.8006738281250019e-5*pow(cos(phi1), 4) - 2.7432421875000009e-5*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4478222656250004e-5*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00093270234375000013*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.0068916339843749994*pow(cos(phi1), 2) + 0.0019690382812499996*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0010392146484374998*pow(cos(phi2), 2) - 0.066947301562499978*pow(cos(phi1 - phi2), 2) + 0.24733308632812487); + tmp[2] = dphi1; + tmp[3] = (6.2753906250000025e-5*F*pow(cos(phi1), 3) - 1.7929687500000001e-5*F*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 3.3881317890172014e-21*F*cos(phi1)*pow(cos(phi2), 2) - 0.0045043359374999994*F*cos(phi1) + 0.0012869531249999999*F*cos(phi2)*cos(phi1 - phi2) + 4.7065429687500025e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 3) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 2.1175823681357508e-22*pow(dphi1, 2)*sin(phi1)*cos(phi1)*pow(cos(phi2), 2) - 0.00033782519531250004*pow(dphi1, 2)*sin(phi1)*cos(phi1) + 9.6521484374999995e-5*pow(dphi1, 2)*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi2) + 9.1441406250000024e-5*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 9.6521484375000009e-5*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.0065634609374999988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) + 7.9409338805090672e-25*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*pow(cos(phi2), 2) + 1.5688476562500007e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3) - 4.482421875000002e-7*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.0001126083984375*pow(dphi2, 2)*sin(phi2)*cos(phi1) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi2)*cos(phi2)*cos(phi1 - phi2) - 4.4824218750000015e-7*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00010668164062500003*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi2), 2) - 0.0076573710937499987*pow(dphi2, 2)*sin(phi1 - phi2) + 2.6383535156250014e-5*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi2), 2) - 0.006279281367187502*sin(phi1)*pow(cos(phi1), 2) - 4.3368086899420177e-19*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0018937515234375003*sin(phi1)*pow(cos(phi2), 2) + 1.3877787807814457e-17*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.45071286257812493*sin(phi1) - 2.6383535156250011e-5*sin(phi2)*pow(cos(phi1), 3)*cos(phi2) + 0.0017940803906250004*sin(phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 0.0018937515234374999*sin(phi2)*cos(phi1)*cos(phi2) - 0.12877510359374997*sin(phi2)*cos(phi1 - phi2))/(4.7065429687500025e-6*pow(cos(phi1), 4) - 2.6894531250000012e-6*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4194335937500006e-6*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 9.1441406250000024e-5*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00067565039062500009*pow(cos(phi1), 2) + 0.00019304296875000002*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00010188378906249999*pow(cos(phi2), 2) - 0.0065634609374999988*pow(cos(phi1 - phi2), 2) + 0.024248341796874993); + tmp[4] = dphi2; + tmp[5] = (-0.0095625000000000016*F*cos(phi1)*cos(phi1 - phi2) + 0.010093749999999999*F*cos(phi2) - 0.00071718750000000016*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi1 - phi2) + 0.00075703125*pow(dphi1, 2)*sin(phi1)*cos(phi2) + 0.00071718750000000016*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) - 0.051478124999999993*pow(dphi1, 2)*sin(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) - 0.00023906250000000004*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi1 - phi2) + 0.00025234375000000002*pow(dphi2, 2)*sin(phi2)*cos(phi2) + 0.00023906250000000004*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.01625625*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) - 0.014071218750000005*sin(phi1)*cos(phi1)*cos(phi2) + 0.95684287500000009*sin(phi1)*cos(phi1 - phi2) + 0.014071218750000003*sin(phi2)*pow(cos(phi1), 2) - 1.0100008124999997*sin(phi2))/(0.00083671875000000022*pow(cos(phi1), 2) - 0.00047812500000000009*cos(phi1)*cos(phi2)*cos(phi1 - phi2) + 0.00025234375000000002*pow(cos(phi2), 2) + 0.01625625*pow(cos(phi1 - phi2), 2) - 0.060057812499999988); + state += dt / 1000. * tmp; + }; +}; diff --git a/examples/generic/server/inc/model.h b/examples/generic/inc/model.h similarity index 100% rename from examples/generic/server/inc/model.h rename to examples/generic/inc/model.h diff --git a/examples/generic/server/local.cmake b/examples/generic/server/local.cmake deleted file mode 100644 index 51932a3..0000000 --- a/examples/generic/server/local.cmake +++ /dev/null @@ -1,27 +0,0 @@ -# LOCAL COPY DEPENDENCY MANAGEMENT -## in case you already have a copy of a dependency and -## don't want to download it for every project separately, -## define a variable named -## FETCHCONTENT_SOURCE_DIR_ -## that points to the PATH of the dependency. -## is the UPPER_CASED name of the dependency -# WARNING: -## Take note the CMAKE will NOT look at this path to -## make sure the dependency is available if you use this. - -# GIT -## make sure that your local updates of this file don't -## get pushed to other people, as their setup differs -## from yours. -## To achieve this, just run the following git command -## in the folder that contains this file -# $ git update-index --skip-worktree local.cmake - -# EXAMPLES -## uncomment the following line if you have a local copy -## of TOOL-LIBS and don't want to download it for every -## project separately. Make sure the PATH points to it. -# WARNING: -## FetchContent will NOT update TOOL-LIBS if you do this. -## It will NOT make sure the correct TAG is checked out. -#set(FETCHCONTENT_SOURCE_DIR_TOOL-LIBS $ENV{HOME}/git/tool-libs/) diff --git a/examples/generic/server/settings.py b/examples/generic/settings.py similarity index 100% rename from examples/generic/server/settings.py rename to examples/generic/settings.py diff --git a/examples/generic/server/src/server.cpp b/examples/generic/sim/server.cpp similarity index 100% rename from examples/generic/server/src/server.cpp rename to examples/generic/sim/server.cpp diff --git a/examples/generic/stm/server.cpp b/examples/generic/stm/server.cpp new file mode 100644 index 0000000..36b6c99 --- /dev/null +++ b/examples/generic/stm/server.cpp @@ -0,0 +1,56 @@ +/** @file server.cpp + * + * Copyright (c) 2023 IACE + */ +#include +#include +#include +#include "model.h" + +#include "comm/min.h" +#include "comm/bufferutils.h" +#include "comm/line.h" + +TTY tty{"/dev/tty"}; +UDP udp{"127.0.0.1", 45670}; + +struct Support { + Support(); + void init(); + LineDelimiter lineout; + Min min; +}; + +Support::Support() + : lineout{tty} + , min{.in = udp, .out = udp} +{} +void Support::init() { + /* e.setHeartbeatTimeout(500, min.out); */ + k.every(500, + [](uint32_t time, uint32_t) { + uint32_t freq{1000}; + static bool state{false}; + freq = e.state==e.IDLE?2000:freq; + freq = e.state==e.RUN?500:freq; + if (time % freq == 0) { + state = !state; + k.log.info("led (would be) %s", state?"on":"off"); + } + }); + e.onEvent(e.STOP).call([](uint32_t){abort();}); +} + +Kernel k; +Support support; +Experiment e{&support.min.reg}; + +int main() { + support.init(); + k.initLog(support.lineout); + k.every(1, support.min, &Min::poll); + + Model model{support.min.out}; + model.init(support.min.reg); + k.run(); +} diff --git a/examples/generic/client/README.rst b/examples/generic/visu/README.rst similarity index 100% rename from examples/generic/client/README.rst rename to examples/generic/visu/README.rst diff --git a/examples/generic/client/connection.py b/examples/generic/visu/connection.py similarity index 100% rename from examples/generic/client/connection.py rename to examples/generic/visu/connection.py diff --git a/examples/generic/client/default.sreg b/examples/generic/visu/default.sreg similarity index 100% rename from examples/generic/client/default.sreg rename to examples/generic/visu/default.sreg diff --git a/examples/generic/client/main.py b/examples/generic/visu/main.py similarity index 100% rename from examples/generic/client/main.py rename to examples/generic/visu/main.py diff --git a/examples/generic/client/settings.py b/examples/generic/visu/settings.py similarity index 100% rename from examples/generic/client/settings.py rename to examples/generic/visu/settings.py diff --git a/examples/generic/client/testbench.py b/examples/generic/visu/testbench.py similarity index 100% rename from examples/generic/client/testbench.py rename to examples/generic/visu/testbench.py diff --git a/examples/generic/client/visualization.py b/examples/generic/visu/visualization.py similarity index 100% rename from examples/generic/client/visualization.py rename to examples/generic/visu/visualization.py From b33eca6d2637b0ca2395c412c489c220c93e1602 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Tue, 10 Dec 2024 01:37:12 +0100 Subject: [PATCH 03/16] Rework example --- examples/generic/CMakeLists.txt | 48 ++--- examples/generic/README.md | 15 -- examples/generic/README.rst | 79 +++++++ examples/generic/inc/Pendulum.h | 20 ++ examples/generic/inc/doublePendulum.h | 28 --- examples/generic/inc/model.h | 36 +--- examples/generic/inc/support.h | 8 + examples/generic/inc/trajType.h | 81 +++++++ examples/generic/{ => scripts}/codegen.py | 19 +- examples/generic/{ => scripts}/settings.py | 0 examples/generic/sim/CMakeLists.txt | 29 +++ examples/generic/sim/NonLinDoublePendulum.h | 32 +++ examples/generic/sim/server.cpp | 56 ----- examples/generic/sim/sim.cpp | 62 ++++++ examples/generic/stm/CMakeLists.txt | 51 +++++ examples/generic/stm/f4.cpp | 225 ++++++++++++++++++++ examples/generic/stm/server.cpp | 56 ----- 17 files changed, 616 insertions(+), 229 deletions(-) delete mode 100644 examples/generic/README.md create mode 100644 examples/generic/README.rst create mode 100644 examples/generic/inc/Pendulum.h delete mode 100644 examples/generic/inc/doublePendulum.h create mode 100644 examples/generic/inc/support.h create mode 100644 examples/generic/inc/trajType.h rename examples/generic/{ => scripts}/codegen.py (71%) rename examples/generic/{ => scripts}/settings.py (100%) create mode 100644 examples/generic/sim/CMakeLists.txt create mode 100644 examples/generic/sim/NonLinDoublePendulum.h delete mode 100644 examples/generic/sim/server.cpp create mode 100644 examples/generic/sim/sim.cpp create mode 100644 examples/generic/stm/CMakeLists.txt create mode 100644 examples/generic/stm/f4.cpp delete mode 100644 examples/generic/stm/server.cpp diff --git a/examples/generic/CMakeLists.txt b/examples/generic/CMakeLists.txt index 781b530..4097adc 100644 --- a/examples/generic/CMakeLists.txt +++ b/examples/generic/CMakeLists.txt @@ -1,41 +1,27 @@ -cmake_minimum_required(VERSION 3.25) +cmake_minimum_required(VERSION 3.24) + +option(SIM "build for host system instead of cross-compiling" TRUE) -set(CMAKE_CXX_STANDARD 17) ## dependencies -include(FetchContent) -include(ExternalProject) include(local.cmake OPTIONAL) +include(dependencies.cmake) -FetchContent_Declare(tool-libs - GIT_REPOSITORY https://github.com/umit-iace/tool-libs - GIT_TAG master - GIT_PROGRESS TRUE -) -FetchContent_MakeAvailable(tool-libs) ## Project declaration -project(server C CXX ASM) +project(Pendulum C CXX ASM) -find_package(Eigen3 3.3 REQUIRED NO_MODULE) +if (NOT SIM) + message("preparing cross-compilation") + add_subdirectory(stm) +else() + message("preparing build for host system") + add_subdirectory(bindings) + add_subdirectory(sim) +endif() -add_custom_command( - OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/inc/doublependulum.h - COMMAND python codegen.py +add_custom_target(visu + COMMAND pipenv run python visu/main.py + COMMENT "run Pendulum visualization" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/settings.py - ${CMAKE_CURRENT_SOURCE_DIR}/codegen.py - VERBATIM - ) - -add_executable(${PROJECT_NAME} src/server.cpp inc/doublependulum.h) +) -target_include_directories(${PROJECT_NAME} PUBLIC inc) -target_link_libraries(${PROJECT_NAME} - tool-libs - tool-libs-linux - Eigen3::Eigen) -add_custom_target(run - DEPENDS ${PROJECT_NAME} - COMMAND ./${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - ) diff --git a/examples/generic/README.md b/examples/generic/README.md deleted file mode 100644 index ff17c50..0000000 --- a/examples/generic/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Tool-Libs server example - -This is an example of how a testing rig can be implemented using the tool-libs -framework. The system equations are generated using a sympy c-printer, after -solving the system equations there. - -## usage - -CMake takes care of generating the equations when necessary. -Make sure you have `ncat` installed (from the `nmap` project) and run -```shell -cmake -S . -B build -cmake --build build --target run -``` -then connect through pywisp diff --git a/examples/generic/README.rst b/examples/generic/README.rst new file mode 100644 index 0000000..ee9a87d --- /dev/null +++ b/examples/generic/README.rst @@ -0,0 +1,79 @@ +Tool-Libs example: Double Pendulum +================================== + +This is an example of how a testing rig can be implemented using the tool-libs +framework. The system equations are generated using a sympy c-printer, after +solving the system equations there. + +Requirements +------------ + +* >=Python 3.8 +* >= uv +* Eigen library + +Simulation +---------- + +* Configure + + .. code-block:: bash + + $ cmake -S . -B build-sim -D SIM=1 + +* Build and run the simulation + + .. code-block:: bash + + $ make -C build-sim run + + or + + .. code-block:: bash + + $ cmake --build build-sim --target run + + +* Connect via PyWisp and control the simulation + + .. code-block:: bash + + $ make -C build-sim visu + + +STM +--- + +* Configure + + .. code-block:: bash + + $ cmake -S . -B build-stm -D SIM=0 + + +* Build the STM code + + .. code-block:: bash + + $ make -C build-stm stm + + +* Upload + + .. code-block:: bash + + $ make -C build-stm upload + + +* Upload via OTA + + .. code-block:: bash + + $ make -C build-stm pload_ota UPLOAD= + + +* Connect via PyWisp and control the simulation + + .. code-block:: bash + + $ make -C build-sim visu diff --git a/examples/generic/inc/Pendulum.h b/examples/generic/inc/Pendulum.h new file mode 100644 index 0000000..91f992d --- /dev/null +++ b/examples/generic/inc/Pendulum.h @@ -0,0 +1,20 @@ +/** @file Pendulum.h + * + * Copyright (c) 2024 IACE + */ +#ifndef PENDULUM_H +#define PENDULUM_H + +struct Pendulum { + using Input = DoublePendulum::Input; + using State = DoublePendulum::State; + + Later u; + DoublePendulum p{}; + State& state{p.state}; + operator const State&() { return state; } + void step(uint32_t, uint32_t dt) { + p.setInput(u.get()); + p.compute(dt); + } +}; diff --git a/examples/generic/inc/doublePendulum.h b/examples/generic/inc/doublePendulum.h deleted file mode 100644 index b353ca0..0000000 --- a/examples/generic/inc/doublePendulum.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once -#include -#include -using Eigen::Matrix; -struct DoublePendulum { - using Input = Matrix; - using State = Matrix; - Input u{}; - State state{}; - void setInput(const Input &input) { u = input; } - void compute(uint32_t dt) { - auto x = state(0); - auto dx = state(1); - auto phi1 = state(2); - auto dphi1 = state(3); - auto phi2 = state(4); - auto dphi2 = state(5); - auto F = u(0); - State tmp{}; - tmp[0] = dx; - tmp[1] = (9.1441406250000024e-5*F*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00033782519531250004*F*pow(cos(phi1), 2) + 2.7105054312137611e-20*F*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 2.7105054312137611e-20*F*pow(cos(phi2), 2) - 0.0065634609374999988*F*pow(cos(phi1 - phi2), 2) + 0.024248341796874993*F + 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 2.5336889648437505e-5*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2) + 1.6940658945086007e-21*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00049225957031250002*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.0018186256347656245*pow(dphi1, 2)*sin(phi1) - 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 7.2391113281250003e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.00049225957031250002*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi1 - phi2) - 0.00051960732421874988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi2) + 4.0498762790596242e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) - 4.2748694056740464e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 8.4456298828125018e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2) - 8.4703294725430034e-22*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1 - phi2), 2) + 0.00060620854492187477*pow(dphi2, 2)*sin(phi2) - 8.0011230468750031e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*pow(cos(phi2), 2) + 0.00057430283203124995*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi2)*cos(phi1 - phi2) + 0.00047094610253906272*sin(phi1)*pow(cos(phi1), 3) - 0.00013455602929687502*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.033803464693359375*sin(phi1)*cos(phi1) + 0.0096581327695312493*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00013455602929687504*sin(phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 0.00014203136425781251*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.0096581327695312493*sin(phi2)*cos(phi1)*cos(phi1 - phi2) - 0.010194695701171871*sin(phi2)*cos(phi2))/(4.8006738281250019e-5*pow(cos(phi1), 4) - 2.7432421875000009e-5*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4478222656250004e-5*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00093270234375000013*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.0068916339843749994*pow(cos(phi1), 2) + 0.0019690382812499996*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0010392146484374998*pow(cos(phi2), 2) - 0.066947301562499978*pow(cos(phi1 - phi2), 2) + 0.24733308632812487); - tmp[2] = dphi1; - tmp[3] = (6.2753906250000025e-5*F*pow(cos(phi1), 3) - 1.7929687500000001e-5*F*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 3.3881317890172014e-21*F*cos(phi1)*pow(cos(phi2), 2) - 0.0045043359374999994*F*cos(phi1) + 0.0012869531249999999*F*cos(phi2)*cos(phi1 - phi2) + 4.7065429687500025e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 3) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 2.1175823681357508e-22*pow(dphi1, 2)*sin(phi1)*cos(phi1)*pow(cos(phi2), 2) - 0.00033782519531250004*pow(dphi1, 2)*sin(phi1)*cos(phi1) + 9.6521484374999995e-5*pow(dphi1, 2)*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi2) + 9.1441406250000024e-5*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 9.6521484375000009e-5*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.0065634609374999988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) + 7.9409338805090672e-25*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*pow(cos(phi2), 2) + 1.5688476562500007e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3) - 4.482421875000002e-7*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.0001126083984375*pow(dphi2, 2)*sin(phi2)*cos(phi1) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi2)*cos(phi2)*cos(phi1 - phi2) - 4.4824218750000015e-7*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00010668164062500003*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi2), 2) - 0.0076573710937499987*pow(dphi2, 2)*sin(phi1 - phi2) + 2.6383535156250014e-5*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi2), 2) - 0.006279281367187502*sin(phi1)*pow(cos(phi1), 2) - 4.3368086899420177e-19*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0018937515234375003*sin(phi1)*pow(cos(phi2), 2) + 1.3877787807814457e-17*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.45071286257812493*sin(phi1) - 2.6383535156250011e-5*sin(phi2)*pow(cos(phi1), 3)*cos(phi2) + 0.0017940803906250004*sin(phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 0.0018937515234374999*sin(phi2)*cos(phi1)*cos(phi2) - 0.12877510359374997*sin(phi2)*cos(phi1 - phi2))/(4.7065429687500025e-6*pow(cos(phi1), 4) - 2.6894531250000012e-6*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4194335937500006e-6*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 9.1441406250000024e-5*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00067565039062500009*pow(cos(phi1), 2) + 0.00019304296875000002*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00010188378906249999*pow(cos(phi2), 2) - 0.0065634609374999988*pow(cos(phi1 - phi2), 2) + 0.024248341796874993); - tmp[4] = dphi2; - tmp[5] = (-0.0095625000000000016*F*cos(phi1)*cos(phi1 - phi2) + 0.010093749999999999*F*cos(phi2) - 0.00071718750000000016*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi1 - phi2) + 0.00075703125*pow(dphi1, 2)*sin(phi1)*cos(phi2) + 0.00071718750000000016*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) - 0.051478124999999993*pow(dphi1, 2)*sin(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) - 0.00023906250000000004*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi1 - phi2) + 0.00025234375000000002*pow(dphi2, 2)*sin(phi2)*cos(phi2) + 0.00023906250000000004*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.01625625*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) - 0.014071218750000005*sin(phi1)*cos(phi1)*cos(phi2) + 0.95684287500000009*sin(phi1)*cos(phi1 - phi2) + 0.014071218750000003*sin(phi2)*pow(cos(phi1), 2) - 1.0100008124999997*sin(phi2))/(0.00083671875000000022*pow(cos(phi1), 2) - 0.00047812500000000009*cos(phi1)*cos(phi2)*cos(phi1 - phi2) + 0.00025234375000000002*pow(cos(phi2), 2) + 0.01625625*pow(cos(phi1 - phi2), 2) - 0.060057812499999988); - state += dt / 1000. * tmp; - }; -}; diff --git a/examples/generic/inc/model.h b/examples/generic/inc/model.h index f0dbcdc..7c8d973 100644 --- a/examples/generic/inc/model.h +++ b/examples/generic/inc/model.h @@ -1,35 +1,15 @@ -#pragma once +/** @file Model.h + * + * Copyright (c) 2024 IACE + */ +#ifndef MODEL_H +#define MODEL_H #include #include #include #include -#include "doublependulum.h" - -struct Pendulum { - using Input = DoublePendulum::Input; - using State = DoublePendulum::State; - - Later u; - DoublePendulum p{}; - State& state{p.state}; - operator const State&() { return state; } - void step(uint32_t, uint32_t dt) { - p.setInput(u.get()); - p.compute(dt); - } -}; - -struct Trajectory { - using Reference = Pendulum::Input; - Reference ref{}; - LinearTrajectory interp{}; - operator Reference&() { return ref; } - void step(uint32_t time, uint32_t) { - ref(0) = interp(time/1000.)[0]; - } -}; struct Model { Sink &out; @@ -55,6 +35,7 @@ struct Model { // frames min.setHandler(21, *this, &Model::getTrajData); } + void sendData(uint32_t t, uint32_t dt) { Frame f{10}; f.pack(t); @@ -65,6 +46,7 @@ struct Model { out.push(std::move(f)); } + void getTrajData(Frame &f) { static SeriesUnpacker su; auto buf = su.unpack(f); @@ -73,3 +55,5 @@ struct Model { } } }; + +#endif //MODEL_H diff --git a/examples/generic/inc/support.h b/examples/generic/inc/support.h new file mode 100644 index 0000000..4fe8c48 --- /dev/null +++ b/examples/generic/inc/support.h @@ -0,0 +1,8 @@ +#pragma once +#include + +extern struct Support { + Support(); + void init(); + Min min; +} support; diff --git a/examples/generic/inc/trajType.h b/examples/generic/inc/trajType.h new file mode 100644 index 0000000..cb8103c --- /dev/null +++ b/examples/generic/inc/trajType.h @@ -0,0 +1,81 @@ +/** @file TrajType.h + * + * Copyright (c) 2024 IACE + */ +#ifndef TRAJTYPE_H +#define TRAJTYPE_H + +#include +#include + +#include "Pendulum.h" + +struct TrajType { + using Des = Reference<5>; + using Out = Pendulum::Input; + + Des des; + Out out{}; + + std::variant traj{5}; + enum Type:uint8_t {NONE, LIN, POLY} type{NONE}; + + void step(uint32_t time, uint32_t) { + double t = time * 0.001; + switch(type) { + case LIN: { + des = std::get(traj).getValue(t); + break; + } + case POLY: { + des = std::get(traj).getValue(t); + break; + } + case NONE: + return; + } + out = des[0]; + } + + void reset() { + this->out = {}; + this->des = {0, 0, 0, 0, 0}; + } + + void mkType(Type t) { + if (type == t) return; + type = (Type) t; + reset(); + + switch(t) { + case LIN: { + traj = LinearTrajectory(5); + break; + } + case POLY: { + traj = SmoothTrajectory({126, -420, 540, -315, 70}, 9); + break; + } + case NONE: break; + } + } + + void setData(Frame &f) { + static SeriesUnpacker dData; + if (dData.unpack(f)) { + switch(type) { + case LIN: { + std::get(traj).setData(std::move(dData.buf)); + break; + } + case POLY: { + std::get(traj).setData(std::move(dData.buf)); + break; + } + case NONE: break; + } + } + } +}; + +#endif //TRAJTYPE_H diff --git a/examples/generic/codegen.py b/examples/generic/scripts/codegen.py similarity index 71% rename from examples/generic/codegen.py rename to examples/generic/scripts/codegen.py index 149691e..49927e1 100644 --- a/examples/generic/codegen.py +++ b/examples/generic/scripts/codegen.py @@ -36,28 +36,13 @@ newline = '\n' -with open('inc/doublependulum.h', 'w') as file: - file.write(f"""/** This file is autogenerated, do not edit */ -#pragma once -#include -#include -using Eigen::Matrix; -struct DoublePendulum {{ - using Input = Matrix; - using State = Matrix; - Input u{{}}; - State state{{}}; - void setInput(const Input &input) {{ u = input; }} - void compute(uint32_t dt) {{ -{f"{newline}".join([ +print(f"""{f"{newline}".join([ f" auto {x} = state({i});" for i, x in enumerate(stateVars) ])} {f"{newline}".join([ f" auto {x} = u({i});" for i, x in enumerate(inputVars) ])} - State tmp{{}}; - {ccode(solution, assign_to="tmp")} - state += dt / 1000. * tmp; + {ccode(solution, assign_to="tmp")} }}; }}; """) diff --git a/examples/generic/settings.py b/examples/generic/scripts/settings.py similarity index 100% rename from examples/generic/settings.py rename to examples/generic/scripts/settings.py diff --git a/examples/generic/sim/CMakeLists.txt b/examples/generic/sim/CMakeLists.txt new file mode 100644 index 0000000..7cdebfa --- /dev/null +++ b/examples/generic/sim/CMakeLists.txt @@ -0,0 +1,29 @@ +add_executable(sim sim.cpp) + +target_include_directories(sim PUBLIC . + ../inc +) +target_link_libraries(sim + tool-libs + tool-libs-linux + Eigen3::Eigen +) + +add_custom_target(run + DEPENDS sim + COMMAND ./sim + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "run simulation" +) +add_custom_target(debug + DEPENDS sim + COMMAND gdb ./sim + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "debug simulation using gdb" +) +add_custom_target(grind + DEPENDS sim + COMMAND valgrind -s --leak-check=full --track-origins=yes ./sim + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} + COMMENT "valgrind simulation" +) diff --git a/examples/generic/sim/NonLinDoublePendulum.h b/examples/generic/sim/NonLinDoublePendulum.h new file mode 100644 index 0000000..79fc81c --- /dev/null +++ b/examples/generic/sim/NonLinDoublePendulum.h @@ -0,0 +1,32 @@ +/** @file NonLinDoublePendulum.h + * + * Copyright (c) 2023 IACE + */ +#include +using Eigen::Matrix; + +namespace NonLinDoublePendulum { + using State = Pendulum::State; + using Input = Pendulum::Input; + + void step(State x, Input u) { + auto p = state(0); + auto dp = state(1); + auto phi1 = state(2); + auto dphi1 = state(3); + auto phi2 = state(4); + auto dphi2 = state(5); + auto F = u(0); + + State dx { + dp, + (9.1441406250000024e-5*F*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00033782519531250004*F*pow(cos(phi1), 2) + 2.7105054312137611e-20*F*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 2.7105054312137611e-20*F*pow(cos(phi2), 2) - 0.0065634609374999988*F*pow(cos(phi1 - phi2), 2) + 0.024248341796874993*F + 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 2.5336889648437505e-5*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2) + 1.6940658945086007e-21*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00049225957031250002*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.0018186256347656245*pow(dphi1, 2)*sin(phi1) - 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 7.2391113281250003e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.00049225957031250002*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi1 - phi2) - 0.00051960732421874988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi2) + 4.0498762790596242e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) - 4.2748694056740464e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 8.4456298828125018e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2) - 8.4703294725430034e-22*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1 - phi2), 2) + 0.00060620854492187477*pow(dphi2, 2)*sin(phi2) - 8.0011230468750031e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*pow(cos(phi2), 2) + 0.00057430283203124995*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi2)*cos(phi1 - phi2) + 0.00047094610253906272*sin(phi1)*pow(cos(phi1), 3) - 0.00013455602929687502*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.033803464693359375*sin(phi1)*cos(phi1) + 0.0096581327695312493*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00013455602929687504*sin(phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 0.00014203136425781251*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.0096581327695312493*sin(phi2)*cos(phi1)*cos(phi1 - phi2) - 0.010194695701171871*sin(phi2)*cos(phi2))/(4.8006738281250019e-5*pow(cos(phi1), 4) - 2.7432421875000009e-5*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4478222656250004e-5*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00093270234375000013*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.0068916339843749994*pow(cos(phi1), 2) + 0.0019690382812499996*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0010392146484374998*pow(cos(phi2), 2) - 0.066947301562499978*pow(cos(phi1 - phi2), 2) + 0.24733308632812487), + dphi1, + (6.2753906250000025e-5*F*pow(cos(phi1), 3) - 1.7929687500000001e-5*F*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 3.3881317890172014e-21*F*cos(phi1)*pow(cos(phi2), 2) - 0.0045043359374999994*F*cos(phi1) + 0.0012869531249999999*F*cos(phi2)*cos(phi1 - phi2) + 4.7065429687500025e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 3) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 2.1175823681357508e-22*pow(dphi1, 2)*sin(phi1)*cos(phi1)*pow(cos(phi2), 2) - 0.00033782519531250004*pow(dphi1, 2)*sin(phi1)*cos(phi1) + 9.6521484374999995e-5*pow(dphi1, 2)*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi2) + 9.1441406250000024e-5*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 9.6521484375000009e-5*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.0065634609374999988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) + 7.9409338805090672e-25*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*pow(cos(phi2), 2) + 1.5688476562500007e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3) - 4.482421875000002e-7*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.0001126083984375*pow(dphi2, 2)*sin(phi2)*cos(phi1) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi2)*cos(phi2)*cos(phi1 - phi2) - 4.4824218750000015e-7*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00010668164062500003*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi2), 2) - 0.0076573710937499987*pow(dphi2, 2)*sin(phi1 - phi2) + 2.6383535156250014e-5*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi2), 2) - 0.006279281367187502*sin(phi1)*pow(cos(phi1), 2) - 4.3368086899420177e-19*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0018937515234375003*sin(phi1)*pow(cos(phi2), 2) + 1.3877787807814457e-17*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.45071286257812493*sin(phi1) - 2.6383535156250011e-5*sin(phi2)*pow(cos(phi1), 3)*cos(phi2) + 0.0017940803906250004*sin(phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 0.0018937515234374999*sin(phi2)*cos(phi1)*cos(phi2) - 0.12877510359374997*sin(phi2)*cos(phi1 - phi2))/(4.7065429687500025e-6*pow(cos(phi1), 4) - 2.6894531250000012e-6*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4194335937500006e-6*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 9.1441406250000024e-5*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00067565039062500009*pow(cos(phi1), 2) + 0.00019304296875000002*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00010188378906249999*pow(cos(phi2), 2) - 0.0065634609374999988*pow(cos(phi1 - phi2), 2) + 0.024248341796874993), + dphi2, + (-0.0095625000000000016*F*cos(phi1)*cos(phi1 - phi2) + 0.010093749999999999*F*cos(phi2) - 0.00071718750000000016*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi1 - phi2) + 0.00075703125*pow(dphi1, 2)*sin(phi1)*cos(phi2) + 0.00071718750000000016*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) - 0.051478124999999993*pow(dphi1, 2)*sin(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) - 0.00023906250000000004*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi1 - phi2) + 0.00025234375000000002*pow(dphi2, 2)*sin(phi2)*cos(phi2) + 0.00023906250000000004*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.01625625*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) - 0.014071218750000005*sin(phi1)*cos(phi1)*cos(phi2) + 0.95684287500000009*sin(phi1)*cos(phi1 - phi2) + 0.014071218750000003*sin(phi2)*pow(cos(phi1), 2) - 1.0100008124999997*sin(phi2))/(0.00083671875000000022*pow(cos(phi1), 2) - 0.00047812500000000009*cos(phi1)*cos(phi2)*cos(phi1 - phi2) + 0.00025234375000000002*pow(cos(phi2), 2) + 0.01625625*pow(cos(phi1 - phi2), 2) - 0.060057812499999988), + }; + + return dx; + }; +}; diff --git a/examples/generic/sim/server.cpp b/examples/generic/sim/server.cpp deleted file mode 100644 index 36b6c99..0000000 --- a/examples/generic/sim/server.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** @file server.cpp - * - * Copyright (c) 2023 IACE - */ -#include -#include -#include -#include "model.h" - -#include "comm/min.h" -#include "comm/bufferutils.h" -#include "comm/line.h" - -TTY tty{"/dev/tty"}; -UDP udp{"127.0.0.1", 45670}; - -struct Support { - Support(); - void init(); - LineDelimiter lineout; - Min min; -}; - -Support::Support() - : lineout{tty} - , min{.in = udp, .out = udp} -{} -void Support::init() { - /* e.setHeartbeatTimeout(500, min.out); */ - k.every(500, - [](uint32_t time, uint32_t) { - uint32_t freq{1000}; - static bool state{false}; - freq = e.state==e.IDLE?2000:freq; - freq = e.state==e.RUN?500:freq; - if (time % freq == 0) { - state = !state; - k.log.info("led (would be) %s", state?"on":"off"); - } - }); - e.onEvent(e.STOP).call([](uint32_t){abort();}); -} - -Kernel k; -Support support; -Experiment e{&support.min.reg}; - -int main() { - support.init(); - k.initLog(support.lineout); - k.every(1, support.min, &Min::poll); - - Model model{support.min.out}; - model.init(support.min.reg); - k.run(); -} diff --git a/examples/generic/sim/sim.cpp b/examples/generic/sim/sim.cpp new file mode 100644 index 0000000..674c51f --- /dev/null +++ b/examples/generic/sim/sim.cpp @@ -0,0 +1,62 @@ +/** @file sim.cpp + * + * Copyright (c) 2024 IACE + */ +#include +#include +#include +#include "model.h" +#include "NonLinDoublePendulum.h" + +TTY tty{"/dev/tty"}; +UDP udp{"127.0.0.1", 45670}; + +void Pendulum::updateState(uint32_t time, uint32_t dt_ms) { + double dt = dt_ms / 1000.; + + state = state + dt * NonLinDoublePendulum::step(state, u); + + k.log.print("%g, %g, %g, %g, %g, %g, %g, %g\n", + state(0), state(1), + state(2), state(3), + state(4), state(5), + u, + dt + ); +} + +void Pendulum::reset() { + state.setZero(); + input.setZero(); +} + +Support::Support() : min{.in = udp, .out = udp }{} + +void Support::init() { + e.onEvent(e.STOP).call([](uint32_t) { + u = Pendulum::Input{}; + }); + + e.onEvent(e.INIT).call([](uint32_t) { + k.log.print("starting simulation"); + }); +} + +Kernel k; +Support support; +Experiment e{&support.min.reg}; + +Kernel k; +Support support; +Experiment e{&support.min.reg}; + +int main() { + k.initLog(tty); + k.setTimeStep(getSimTimeStep(argc, argv)); + k.every(1, support.min, &Min::poll); + support.init(); + + Model m; + m.init(); + k.run(); +} diff --git a/examples/generic/stm/CMakeLists.txt b/examples/generic/stm/CMakeLists.txt new file mode 100644 index 0000000..5c17ef7 --- /dev/null +++ b/examples/generic/stm/CMakeLists.txt @@ -0,0 +1,51 @@ +## config +stm32_fetch_cmsis(F4) +stm32_fetch_hal(F4) + +find_package(CMSIS COMPONENTS STM32F401RC REQUIRED) +find_package(HAL COMPONENTS STM32F401RC REQUIRED) + +add_executable(stm ${PROJECT_SOURCES} f4.cpp) + +target_compile_options(stm PUBLIC + # -fanalyzer + -Wall + # -Wextra +) + +target_compile_features(stm PRIVATE cxx_std_17) + +target_include_directories(stm PUBLIC . + ../inc +) + +target_link_options(stm PUBLIC + LINKER:--print-memory-usage +) + +target_link_libraries(stm + CMSIS::STM32::F401RC + HAL::STM32::F4::CORTEX + HAL::STM32::F4::DMA + HAL::STM32::F4::GPIO + HAL::STM32::F4::PWREx + HAL::STM32::F4::RCCEx + HAL::STM32::F4::TIMEx + HAL::STM32::F4::UART + STM32::NoSys + tool-libs-stm-uart + Eigen3::Eigen +) + +add_custom_target(upload + openocd -f interface/stlink.cfg -f target/stm32f4x.cfg -c "program stm.elf verify reset exit" + DEPENDS stm +) +add_custom_target(upload_ota + COMMAND arm-none-eabi-objcopy -O binary stm.elf stm.bin + COMMAND echo uploading with curl to $(UPLOAD) + COMMAND curl -v -F "update=@stm.bin" $(UPLOAD)/updateSTM + DEPENDS stm + BYPRODUCTS stm.bin + VERBATIM +) diff --git a/examples/generic/stm/f4.cpp b/examples/generic/stm/f4.cpp new file mode 100644 index 0000000..2c862ff --- /dev/null +++ b/examples/generic/stm/f4.cpp @@ -0,0 +1,225 @@ +/** @file f4.cpp + * + * Copyright (c) 2024 IACE + */ +#include +#include +#include +#include +#include + +#include "model.h" +#include "support.h" + +static void SystemClockReset(); + +static void SystemClockConfig(); + +struct board { + bool initclocks = [](){ + SystemClockReset(); + SystemClockConfig(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + __HAL_RCC_USART1_CLK_ENABLE(); + __HAL_RCC_TIM2_CLK_ENABLE(); + __HAL_RCC_TIM3_CLK_ENABLE(); + __HAL_RCC_TIM4_CLK_ENABLE(); + __HAL_RCC_TIM5_CLK_ENABLE(); + + HAL_NVIC_EnableIRQ(USART1_IRQn); + return true; + }(); + DIO led {GPIO_PIN_8, GPIOB}; + DIO button {GPIO_PIN_0, GPIOA, GPIO_MODE_INPUT, GPIO_PULLUP}; + UART::HW serialMin{{ + .uart = USART1, + .rx = AFIO {GPIO_PIN_10, GPIOA, GPIO_AF7_USART1}, + .tx = AFIO {GPIO_PIN_9, GPIOA, GPIO_AF7_USART1}, + .baudrate = 115200, + }}; + TIMER::Encoder enc{{ + .tim = TIM4, + .a = AFIO {GPIO_PIN_6, GPIOB, GPIO_AF2_TIM4}, + .b = AFIO {GPIO_PIN_7, GPIOB, GPIO_AF2_TIM4}, + .factor = 2 * M_PI / 8000, + .filter = 0xf, + }}; + Filter::MACD diff { 7, 30, 1 }; + struct { + DIO powergood {GPIO_PIN_9, GPIOB, GPIO_MODE_INPUT, GPIO_PULLUP}; + TIMER::HW tim {TIM5, 83, 1000}; // 84MHz/(83+1)/1000 == 1kHz + TIMER::PWM fan{{ + .tim = &tim, + .chan = TIM_CHANNEL_2, + .pin = AFIO {GPIO_PIN_1, GPIOA, GPIO_AF2_TIM5}, + }}; + } supply; + + struct { + TIMER::HW tim {TIM2, 41, 1000}; // 84MHz/(41+1)/1000 == 2kHz + MotorDriver driver {{ + .pwm = TIMER::PWM {{ + .tim = &tim, + .chan = TIM_CHANNEL_4, + .pin = AFIO {GPIO_PIN_3, GPIOA, GPIO_AF1_TIM2}, + }}, + .enA = DIO {GPIO_PIN_4, GPIOA}, + .enB = DIO {GPIO_PIN_5, GPIOA}, + }}; + TIMER::Encoder enc {{ + .tim = TIM3, + .a = AFIO {GPIO_PIN_6, GPIOA, GPIO_AF2_TIM3}, + .b = AFIO {GPIO_PIN_7, GPIOA, GPIO_AF2_TIM3}, + .factor = -2 * M_PI / 2096 / 4, + .filter = 0xf, + }}; + Filter::MACD diff { 7, 30, 1 }; + } motor; +} board{}; + +void handleHWFrame(Frame &f) { + auto enable = f.unpack(); + if (enable) { + board.motor.driver.enable(); + } else { + board.motor.driver.disable(); + } +} +void sendHWFrame(uint32_t t, uint32_t); + +Support::Support() + : min{.in{board.serialMin}, .out{board.serialMin}} {} + +void Support::init() { + board.supply.fan.pwm(0.8); + k.every(1, [](uint32_t, uint32_t) { + board.enc.measure(); + board.diff(board.enc.getPosition()); + board.motor.enc.measure(); + board.motor.diff(board.motor.enc.getPosition()); + }); + e.during(e.RUN).every(500, [](uint32_t, uint32_t) { + board.led.toggle(); + }); + e.during(e.IDLE).every(2000, [](uint32_t, uint32_t) { + board.led.toggle(); + }); + e.onEvent(e.STOP).call([](uint32_t) { + board.motor.driver.disable(); + }); + min.reg.setHandler(5, handleHWFrame); + e.during(e.RUN).every(20, sendHWFrame); +} + +void Furuta::reset() { + board.enc.zero(); + board.motor.enc.zero(); + board.diff.reset(); + board.motor.diff.reset(); +} + +void Furuta::setPwm(double val) { + board.motor.driver.pwm(val); +} + +void Furuta::updateState(uint32_t t, uint32_t dt) { + state.th1(0) = board.motor.enc.getPosition(); + state.th1(1) = board.motor.diff(); + state.th2(0) = board.enc.getPosition(); + state.th2(1) = board.diff(); +} + +Kernel k; +Support support; +Experiment e{&support.min.reg}; + +void sendHWFrame(uint32_t t, uint32_t) { + Frame f{7}; + f.pack(t).pack(!board.motor.driver.disabled); + support.min.out.push(f); +} + +int main() { + k.every(1, support.min, &Min::poll); + support.init(); + + Model m; + m.init(); + + k.run(); +} + +extern "C" void USART1_IRQHandler() { + board.serialMin.irqHandler(); +} + +static void SystemClockReset() { + RCC_ClkInitTypeDef RCC_ClkInitStruct = { + .ClockType = RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_HCLK + | RCC_CLOCKTYPE_PCLK1 + | RCC_CLOCKTYPE_PCLK2, + .SYSCLKSource = RCC_SYSCLKSOURCE_HSI, + .AHBCLKDivider = RCC_SYSCLK_DIV1, + .APB1CLKDivider = RCC_HCLK_DIV1, + .APB2CLKDivider = RCC_HCLK_DIV1, + }; + + while (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK); + + RCC_OscInitTypeDef RCC_OscInitStruct = { + .OscillatorType = RCC_OSCILLATORTYPE_HSE + | RCC_OSCILLATORTYPE_HSI + | RCC_OSCILLATORTYPE_LSE + | RCC_OSCILLATORTYPE_LSI, + .HSEState = RCC_HSE_OFF, + .LSEState = RCC_LSE_OFF, + .HSIState = RCC_HSI_ON, + .HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT, + .LSIState = RCC_LSI_OFF, + .PLL = { + .PLLState = RCC_PLL_OFF, + }, + }; + while (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK); +} + +/** + * @brief System clock configuration (84 MHz) + */ +static void SystemClockConfig() { + /** Configure the main internal regulator output voltage */ + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + /** Initializes the CPU, AHB and APB busses clocks */ + RCC_OscInitTypeDef RCC_OscInitStruct = { + .OscillatorType = RCC_OSCILLATORTYPE_HSE, + .HSEState = RCC_HSE_ON, + .PLL = { + .PLLState = RCC_PLL_ON, + .PLLSource = RCC_PLLSOURCE_HSE, + .PLLM = 25, + .PLLN = 336, + .PLLP = RCC_PLLP_DIV4, // => 84MHz + .PLLQ = 7, // => 48MHz + }, + }; + while (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK); + /** Initializes the CPU, AHB and APB busses clocks */ + RCC_ClkInitTypeDef RCC_ClkInitStruct = { + .ClockType = RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_HCLK + | RCC_CLOCKTYPE_PCLK1 + | RCC_CLOCKTYPE_PCLK2, + .SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK, + .AHBCLKDivider = RCC_SYSCLK_DIV1, // => 84MHz + .APB1CLKDivider = RCC_HCLK_DIV2, // => 42MHz, TIM: 84MHz + .APB2CLKDivider = RCC_HCLK_DIV2, // => 42MHz, TIM: 84MHz + }; + + while (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK); +} + diff --git a/examples/generic/stm/server.cpp b/examples/generic/stm/server.cpp deleted file mode 100644 index 36b6c99..0000000 --- a/examples/generic/stm/server.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** @file server.cpp - * - * Copyright (c) 2023 IACE - */ -#include -#include -#include -#include "model.h" - -#include "comm/min.h" -#include "comm/bufferutils.h" -#include "comm/line.h" - -TTY tty{"/dev/tty"}; -UDP udp{"127.0.0.1", 45670}; - -struct Support { - Support(); - void init(); - LineDelimiter lineout; - Min min; -}; - -Support::Support() - : lineout{tty} - , min{.in = udp, .out = udp} -{} -void Support::init() { - /* e.setHeartbeatTimeout(500, min.out); */ - k.every(500, - [](uint32_t time, uint32_t) { - uint32_t freq{1000}; - static bool state{false}; - freq = e.state==e.IDLE?2000:freq; - freq = e.state==e.RUN?500:freq; - if (time % freq == 0) { - state = !state; - k.log.info("led (would be) %s", state?"on":"off"); - } - }); - e.onEvent(e.STOP).call([](uint32_t){abort();}); -} - -Kernel k; -Support support; -Experiment e{&support.min.reg}; - -int main() { - support.init(); - k.initLog(support.lineout); - k.every(1, support.min, &Min::poll); - - Model model{support.min.out}; - model.init(support.min.reg); - k.run(); -} From 5f55b79db2ee06b132c0e27003875a7b102b9c9e Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Tue, 10 Dec 2024 01:50:51 +0100 Subject: [PATCH 04/16] Include traj to visu, rename connection, datapoint handling --- examples/generic/visu/connection.py | 9 ++--- examples/generic/visu/main.py | 18 +++++++--- examples/generic/visu/testbench.py | 28 ++++++++-------- examples/generic/visu/trajectory.py | 51 +++++++++++++++++++++++++++++ 4 files changed, 82 insertions(+), 24 deletions(-) create mode 100644 examples/generic/visu/trajectory.py diff --git a/examples/generic/visu/connection.py b/examples/generic/visu/connection.py index 4e50aa0..3a2c4cc 100644 --- a/examples/generic/visu/connection.py +++ b/examples/generic/visu/connection.py @@ -3,10 +3,11 @@ from pywisp.connection import * -class ConnTestTCP(UdpConnection): - settings = OrderedDict([("ip", '127.0.0.1'), - ("port", '45670'), - ]) +class Connection(UdpConnection): + settings = { + "ip", '127.0.0.1', + "port", '45670', + } def __init__(self): super().__init__(**self.settings) diff --git a/examples/generic/visu/main.py b/examples/generic/visu/main.py index c0392f5..7df8b5c 100644 --- a/examples/generic/visu/main.py +++ b/examples/generic/visu/main.py @@ -2,14 +2,22 @@ import sys import pywisp as pw import testbench +import trajectory +import visualization from PyQt5.QtWidgets import QApplication -from connection import ConnTestTCP -from visualization import MplTwoPendulumVisualizer +from connection import Connection if __name__ == '__main__': - pw.registerConnection(ConnTestTCP) - pw.registerExperimentModule(testbench.TwoPendulum) - pw.registerVisualizer(MplTwoPendulumVisualizer) + # connection + pw.registerConnection(Connection) + + # model + pw.registerExperimentModule(testbench.DoublePendulum) + # trajectory + pw.registerExperimentModule(trajectory.Trajectory) + + # visu + pw.registerVisualizer(visualization.MplDoublePendulumVisualizer) app = QApplication(sys.argv) form = pw.MainGui() form.show() diff --git a/examples/generic/visu/testbench.py b/examples/generic/visu/testbench.py index 7bebd01..933c7ef 100644 --- a/examples/generic/visu/testbench.py +++ b/examples/generic/visu/testbench.py @@ -1,21 +1,23 @@ # -*- coding: utf-8 -*- +import struct from collections import OrderedDict -import struct -from connection import ConnTestTCP from pywisp.experimentModules import ExperimentModule +from connection import Connection + -class TwoPendulum(ExperimentModule): - dataPoints = ['x', - 'phi1', - 'phi2', - 'u', - ] +class DoublePendulum(ExperimentModule): + dataPoints = [ + 'pos', + 'phi1', + 'phi2', + 'u', + ] publicSettings = OrderedDict() - connection = ConnTestTCP.__name__ + connection = Connection.__name__ def __init__(self): ExperimentModule.__init__(self) @@ -24,13 +26,9 @@ def handleFrame(self, frame): dataPoints = {} fid = frame.min_id if fid == 10: - data = struct.unpack(' Date: Tue, 10 Dec 2024 01:51:22 +0100 Subject: [PATCH 05/16] Add uv instead of pipenv --- examples/generic/pyproject.toml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 examples/generic/pyproject.toml diff --git a/examples/generic/pyproject.toml b/examples/generic/pyproject.toml new file mode 100644 index 0000000..10ce6cc --- /dev/null +++ b/examples/generic/pyproject.toml @@ -0,0 +1,12 @@ +[project] +name = "doublePendulum" +version = "0.1.0" +description = "Tool-Libs/PyWisp Example using double pendulum" +readme = "README.rst" +requires-python = ">=3.10" +dependencies = [ + "pywisp", +] + +[tool.uv.sources] +pywisp = { path = "../../" } From 7bdc4641855bbe30d62f97d37170b562251c62c3 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Tue, 10 Dec 2024 01:52:28 +0100 Subject: [PATCH 06/16] Remove 3.8/3.9 and set minimal numpy version to solve distutils issue, add uv workspace --- pyproject.toml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 63592f8..0a37771 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -19,15 +19,13 @@ classifiers = [ "Intended Audience :: Developers", "License :: GPL V3 License", "Programming Language :: Python :: 3", - "Programming Language :: Python :: 3.8", - "Programming Language :: Python :: 3.9", "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: 3 :: Only", ] dependencies = [ - 'numpy', + 'numpy>1.24.4', 'PyQt5', 'matplotlib', 'pyqtgraph', @@ -48,3 +46,6 @@ include-package-data = true packages = ["pywisp"] [tool.setuptools.package-dir] pywisp = "pywisp" + +[tool.uv.workspace] +members = ["examples/generic/doublePendulum"] From 59bca3658b4de01e3eac420e858fded1559fdcf2 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Tue, 10 Dec 2024 02:00:44 +0100 Subject: [PATCH 07/16] Typos --- examples/generic/CMakeLists.txt | 2 +- examples/generic/README.rst | 4 ++-- examples/generic/visu/default.sreg | 4 ++-- examples/generic/visu/visualization.py | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/generic/CMakeLists.txt b/examples/generic/CMakeLists.txt index 4097adc..0c2cbeb 100644 --- a/examples/generic/CMakeLists.txt +++ b/examples/generic/CMakeLists.txt @@ -20,7 +20,7 @@ else() endif() add_custom_target(visu - COMMAND pipenv run python visu/main.py + COMMAND uv run visu/main.py COMMENT "run Pendulum visualization" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) diff --git a/examples/generic/README.rst b/examples/generic/README.rst index ee9a87d..507e20b 100644 --- a/examples/generic/README.rst +++ b/examples/generic/README.rst @@ -9,8 +9,8 @@ Requirements ------------ * >=Python 3.8 -* >= uv -* Eigen library +* >=uv 0.5.6-1 +* >=Eigen 3.3 Simulation ---------- diff --git a/examples/generic/visu/default.sreg b/examples/generic/visu/default.sreg index 932b93a..dc790c8 100644 --- a/examples/generic/visu/default.sreg +++ b/examples/generic/visu/default.sreg @@ -2,10 +2,10 @@ - Name: RunSystem - TwoPendulum: + DoublePendulum: Visu: - MplTwoPendulumVisualizer: + MplDoublePendulumVisualizer: Config: MovingWindowSize: 10 diff --git a/examples/generic/visu/visualization.py b/examples/generic/visu/visualization.py index e534414..db5c7e4 100644 --- a/examples/generic/visu/visualization.py +++ b/examples/generic/visu/visualization.py @@ -9,7 +9,7 @@ from pywisp.visualization import MplVisualizer -class MplTwoPendulumVisualizer(MplVisualizer): +class MplDoublePendulumVisualizer(MplVisualizer): def __init__(self, q_widget, q_layout): MplVisualizer.__init__(self, q_widget, q_layout) self.axes.set_xlim(st.xMinPlot, st.xMaxPlot) From f8714d46c9cbcca0fca38a38af1850a947f89964 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Wed, 11 Dec 2024 00:54:44 +0100 Subject: [PATCH 08/16] Bring sim to work --- .gitignore | 4 +- examples/generic/CMakeLists.txt | 1 - examples/generic/inc/Pendulum.h | 32 ++++--- .../generic/inc/{trajType.h => TrajType.h} | 10 +-- examples/generic/inc/model.h | 85 ++++++++++++------- examples/generic/scripts/codegen.py | 10 +-- examples/generic/sim/NonLinDoublePendulum.h | 16 ++-- examples/generic/sim/sim.cpp | 15 ++-- examples/generic/visu/connection.py | 9 +- examples/generic/visu/default.sreg | 5 ++ examples/generic/visu/testbench.py | 20 ++++- examples/generic/visu/trajectory.py | 3 - examples/generic/visu/visualization.py | 2 +- 13 files changed, 133 insertions(+), 79 deletions(-) rename examples/generic/inc/{trajType.h => TrajType.h} (86%) diff --git a/.gitignore b/.gitignore index 0f6952f..d3975fc 100644 --- a/.gitignore +++ b/.gitignore @@ -105,4 +105,6 @@ Makefile server -sh.exe.stackdump \ No newline at end of file +sh.exe.stackdump + +uv.lock diff --git a/examples/generic/CMakeLists.txt b/examples/generic/CMakeLists.txt index 0c2cbeb..2a15d69 100644 --- a/examples/generic/CMakeLists.txt +++ b/examples/generic/CMakeLists.txt @@ -15,7 +15,6 @@ if (NOT SIM) add_subdirectory(stm) else() message("preparing build for host system") - add_subdirectory(bindings) add_subdirectory(sim) endif() diff --git a/examples/generic/inc/Pendulum.h b/examples/generic/inc/Pendulum.h index 91f992d..ad53f83 100644 --- a/examples/generic/inc/Pendulum.h +++ b/examples/generic/inc/Pendulum.h @@ -5,16 +5,28 @@ #ifndef PENDULUM_H #define PENDULUM_H +#include +#include + struct Pendulum { - using Input = DoublePendulum::Input; - using State = DoublePendulum::State; + using State = Eigen::Matrix; + State state{}; + + using Input = double; + Later input; + double in; + + void tick(uint32_t time, uint32_t dt) { + this->in = input.get(); + auto u = this->in; + setInput(u); - Later u; - DoublePendulum p{}; - State& state{p.state}; - operator const State&() { return state; } - void step(uint32_t, uint32_t dt) { - p.setInput(u.get()); - p.compute(dt); - } + updateState(time, dt); + }; + + void reset(); + void setInput(double); + void updateState(uint32_t time, uint32_t dt); }; + +#endif //PENDULUM_H diff --git a/examples/generic/inc/trajType.h b/examples/generic/inc/TrajType.h similarity index 86% rename from examples/generic/inc/trajType.h rename to examples/generic/inc/TrajType.h index cb8103c..7400d2b 100644 --- a/examples/generic/inc/trajType.h +++ b/examples/generic/inc/TrajType.h @@ -11,13 +11,13 @@ #include "Pendulum.h" struct TrajType { - using Des = Reference<5>; + using Des = Reference<2>; using Out = Pendulum::Input; Des des; Out out{}; - std::variant traj{5}; + std::variant traj{2}; enum Type:uint8_t {NONE, LIN, POLY} type{NONE}; void step(uint32_t time, uint32_t) { @@ -39,7 +39,7 @@ struct TrajType { void reset() { this->out = {}; - this->des = {0, 0, 0, 0, 0}; + this->des = {0, 0}; } void mkType(Type t) { @@ -49,11 +49,11 @@ struct TrajType { switch(t) { case LIN: { - traj = LinearTrajectory(5); + traj = LinearTrajectory(2); break; } case POLY: { - traj = SmoothTrajectory({126, -420, 540, -315, 70}, 9); + traj = SmoothTrajectory({3, -2}, 3); break; } case NONE: break; diff --git a/examples/generic/inc/model.h b/examples/generic/inc/model.h index 7c8d973..7e7a467 100644 --- a/examples/generic/inc/model.h +++ b/examples/generic/inc/model.h @@ -5,55 +5,82 @@ #ifndef MODEL_H #define MODEL_H -#include +#include #include -#include #include +#include + +#include "support.h" +#include "Pendulum.h" +#include "TrajType.h" struct Model { - Sink &out; - Pendulum p{ - .u = (Later) - traj, + enum Config {TRAJ}; + + Pendulum doublePendulum { + .input = (Later) trajType.out, }; - Trajectory traj; + + TrajType trajType; + + FrameRegistry &fr {support.min.reg}; + Min::Out &out {support.min.out}; void reset(uint32_t) { - p.state = Pendulum::State{0, 0, -0.1, 0.1, -0.1, 0.1}; - traj.ref = Trajectory::Reference{}; + doublePendulum.reset(); + trajType.reset(); } - void init(FrameRegistry &min) { - // resets + void init() { + // reset e.onEvent(e.INIT).call(*this, &Model::reset); + + e.during(e.RUN).every(1, doublePendulum, &Pendulum::tick); + // timesteps - e.during(e.RUN).every(200, traj, &Trajectory::step); - e.during(e.RUN).every(1, p, &Pendulum::step); + e.during(e.RUN).every(2, trajType, &TrajType::step); + + e.during(e.RUN).every(20, *this, &Model::sendModelData); + e.during(e.RUN).every(20, *this, &Model::sendTrajData); - e.during(e.RUN).every(25, *this, &Model::sendData); - // frames - min.setHandler(21, *this, &Model::getTrajData); + fr.setHandler(10, *this, &Model::setModelData); + fr.setHandler(20, *this, &Model::setTrajType); + fr.setHandler(21, trajType, &TrajType::setData); } - void sendData(uint32_t t, uint32_t dt) { - Frame f{10}; - f.pack(t); - f.pack(p.state[0]); - f.pack(p.state[2]); - f.pack(p.state[4]); - f.pack(traj.ref[0]); - out.push(std::move(f)); + void sendModelData(uint32_t time, uint32_t) { + Frame f{15}; + f.pack(time); + f.pack(doublePendulum.state(0)); + f.pack(doublePendulum.state(2)); + f.pack(doublePendulum.state(4)); + f.pack(doublePendulum.in); + out.push(f); + } + void sendTrajData(uint32_t time, uint32_t) { + Frame f{25}; + f.pack(time); + f.pack(trajType.des[0]); + f.pack(trajType.des[1]); + out.push(f); } - void getTrajData(Frame &f) { - static SeriesUnpacker su; - auto buf = su.unpack(f); - if (buf) { - traj.interp.setData(std::move(*buf)); + void setModelData(Frame &f) { + auto config = f.unpack(); + + switch (config) { + case Config::TRAJ: { + doublePendulum.input = (Later) trajType.out; + break; + } } } + + void setTrajType(Frame &f) { + trajType.mkType(f.unpack()); + } }; #endif //MODEL_H diff --git a/examples/generic/scripts/codegen.py b/examples/generic/scripts/codegen.py index 49927e1..3b78a25 100644 --- a/examples/generic/scripts/codegen.py +++ b/examples/generic/scripts/codegen.py @@ -2,7 +2,7 @@ from sympy import cos, sin, Matrix import numpy as np import settings as st -x, dx, phi1, dphi1, phi2, dphi2 = sp.symbols('x, dx, phi1, dphi1, phi2, dphi2') +p, d, phi1, dphi1, phi2, dphi2 = sp.symbols('p, dp, phi1, dphi1, phi2, dphi2') F, = sp.symbols('F,') m = st.cartMass @@ -25,11 +25,11 @@ [m2 * g * l2 * sin(phi2) + 2 * m2 * l1 * l2 * sin(phi1 - phi2) * dphi1 ** 2]]) solution = M.solve(B) -solution = solution.row_insert(0, Matrix([dx])) +solution = solution.row_insert(0, Matrix([dp])) solution = solution.row_insert(2, Matrix([dphi1])) solution = solution.row_insert(4, Matrix([dphi2])) -stateVars = np.array([x, dx, phi1, dphi1, phi2, dphi2]) +stateVars = np.array([p, dp, phi1, dphi1, phi2, dphi2]) inputVars = np.array([F]) from sympy.printing import ccode @@ -37,10 +37,10 @@ newline = '\n' print(f"""{f"{newline}".join([ -f" auto {x} = state({i});" for i, x in enumerate(stateVars) +f" auto {p} = state({i});" for i, p in enumerate(stateVars) ])} {f"{newline}".join([ -f" auto {x} = u({i});" for i, x in enumerate(inputVars) +f" auto {p} = u({i});" for i, p in enumerate(inputVars) ])} {ccode(solution, assign_to="tmp")} }}; diff --git a/examples/generic/sim/NonLinDoublePendulum.h b/examples/generic/sim/NonLinDoublePendulum.h index 79fc81c..7f265a4 100644 --- a/examples/generic/sim/NonLinDoublePendulum.h +++ b/examples/generic/sim/NonLinDoublePendulum.h @@ -9,14 +9,14 @@ namespace NonLinDoublePendulum { using State = Pendulum::State; using Input = Pendulum::Input; - void step(State x, Input u) { - auto p = state(0); - auto dp = state(1); - auto phi1 = state(2); - auto dphi1 = state(3); - auto phi2 = state(4); - auto dphi2 = state(5); - auto F = u(0); + State step(State x, Input u) { + auto p = x(0); + auto dp = x(1); + auto phi1 = x(2); + auto dphi1 = x(3); + auto phi2 = x(4); + auto dphi2 = x(5); + auto F = u; State dx { dp, diff --git a/examples/generic/sim/sim.cpp b/examples/generic/sim/sim.cpp index 674c51f..de8a4c1 100644 --- a/examples/generic/sim/sim.cpp +++ b/examples/generic/sim/sim.cpp @@ -6,10 +6,12 @@ #include #include #include "model.h" +#include "Pendulum.h" #include "NonLinDoublePendulum.h" TTY tty{"/dev/tty"}; UDP udp{"127.0.0.1", 45670}; +Pendulum::Input u; void Pendulum::updateState(uint32_t time, uint32_t dt_ms) { double dt = dt_ms / 1000.; @@ -26,8 +28,11 @@ void Pendulum::updateState(uint32_t time, uint32_t dt_ms) { } void Pendulum::reset() { - state.setZero(); - input.setZero(); + state = Pendulum::State{0, 0, -0.1, 0.1, -0.1, 0.1}; +} + +void Pendulum::setInput(double val) { + u = val; } Support::Support() : min{.in = udp, .out = udp }{} @@ -46,11 +51,7 @@ Kernel k; Support support; Experiment e{&support.min.reg}; -Kernel k; -Support support; -Experiment e{&support.min.reg}; - -int main() { +int main(int argc, char *argv[]) { k.initLog(tty); k.setTimeStep(getSimTimeStep(argc, argv)); k.every(1, support.min, &Min::poll); diff --git a/examples/generic/visu/connection.py b/examples/generic/visu/connection.py index 3a2c4cc..7ec59ab 100644 --- a/examples/generic/visu/connection.py +++ b/examples/generic/visu/connection.py @@ -1,12 +1,11 @@ # -*- coding: utf-8 -*- -from collections import OrderedDict -from pywisp.connection import * +from pywisp import connection -class Connection(UdpConnection): +class Connection(connection.UdpConnection): settings = { - "ip", '127.0.0.1', - "port", '45670', + "ip": '127.0.0.1', + "port": 45670, } def __init__(self): diff --git a/examples/generic/visu/default.sreg b/examples/generic/visu/default.sreg index dc790c8..5c4da6d 100644 --- a/examples/generic/visu/default.sreg +++ b/examples/generic/visu/default.sreg @@ -4,6 +4,11 @@ DoublePendulum: + Trajectory: + Type: 1 + times: [0, 1] + values: [0, 0.15] + Visu: MplDoublePendulumVisualizer: diff --git a/examples/generic/visu/testbench.py b/examples/generic/visu/testbench.py index 933c7ef..c0297a0 100644 --- a/examples/generic/visu/testbench.py +++ b/examples/generic/visu/testbench.py @@ -15,17 +15,29 @@ class DoublePendulum(ExperimentModule): 'u', ] - publicSettings = OrderedDict() + publicSettings = OrderedDict([ + ("Config", 0), + ]) connection = Connection.__name__ - def __init__(self): - ExperimentModule.__init__(self) + def getParams(self, data): + payloadConfig = struct.pack(' Date: Wed, 11 Dec 2024 01:36:49 +0100 Subject: [PATCH 09/16] Remove autogen file --- examples/generic/sim/NonLinDoublePendulum.h | 32 --------------------- 1 file changed, 32 deletions(-) delete mode 100644 examples/generic/sim/NonLinDoublePendulum.h diff --git a/examples/generic/sim/NonLinDoublePendulum.h b/examples/generic/sim/NonLinDoublePendulum.h deleted file mode 100644 index 7f265a4..0000000 --- a/examples/generic/sim/NonLinDoublePendulum.h +++ /dev/null @@ -1,32 +0,0 @@ -/** @file NonLinDoublePendulum.h - * - * Copyright (c) 2023 IACE - */ -#include -using Eigen::Matrix; - -namespace NonLinDoublePendulum { - using State = Pendulum::State; - using Input = Pendulum::Input; - - State step(State x, Input u) { - auto p = x(0); - auto dp = x(1); - auto phi1 = x(2); - auto dphi1 = x(3); - auto phi2 = x(4); - auto dphi2 = x(5); - auto F = u; - - State dx { - dp, - (9.1441406250000024e-5*F*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00033782519531250004*F*pow(cos(phi1), 2) + 2.7105054312137611e-20*F*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 2.7105054312137611e-20*F*pow(cos(phi2), 2) - 0.0065634609374999988*F*pow(cos(phi1 - phi2), 2) + 0.024248341796874993*F + 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 2.5336889648437505e-5*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2) + 1.6940658945086007e-21*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00049225957031250002*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.0018186256347656245*pow(dphi1, 2)*sin(phi1) - 6.8581054687500023e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 7.2391113281250003e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.00049225957031250002*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi1 - phi2) - 0.00051960732421874988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi2) + 4.0498762790596242e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) - 4.2748694056740464e-24*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 8.4456298828125018e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2) - 8.4703294725430034e-22*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1 - phi2), 2) + 0.00060620854492187477*pow(dphi2, 2)*sin(phi2) - 8.0011230468750031e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3) + 2.2860351562500008e-6*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*pow(cos(phi2), 2) + 0.00057430283203124995*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1) - 0.00016408652343749997*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi2)*cos(phi1 - phi2) + 0.00047094610253906272*sin(phi1)*pow(cos(phi1), 3) - 0.00013455602929687502*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.033803464693359375*sin(phi1)*cos(phi1) + 0.0096581327695312493*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00013455602929687504*sin(phi2)*pow(cos(phi1), 3)*cos(phi1 - phi2) + 0.00014203136425781251*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) + 0.0096581327695312493*sin(phi2)*cos(phi1)*cos(phi1 - phi2) - 0.010194695701171871*sin(phi2)*cos(phi2))/(4.8006738281250019e-5*pow(cos(phi1), 4) - 2.7432421875000009e-5*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4478222656250004e-5*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00093270234375000013*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.0068916339843749994*pow(cos(phi1), 2) + 0.0019690382812499996*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0010392146484374998*pow(cos(phi2), 2) - 0.066947301562499978*pow(cos(phi1 - phi2), 2) + 0.24733308632812487), - dphi1, - (6.2753906250000025e-5*F*pow(cos(phi1), 3) - 1.7929687500000001e-5*F*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 3.3881317890172014e-21*F*cos(phi1)*pow(cos(phi2), 2) - 0.0045043359374999994*F*cos(phi1) + 0.0012869531249999999*F*cos(phi2)*cos(phi1 - phi2) + 4.7065429687500025e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 3) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) + 2.1175823681357508e-22*pow(dphi1, 2)*sin(phi1)*cos(phi1)*pow(cos(phi2), 2) - 0.00033782519531250004*pow(dphi1, 2)*sin(phi1)*cos(phi1) + 9.6521484374999995e-5*pow(dphi1, 2)*sin(phi1)*cos(phi2)*cos(phi1 - phi2) - 1.3447265625000006e-6*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 3)*cos(phi2) + 9.1441406250000024e-5*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 9.6521484375000009e-5*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.0065634609374999988*pow(dphi1, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) + 7.9409338805090672e-25*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3)*pow(cos(phi2), 2) + 1.5688476562500007e-6*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 3) - 4.482421875000002e-7*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2)*cos(phi1 - phi2) - 0.0001126083984375*pow(dphi2, 2)*sin(phi2)*cos(phi1) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi2)*cos(phi2)*cos(phi1 - phi2) - 4.4824218750000015e-7*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 0.00010668164062500003*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) + 3.2173828125000003e-5*pow(dphi2, 2)*sin(phi1 - phi2)*pow(cos(phi2), 2) - 0.0076573710937499987*pow(dphi2, 2)*sin(phi1 - phi2) + 2.6383535156250014e-5*sin(phi1)*pow(cos(phi1), 2)*pow(cos(phi2), 2) - 0.006279281367187502*sin(phi1)*pow(cos(phi1), 2) - 4.3368086899420177e-19*sin(phi1)*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.0018937515234375003*sin(phi1)*pow(cos(phi2), 2) + 1.3877787807814457e-17*sin(phi1)*pow(cos(phi1 - phi2), 2) + 0.45071286257812493*sin(phi1) - 2.6383535156250011e-5*sin(phi2)*pow(cos(phi1), 3)*cos(phi2) + 0.0017940803906250004*sin(phi2)*pow(cos(phi1), 2)*cos(phi1 - phi2) + 0.0018937515234374999*sin(phi2)*cos(phi1)*cos(phi2) - 0.12877510359374997*sin(phi2)*cos(phi1 - phi2))/(4.7065429687500025e-6*pow(cos(phi1), 4) - 2.6894531250000012e-6*pow(cos(phi1), 3)*cos(phi2)*cos(phi1 - phi2) + 1.4194335937500006e-6*pow(cos(phi1), 2)*pow(cos(phi2), 2) + 9.1441406250000024e-5*pow(cos(phi1), 2)*pow(cos(phi1 - phi2), 2) - 0.00067565039062500009*pow(cos(phi1), 2) + 0.00019304296875000002*cos(phi1)*cos(phi2)*cos(phi1 - phi2) - 0.00010188378906249999*pow(cos(phi2), 2) - 0.0065634609374999988*pow(cos(phi1 - phi2), 2) + 0.024248341796874993), - dphi2, - (-0.0095625000000000016*F*cos(phi1)*cos(phi1 - phi2) + 0.010093749999999999*F*cos(phi2) - 0.00071718750000000016*pow(dphi1, 2)*sin(phi1)*cos(phi1)*cos(phi1 - phi2) + 0.00075703125*pow(dphi1, 2)*sin(phi1)*cos(phi2) + 0.00071718750000000016*pow(dphi1, 2)*sin(phi1 - phi2)*pow(cos(phi1), 2) - 0.051478124999999993*pow(dphi1, 2)*sin(phi1 - phi2) - 4.2351647362715017e-22*pow(dphi2, 2)*sin(phi2)*pow(cos(phi1), 2)*cos(phi2) - 0.00023906250000000004*pow(dphi2, 2)*sin(phi2)*cos(phi1)*cos(phi1 - phi2) + 0.00025234375000000002*pow(dphi2, 2)*sin(phi2)*cos(phi2) + 0.00023906250000000004*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1)*cos(phi2) - 0.01625625*pow(dphi2, 2)*sin(phi1 - phi2)*cos(phi1 - phi2) - 0.014071218750000005*sin(phi1)*cos(phi1)*cos(phi2) + 0.95684287500000009*sin(phi1)*cos(phi1 - phi2) + 0.014071218750000003*sin(phi2)*pow(cos(phi1), 2) - 1.0100008124999997*sin(phi2))/(0.00083671875000000022*pow(cos(phi1), 2) - 0.00047812500000000009*cos(phi1)*cos(phi2)*cos(phi1 - phi2) + 0.00025234375000000002*pow(cos(phi2), 2) + 0.01625625*pow(cos(phi1 - phi2), 2) - 0.060057812499999988), - }; - - return dx; - }; -}; From 1fa4e4a1b3423be2dfc72920e92af40907ba04a2 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Wed, 11 Dec 2024 01:40:44 +0100 Subject: [PATCH 10/16] Fix autocode --- examples/generic/CMakeLists.txt | 10 +++++++++- examples/generic/README.rst | 3 +++ examples/generic/pyproject.toml | 1 + examples/generic/scripts/codegen.py | 25 +++++++++++++++++++------ examples/generic/sim/CMakeLists.txt | 11 ++++++++++- 5 files changed, 42 insertions(+), 8 deletions(-) diff --git a/examples/generic/CMakeLists.txt b/examples/generic/CMakeLists.txt index 2a15d69..ff0fb28 100644 --- a/examples/generic/CMakeLists.txt +++ b/examples/generic/CMakeLists.txt @@ -6,10 +6,18 @@ option(SIM "build for host system instead of cross-compiling" TRUE) include(local.cmake OPTIONAL) include(dependencies.cmake) - ## Project declaration project(Pendulum C CXX ASM) +add_custom_command( + OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/sim/NonLinDoublePendulum.h + COMMAND uv run scripts/codegen.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/settings.py + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/codegen.py + VERBATIM + ) + if (NOT SIM) message("preparing cross-compilation") add_subdirectory(stm) diff --git a/examples/generic/README.rst b/examples/generic/README.rst index 507e20b..b0656a8 100644 --- a/examples/generic/README.rst +++ b/examples/generic/README.rst @@ -11,6 +11,9 @@ Requirements * >=Python 3.8 * >=uv 0.5.6-1 * >=Eigen 3.3 +* >=cmake 3.24 +* >=arm-none-eabi-* 14.2.0 +* ncat (from ``nmap`` project) Simulation ---------- diff --git a/examples/generic/pyproject.toml b/examples/generic/pyproject.toml index 10ce6cc..e968404 100644 --- a/examples/generic/pyproject.toml +++ b/examples/generic/pyproject.toml @@ -6,6 +6,7 @@ readme = "README.rst" requires-python = ">=3.10" dependencies = [ "pywisp", + "sympy>=1.13.3", ] [tool.uv.sources] diff --git a/examples/generic/scripts/codegen.py b/examples/generic/scripts/codegen.py index 3b78a25..4a5c508 100644 --- a/examples/generic/scripts/codegen.py +++ b/examples/generic/scripts/codegen.py @@ -1,8 +1,10 @@ import sympy as sp from sympy import cos, sin, Matrix +from sympy.printing import ccode import numpy as np import settings as st -p, d, phi1, dphi1, phi2, dphi2 = sp.symbols('p, dp, phi1, dphi1, phi2, dphi2') + +p, dp, phi1, dphi1, phi2, dphi2 = sp.symbols('p, dp, phi1, dphi1, phi2, dphi2') F, = sp.symbols('F,') m = st.cartMass @@ -32,17 +34,28 @@ stateVars = np.array([p, dp, phi1, dphi1, phi2, dphi2]) inputVars = np.array([F]) -from sympy.printing import ccode newline = '\n' -print(f"""{f"{newline}".join([ -f" auto {p} = state({i});" for i, p in enumerate(stateVars) +with open('NonLinDoublePendulum.h', 'w') as file: + file.write(f"""/** This file is autogenerated, do not edit */ +#pragma once +#include +#include +using Eigen::Matrix; +namespace NonLinDoublePendulum {{ + using State = Pendulum::State; + using Input = Pendulum::Input; + State step(State x, Input u) {{ +{f"{newline}".join([ +f" auto {p} = x({i});" for i, p in enumerate(stateVars) ])} {f"{newline}".join([ -f" auto {p} = u({i});" for i, p in enumerate(inputVars) +f" auto {p} = u;" for i, p in enumerate(inputVars) ])} - {ccode(solution, assign_to="tmp")} + State dx; + {ccode(solution, assign_to="dx")} + return dx; }}; }}; """) diff --git a/examples/generic/sim/CMakeLists.txt b/examples/generic/sim/CMakeLists.txt index 7cdebfa..f0730b4 100644 --- a/examples/generic/sim/CMakeLists.txt +++ b/examples/generic/sim/CMakeLists.txt @@ -1,4 +1,13 @@ -add_executable(sim sim.cpp) +add_executable(sim sim.cpp NonLinDoublePendulum.h) + +add_custom_command( + OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/NonLinDoublePendulum.h + COMMAND uv run ../scripts/codegen.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/../scripts/settings.py + ${CMAKE_CURRENT_SOURCE_DIR}/../scripts/codegen.py + VERBATIM + ) target_include_directories(sim PUBLIC . ../inc From 67d9a19472f81688840bcf253f1f398b0c896ca6 Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Wed, 11 Dec 2024 01:48:52 +0100 Subject: [PATCH 11/16] Fix stm code example --- examples/generic/CMakeLists.txt | 9 - examples/generic/stm/f4.cpp | 77 +--- examples/generic/stm/stm32f4xx_hal_conf.h | 492 ++++++++++++++++++++++ 3 files changed, 506 insertions(+), 72 deletions(-) create mode 100644 examples/generic/stm/stm32f4xx_hal_conf.h diff --git a/examples/generic/CMakeLists.txt b/examples/generic/CMakeLists.txt index ff0fb28..6961aa5 100644 --- a/examples/generic/CMakeLists.txt +++ b/examples/generic/CMakeLists.txt @@ -9,15 +9,6 @@ include(dependencies.cmake) ## Project declaration project(Pendulum C CXX ASM) -add_custom_command( - OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/sim/NonLinDoublePendulum.h - COMMAND uv run scripts/codegen.py - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} - DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/settings.py - ${CMAKE_CURRENT_SOURCE_DIR}/scripts/codegen.py - VERBATIM - ) - if (NOT SIM) message("preparing cross-compilation") add_subdirectory(stm) diff --git a/examples/generic/stm/f4.cpp b/examples/generic/stm/f4.cpp index 2c862ff..e1360af 100644 --- a/examples/generic/stm/f4.cpp +++ b/examples/generic/stm/f4.cpp @@ -40,14 +40,6 @@ struct board { .tx = AFIO {GPIO_PIN_9, GPIOA, GPIO_AF7_USART1}, .baudrate = 115200, }}; - TIMER::Encoder enc{{ - .tim = TIM4, - .a = AFIO {GPIO_PIN_6, GPIOB, GPIO_AF2_TIM4}, - .b = AFIO {GPIO_PIN_7, GPIOB, GPIO_AF2_TIM4}, - .factor = 2 * M_PI / 8000, - .filter = 0xf, - }}; - Filter::MACD diff { 7, 30, 1 }; struct { DIO powergood {GPIO_PIN_9, GPIOB, GPIO_MODE_INPUT, GPIO_PULLUP}; TIMER::HW tim {TIM5, 83, 1000}; // 84MHz/(83+1)/1000 == 1kHz @@ -58,37 +50,16 @@ struct board { }}; } supply; - struct { - TIMER::HW tim {TIM2, 41, 1000}; // 84MHz/(41+1)/1000 == 2kHz - MotorDriver driver {{ - .pwm = TIMER::PWM {{ - .tim = &tim, - .chan = TIM_CHANNEL_4, - .pin = AFIO {GPIO_PIN_3, GPIOA, GPIO_AF1_TIM2}, - }}, - .enA = DIO {GPIO_PIN_4, GPIOA}, - .enB = DIO {GPIO_PIN_5, GPIOA}, - }}; - TIMER::Encoder enc {{ - .tim = TIM3, - .a = AFIO {GPIO_PIN_6, GPIOA, GPIO_AF2_TIM3}, - .b = AFIO {GPIO_PIN_7, GPIOA, GPIO_AF2_TIM3}, - .factor = -2 * M_PI / 2096 / 4, - .filter = 0xf, - }}; - Filter::MACD diff { 7, 30, 1 }; - } motor; -} board{}; + TIMER::HW pwmTim{TIM3, (2 - 1), 1500}; // APB1=84MHz/2 -> 42MHz; -> 42MHz/1500 = ~28kHz -void handleHWFrame(Frame &f) { - auto enable = f.unpack(); - if (enable) { - board.motor.driver.enable(); - } else { - board.motor.driver.disable(); - } -} -void sendHWFrame(uint32_t t, uint32_t); + TIMER::PWM input = TIMER::PWM{{ + .tim = &pwmTim, + .chan = TIM_CHANNEL_1, + .pin = AFIO{GPIO_PIN_6, GPIOC, GPIO_AF2_TIM3, + GPIO_PULLDOWN + }, + }}; +} board{}; Support::Support() : min{.in{board.serialMin}, .out{board.serialMin}} {} @@ -96,10 +67,6 @@ Support::Support() void Support::init() { board.supply.fan.pwm(0.8); k.every(1, [](uint32_t, uint32_t) { - board.enc.measure(); - board.diff(board.enc.getPosition()); - board.motor.enc.measure(); - board.motor.diff(board.motor.enc.getPosition()); }); e.during(e.RUN).every(500, [](uint32_t, uint32_t) { board.led.toggle(); @@ -108,40 +75,24 @@ void Support::init() { board.led.toggle(); }); e.onEvent(e.STOP).call([](uint32_t) { - board.motor.driver.disable(); + board.input.pwm(0); }); - min.reg.setHandler(5, handleHWFrame); - e.during(e.RUN).every(20, sendHWFrame); } -void Furuta::reset() { - board.enc.zero(); - board.motor.enc.zero(); - board.diff.reset(); - board.motor.diff.reset(); +void Pendulum::reset() { } -void Furuta::setPwm(double val) { - board.motor.driver.pwm(val); +void Pendulum::setInput(double val) { + board.input.pwm(fmin(1, fmax(0, val))); } -void Furuta::updateState(uint32_t t, uint32_t dt) { - state.th1(0) = board.motor.enc.getPosition(); - state.th1(1) = board.motor.diff(); - state.th2(0) = board.enc.getPosition(); - state.th2(1) = board.diff(); +void Pendulum::updateState(uint32_t t, uint32_t dt) { } Kernel k; Support support; Experiment e{&support.min.reg}; -void sendHWFrame(uint32_t t, uint32_t) { - Frame f{7}; - f.pack(t).pack(!board.motor.driver.disabled); - support.min.out.push(f); -} - int main() { k.every(1, support.min, &Min::poll); support.init(); diff --git a/examples/generic/stm/stm32f4xx_hal_conf.h b/examples/generic/stm/stm32f4xx_hal_conf.h new file mode 100644 index 0000000..79a9550 --- /dev/null +++ b/examples/generic/stm/stm32f4xx_hal_conf.h @@ -0,0 +1,492 @@ +/** + ****************************************************************************** + * @file stm32f4xx_hal_conf_template.h + * @author MCD Application Team + * @brief HAL configuration template file. + * This file should be copied to the application folder and renamed + * to stm32f4xx_hal_conf.h. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2017 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_HAL_CONF_H +#define __STM32F4xx_HAL_CONF_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* ########################## Module Selection ############################## */ +/** + * @brief This is the list of modules to be used in the HAL driver + */ +#define HAL_MODULE_ENABLED + +/* #define HAL_ADC_MODULE_ENABLED */ +/* #define HAL_CRYP_MODULE_ENABLED */ +/* #define HAL_CAN_MODULE_ENABLED */ +/* #define HAL_CRC_MODULE_ENABLED */ +/* #define HAL_CAN_LEGACY_MODULE_ENABLED */ +/* #define HAL_CRYP_MODULE_ENABLED */ +/* #define HAL_DAC_MODULE_ENABLED */ +/* #define HAL_DCMI_MODULE_ENABLED */ +/* #define HAL_DMA2D_MODULE_ENABLED */ +/* #define HAL_ETH_MODULE_ENABLED */ +/* #define HAL_NAND_MODULE_ENABLED */ +/* #define HAL_NOR_MODULE_ENABLED */ +/* #define HAL_PCCARD_MODULE_ENABLED */ +/* #define HAL_SRAM_MODULE_ENABLED */ +/* #define HAL_SDRAM_MODULE_ENABLED */ +/* #define HAL_HASH_MODULE_ENABLED */ +/* #define HAL_I2C_MODULE_ENABLED */ +/* #define HAL_I2S_MODULE_ENABLED */ +/* #define HAL_IWDG_MODULE_ENABLED */ +/* #define HAL_LTDC_MODULE_ENABLED */ +/* #define HAL_RNG_MODULE_ENABLED */ +/* #define HAL_RTC_MODULE_ENABLED */ +/* #define HAL_SAI_MODULE_ENABLED */ +/* #define HAL_SD_MODULE_ENABLED */ +/* #define HAL_MMC_MODULE_ENABLED */ +/* #define HAL_SPI_MODULE_ENABLED */ +#define HAL_TIM_MODULE_ENABLED +#define HAL_UART_MODULE_ENABLED +/* #define HAL_USART_MODULE_ENABLED */ +/* #define HAL_IRDA_MODULE_ENABLED */ +/* #define HAL_SMARTCARD_MODULE_ENABLED */ +/* #define HAL_SMBUS_MODULE_ENABLED */ +/* #define HAL_WWDG_MODULE_ENABLED */ +/* #define HAL_PCD_MODULE_ENABLED */ +/* #define HAL_HCD_MODULE_ENABLED */ +/* #define HAL_DSI_MODULE_ENABLED */ +/* #define HAL_QSPI_MODULE_ENABLED */ +/* #define HAL_QSPI_MODULE_ENABLED */ +/* #define HAL_CEC_MODULE_ENABLED */ +/* #define HAL_FMPI2C_MODULE_ENABLED */ +/* #define HAL_FMPSMBUS_MODULE_ENABLED */ +/* #define HAL_SPDIFRX_MODULE_ENABLED */ +/* #define HAL_DFSDM_MODULE_ENABLED */ +/* #define HAL_LPTIM_MODULE_ENABLED */ +#define HAL_GPIO_MODULE_ENABLED +#define HAL_EXTI_MODULE_ENABLED +#define HAL_DMA_MODULE_ENABLED +#define HAL_RCC_MODULE_ENABLED +#define HAL_FLASH_MODULE_ENABLED +#define HAL_PWR_MODULE_ENABLED +#define HAL_CORTEX_MODULE_ENABLED + +/* ########################## HSE/HSI Values adaptation ##################### */ +/** + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined (HSE_VALUE) +#define HSE_VALUE 25000000U /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSE_STARTUP_TIMEOUT) +#define HSE_STARTUP_TIMEOUT 100U /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined (HSI_VALUE) +#define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @brief Internal Low Speed oscillator (LSI) value. + */ +#if !defined (LSI_VALUE) +#define LSI_VALUE 32000U /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz + The real value may vary depending on the variations + in voltage and temperature.*/ +/** + * @brief External Low Speed oscillator (LSE) value. + */ +#if !defined (LSE_VALUE) +#define LSE_VALUE 32768U /*!< Value of the External Low Speed oscillator in Hz */ +#endif /* LSE_VALUE */ + +#if !defined (LSE_STARTUP_TIMEOUT) +#define LSE_STARTUP_TIMEOUT 5000U /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +/** + * @brief External clock source for I2S peripheral + * This value is used by the I2S HAL module to compute the I2S clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined (EXTERNAL_CLOCK_VALUE) +#define EXTERNAL_CLOCK_VALUE 12288000U /*!< Value of the External audio frequency in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ + +/* Tip: To avoid modifying this file each time you need to use different HSE, + === you can define the HSE value in your toolchain compiler preprocessor. */ + +/* ########################### System Configuration ######################### */ +/** + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE 3300U /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY 0U /*!< tick interrupt priority */ +#define USE_RTOS 0U +#define PREFETCH_ENABLE 1U +#define INSTRUCTION_CACHE_ENABLE 1U +#define DATA_CACHE_ENABLE 1U + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ +#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */ +#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ +#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ +#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ +#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ +#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ +#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ +#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ +#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ +#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ +#define USE_HAL_FMPI2C_REGISTER_CALLBACKS 0U /* FMPI2C register callback disabled */ +#define USE_HAL_FMPSMBUS_REGISTER_CALLBACKS 0U /* FMPSMBUS register callback disabled */ +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ +#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ +#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ +#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ +#define USE_HAL_PCCARD_REGISTER_CALLBACKS 0U /* PCCARD register callback disabled */ +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ +#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ +#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ +#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ +#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ +#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ +#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ +#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ +#define USE_HAL_TIM_REGISTER_CALLBACKS 1U /* TIM register callback disabled */ +#define USE_HAL_UART_REGISTER_CALLBACKS 1U /* UART register callback disabled */ +#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ + +/* ########################## Assert Selection ############################## */ +/** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ +/* #define USE_FULL_ASSERT 1U */ + +/* ################## Ethernet peripheral configuration ##################### */ + +/* Section 1 : Ethernet peripheral configuration */ + +/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */ +#define MAC_ADDR0 2U +#define MAC_ADDR1 0U +#define MAC_ADDR2 0U +#define MAC_ADDR3 0U +#define MAC_ADDR4 0U +#define MAC_ADDR5 0U + +/* Definition of the Ethernet driver buffers size and count */ +#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ +#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ +#define ETH_RXBUFNB 4U /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ +#define ETH_TXBUFNB 4U /* 4 Tx buffers of size ETH_TX_BUF_SIZE */ + +/* Section 2: PHY configuration section */ + +/* DP83848_PHY_ADDRESS Address*/ +#define DP83848_PHY_ADDRESS 0x01U +/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ +#define PHY_RESET_DELAY 0x000000FFU +/* PHY Configuration delay */ +#define PHY_CONFIG_DELAY 0x00000FFFU + +#define PHY_READ_TO 0x0000FFFFU +#define PHY_WRITE_TO 0x0000FFFFU + +/* Section 3: Common PHY Registers */ + +#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */ +#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */ + +#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */ +#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */ +#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */ +#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000U) /*!< Set the half-duplex mode at 100 Mb/s */ +#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100U) /*!< Set the full-duplex mode at 10 Mb/s */ +#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000U) /*!< Set the half-duplex mode at 10 Mb/s */ +#define PHY_AUTONEGOTIATION ((uint16_t)0x1000U) /*!< Enable auto-negotiation function */ +#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200U) /*!< Restart auto-negotiation function */ +#define PHY_POWERDOWN ((uint16_t)0x0800U) /*!< Select the power down mode */ +#define PHY_ISOLATE ((uint16_t)0x0400U) /*!< Isolate PHY from MII */ + +#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */ +#define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */ +#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */ + +/* Section 4: Extended PHY Registers */ +#define PHY_SR ((uint16_t)0x10U) /*!< PHY status register Offset */ + +#define PHY_SPEED_STATUS ((uint16_t)0x0002U) /*!< PHY Speed mask */ +#define PHY_DUPLEX_STATUS ((uint16_t)0x0004U) /*!< PHY Duplex mask */ + +/* ################## SPI peripheral configuration ########################## */ + +/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver +* Activated: CRC code is present inside driver +* Deactivated: CRC code cleaned from driver +*/ + +#define USE_SPI_CRC 0U + +/* Includes ------------------------------------------------------------------*/ +/** + * @brief Include module's header file + */ + +#ifdef HAL_RCC_MODULE_ENABLED +#include "stm32f4xx_hal_rcc.h" +#endif /* HAL_RCC_MODULE_ENABLED */ + +#ifdef HAL_GPIO_MODULE_ENABLED +#include "stm32f4xx_hal_gpio.h" +#endif /* HAL_GPIO_MODULE_ENABLED */ + +#ifdef HAL_EXTI_MODULE_ENABLED +#include "stm32f4xx_hal_exti.h" +#endif /* HAL_EXTI_MODULE_ENABLED */ + +#ifdef HAL_DMA_MODULE_ENABLED +#include "stm32f4xx_hal_dma.h" +#endif /* HAL_DMA_MODULE_ENABLED */ + +#ifdef HAL_CORTEX_MODULE_ENABLED +#include "stm32f4xx_hal_cortex.h" +#endif /* HAL_CORTEX_MODULE_ENABLED */ + +#ifdef HAL_ADC_MODULE_ENABLED +#include "stm32f4xx_hal_adc.h" +#endif /* HAL_ADC_MODULE_ENABLED */ + +#ifdef HAL_CAN_MODULE_ENABLED +#include "stm32f4xx_hal_can.h" +#endif /* HAL_CAN_MODULE_ENABLED */ + +#ifdef HAL_CAN_LEGACY_MODULE_ENABLED +#include "stm32f4xx_hal_can_legacy.h" +#endif /* HAL_CAN_LEGACY_MODULE_ENABLED */ + +#ifdef HAL_CRC_MODULE_ENABLED +#include "stm32f4xx_hal_crc.h" +#endif /* HAL_CRC_MODULE_ENABLED */ + +#ifdef HAL_CRYP_MODULE_ENABLED +#include "stm32f4xx_hal_cryp.h" +#endif /* HAL_CRYP_MODULE_ENABLED */ + +#ifdef HAL_DMA2D_MODULE_ENABLED +#include "stm32f4xx_hal_dma2d.h" +#endif /* HAL_DMA2D_MODULE_ENABLED */ + +#ifdef HAL_DAC_MODULE_ENABLED +#include "stm32f4xx_hal_dac.h" +#endif /* HAL_DAC_MODULE_ENABLED */ + +#ifdef HAL_DCMI_MODULE_ENABLED +#include "stm32f4xx_hal_dcmi.h" +#endif /* HAL_DCMI_MODULE_ENABLED */ + +#ifdef HAL_ETH_MODULE_ENABLED +#include "stm32f4xx_hal_eth.h" +#endif /* HAL_ETH_MODULE_ENABLED */ + +#ifdef HAL_FLASH_MODULE_ENABLED +#include "stm32f4xx_hal_flash.h" +#endif /* HAL_FLASH_MODULE_ENABLED */ + +#ifdef HAL_SRAM_MODULE_ENABLED +#include "stm32f4xx_hal_sram.h" +#endif /* HAL_SRAM_MODULE_ENABLED */ + +#ifdef HAL_NOR_MODULE_ENABLED +#include "stm32f4xx_hal_nor.h" +#endif /* HAL_NOR_MODULE_ENABLED */ + +#ifdef HAL_NAND_MODULE_ENABLED +#include "stm32f4xx_hal_nand.h" +#endif /* HAL_NAND_MODULE_ENABLED */ + +#ifdef HAL_PCCARD_MODULE_ENABLED +#include "stm32f4xx_hal_pccard.h" +#endif /* HAL_PCCARD_MODULE_ENABLED */ + +#ifdef HAL_SDRAM_MODULE_ENABLED +#include "stm32f4xx_hal_sdram.h" +#endif /* HAL_SDRAM_MODULE_ENABLED */ + +#ifdef HAL_HASH_MODULE_ENABLED +#include "stm32f4xx_hal_hash.h" +#endif /* HAL_HASH_MODULE_ENABLED */ + +#ifdef HAL_I2C_MODULE_ENABLED +#include "stm32f4xx_hal_i2c.h" +#endif /* HAL_I2C_MODULE_ENABLED */ + +#ifdef HAL_SMBUS_MODULE_ENABLED +#include "stm32f4xx_hal_smbus.h" +#endif /* HAL_SMBUS_MODULE_ENABLED */ + +#ifdef HAL_I2S_MODULE_ENABLED +#include "stm32f4xx_hal_i2s.h" +#endif /* HAL_I2S_MODULE_ENABLED */ + +#ifdef HAL_IWDG_MODULE_ENABLED +#include "stm32f4xx_hal_iwdg.h" +#endif /* HAL_IWDG_MODULE_ENABLED */ + +#ifdef HAL_LTDC_MODULE_ENABLED +#include "stm32f4xx_hal_ltdc.h" +#endif /* HAL_LTDC_MODULE_ENABLED */ + +#ifdef HAL_PWR_MODULE_ENABLED +#include "stm32f4xx_hal_pwr.h" +#endif /* HAL_PWR_MODULE_ENABLED */ + +#ifdef HAL_RNG_MODULE_ENABLED +#include "stm32f4xx_hal_rng.h" +#endif /* HAL_RNG_MODULE_ENABLED */ + +#ifdef HAL_RTC_MODULE_ENABLED +#include "stm32f4xx_hal_rtc.h" +#endif /* HAL_RTC_MODULE_ENABLED */ + +#ifdef HAL_SAI_MODULE_ENABLED +#include "stm32f4xx_hal_sai.h" +#endif /* HAL_SAI_MODULE_ENABLED */ + +#ifdef HAL_SD_MODULE_ENABLED +#include "stm32f4xx_hal_sd.h" +#endif /* HAL_SD_MODULE_ENABLED */ + +#ifdef HAL_SPI_MODULE_ENABLED +#include "stm32f4xx_hal_spi.h" +#endif /* HAL_SPI_MODULE_ENABLED */ + +#ifdef HAL_TIM_MODULE_ENABLED +#include "stm32f4xx_hal_tim.h" +#endif /* HAL_TIM_MODULE_ENABLED */ + +#ifdef HAL_UART_MODULE_ENABLED +#include "stm32f4xx_hal_uart.h" +#endif /* HAL_UART_MODULE_ENABLED */ + +#ifdef HAL_USART_MODULE_ENABLED +#include "stm32f4xx_hal_usart.h" +#endif /* HAL_USART_MODULE_ENABLED */ + +#ifdef HAL_IRDA_MODULE_ENABLED +#include "stm32f4xx_hal_irda.h" +#endif /* HAL_IRDA_MODULE_ENABLED */ + +#ifdef HAL_SMARTCARD_MODULE_ENABLED +#include "stm32f4xx_hal_smartcard.h" +#endif /* HAL_SMARTCARD_MODULE_ENABLED */ + +#ifdef HAL_WWDG_MODULE_ENABLED +#include "stm32f4xx_hal_wwdg.h" +#endif /* HAL_WWDG_MODULE_ENABLED */ + +#ifdef HAL_PCD_MODULE_ENABLED +#include "stm32f4xx_hal_pcd.h" +#endif /* HAL_PCD_MODULE_ENABLED */ + +#ifdef HAL_HCD_MODULE_ENABLED +#include "stm32f4xx_hal_hcd.h" +#endif /* HAL_HCD_MODULE_ENABLED */ + +#ifdef HAL_DSI_MODULE_ENABLED +#include "stm32f4xx_hal_dsi.h" +#endif /* HAL_DSI_MODULE_ENABLED */ + +#ifdef HAL_QSPI_MODULE_ENABLED +#include "stm32f4xx_hal_qspi.h" +#endif /* HAL_QSPI_MODULE_ENABLED */ + +#ifdef HAL_CEC_MODULE_ENABLED +#include "stm32f4xx_hal_cec.h" +#endif /* HAL_CEC_MODULE_ENABLED */ + +#ifdef HAL_FMPI2C_MODULE_ENABLED +#include "stm32f4xx_hal_fmpi2c.h" +#endif /* HAL_FMPI2C_MODULE_ENABLED */ + +#ifdef HAL_FMPSMBUS_MODULE_ENABLED +#include "stm32f4xx_hal_fmpsmbus.h" +#endif /* HAL_FMPSMBUS_MODULE_ENABLED */ + +#ifdef HAL_SPDIFRX_MODULE_ENABLED +#include "stm32f4xx_hal_spdifrx.h" +#endif /* HAL_SPDIFRX_MODULE_ENABLED */ + +#ifdef HAL_DFSDM_MODULE_ENABLED +#include "stm32f4xx_hal_dfsdm.h" +#endif /* HAL_DFSDM_MODULE_ENABLED */ + +#ifdef HAL_LPTIM_MODULE_ENABLED +#include "stm32f4xx_hal_lptim.h" +#endif /* HAL_LPTIM_MODULE_ENABLED */ + +#ifdef HAL_MMC_MODULE_ENABLED +#include "stm32f4xx_hal_mmc.h" +#endif /* HAL_MMC_MODULE_ENABLED */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert_failed(uint8_t* file, uint32_t line); +#else +#define assert_param(expr) ((void)0U) +#endif /* USE_FULL_ASSERT */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F4xx_HAL_CONF_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From d9be91df954add6faa9b7dc3dd7fdde2b4e5687f Mon Sep 17 00:00:00 2001 From: Jens Wurm Date: Wed, 11 Dec 2024 02:01:12 +0100 Subject: [PATCH 12/16] Update doc a bit --- docs/examples/generic.rst | 2 +- docs/examples/index.rst | 12 +----------- docs/examples/serial.rst | 8 -------- docs/examples/tcp.rst | 4 ++-- examples/generic/README.rst | 2 ++ pyproject.toml | 4 ++-- 6 files changed, 8 insertions(+), 24 deletions(-) delete mode 100644 docs/examples/serial.rst diff --git a/docs/examples/generic.rst b/docs/examples/generic.rst index 5e88f24..386b564 100644 --- a/docs/examples/generic.rst +++ b/docs/examples/generic.rst @@ -10,5 +10,5 @@ Currently one implementation variant is available for: Generic ------- -.. include:: ../../examples/generic/server/README.rst +.. include:: ../../examples/generic/README.rst :start-after: sphinx-marker diff --git a/docs/examples/index.rst b/docs/examples/index.rst index ca6492c..5b96355 100644 --- a/docs/examples/index.rst +++ b/docs/examples/index.rst @@ -3,20 +3,10 @@ Examples ======== -In the following sections examples for serial and Tcp/IP communications can be found with a running server and client -for the accordingly system and can be used as starting point for an own implementations. The examples use the same -program at the client and server side. If the experiment is started, a ramp trajectory is simulated and data (double, -float, int, char) is send to the GUI and can be changed by user interaction. All examples are runnable with the enclosed -server implementations. - -In the Tcp/IP section an additional example completely in `Python` can be found (Generic). The server simulates a -pendulum with two connected connecting rods on a cart. The user can interact directly with the server by control the -force. The position and the angles of the rods can be visualized by graphs or an animation that represents the physical -test rig. .. toctree:: :maxdepth: 1 - serial + generic tcp diff --git a/docs/examples/serial.rst b/docs/examples/serial.rst deleted file mode 100644 index 95685aa..0000000 --- a/docs/examples/serial.rst +++ /dev/null @@ -1,8 +0,0 @@ -====================== -Serial - Communication -====================== - -Currently one implementation variant is available: - -.. contents:: :local: - diff --git a/docs/examples/tcp.rst b/docs/examples/tcp.rst index 9f8c864..5b050c0 100644 --- a/docs/examples/tcp.rst +++ b/docs/examples/tcp.rst @@ -1,8 +1,8 @@ ====================== -Tcp/IP - Communication +TCP/IP - Communication ====================== -Currently two implementation variants are available for: +Currently one implementation variant is available for: .. contents:: :local: diff --git a/examples/generic/README.rst b/examples/generic/README.rst index b0656a8..3f9f9f3 100644 --- a/examples/generic/README.rst +++ b/examples/generic/README.rst @@ -1,6 +1,8 @@ Tool-Libs example: Double Pendulum ================================== +.. sphinx-marker + This is an example of how a testing rig can be implemented using the tool-libs framework. The system equations are generated using a sympy c-printer, after solving the system equations there. diff --git a/pyproject.toml b/pyproject.toml index 0a37771..2095740 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ authors = [ { name = "Jonathan Halmen", email = "jonathan.halmen@umit-tirol.at"}, ] readme = "README.rst" -requires-python = ">=3.8" +requires-python = ">=3.10" keywords = ["pywisp"] license = {file = "LICENSE"} classifiers = [ @@ -33,7 +33,7 @@ dependencies = [ 'PyYAML', 'python-dateutil', 'pandas', - 'sphinx', + "sphinx", 'build', ] From 68ebe2c804818e78443510da7ad0f6c4f84e4862 Mon Sep 17 00:00:00 2001 From: Jonathan Halmen Date: Thu, 12 Dec 2024 13:55:28 +0100 Subject: [PATCH 13/16] readme: fix errors and typo --- examples/generic/README.rst | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/examples/generic/README.rst b/examples/generic/README.rst index 3f9f9f3..76471b5 100644 --- a/examples/generic/README.rst +++ b/examples/generic/README.rst @@ -15,7 +15,6 @@ Requirements * >=Eigen 3.3 * >=cmake 3.24 * >=arm-none-eabi-* 14.2.0 -* ncat (from ``nmap`` project) Simulation ---------- @@ -63,7 +62,7 @@ STM $ make -C build-stm stm -* Upload +* Upload via ST-Link programmer .. code-block:: bash @@ -74,7 +73,7 @@ STM .. code-block:: bash - $ make -C build-stm pload_ota UPLOAD= + $ make -C build-stm upload_ota UPLOAD= * Connect via PyWisp and control the simulation From 6ff0a53b5ffddd0d65a6919f4580cd2bbff52f3c Mon Sep 17 00:00:00 2001 From: Jonathan Halmen Date: Thu, 12 Dec 2024 13:56:03 +0100 Subject: [PATCH 14/16] examples: commit missing dependencies file --- examples/generic/dependencies.cmake | 19 +++++++++++++++++++ examples/generic/sim/CMakeLists.txt | 1 - 2 files changed, 19 insertions(+), 1 deletion(-) create mode 100644 examples/generic/dependencies.cmake diff --git a/examples/generic/dependencies.cmake b/examples/generic/dependencies.cmake new file mode 100644 index 0000000..3603e7d --- /dev/null +++ b/examples/generic/dependencies.cmake @@ -0,0 +1,19 @@ +include(FetchContent) + +if(NOT SIM) + FetchContent_Declare(stm_cmake + GIT_REPOSITORY https://github.com/ObKo/stm32-cmake + GIT_TAG v2.1.0 + GIT_PROGRESS TRUE + ) + FetchContent_MakeAvailable(stm_cmake) + set(CMAKE_TOOLCHAIN_FILE ${stm_cmake_SOURCE_DIR}/cmake/stm32_gcc.cmake) +endif() + +FetchContent_Declare(tool-libs + GIT_REPOSITORY https://github.com/umit-iace/tool-libs + GIT_TAG master + GIT_PROGRESS TRUE +) +FetchContent_MakeAvailable(tool-libs) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) diff --git a/examples/generic/sim/CMakeLists.txt b/examples/generic/sim/CMakeLists.txt index f0730b4..33c6e7f 100644 --- a/examples/generic/sim/CMakeLists.txt +++ b/examples/generic/sim/CMakeLists.txt @@ -13,7 +13,6 @@ target_include_directories(sim PUBLIC . ../inc ) target_link_libraries(sim - tool-libs tool-libs-linux Eigen3::Eigen ) From a66f61e95cecdcc15e127cce2d6d9faf749b673c Mon Sep 17 00:00:00 2001 From: Jonathan Halmen Date: Thu, 12 Dec 2024 13:56:35 +0100 Subject: [PATCH 15/16] gitignore: clean up all these files belong into a build directory instead of being ignored by git. The ones that don't belong into a build directory need to be known by git. --- .gitignore | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index d3975fc..e546487 100644 --- a/.gitignore +++ b/.gitignore @@ -9,7 +9,7 @@ __pycache__/ # Distribution / packaging .Python env/ -build/ +*build* develop-eggs/ dist/ downloads/ @@ -53,9 +53,6 @@ coverage.xml *.log local_settings.py -# Sphinx documentation -docs/_build/ - # PyBuilder target/ @@ -71,9 +68,6 @@ target/ # Pycharm / Clion .idea -# CMake -cmake-build-*/ - # MacOSX Files .DS_Store @@ -95,16 +89,6 @@ slprj/ *.asv Thumbs.db -# Arduino -build-uno - -CMakeCache.txt -CMakeFiles -Makefile -*.cmake - -server - sh.exe.stackdump uv.lock From a774f449ed788eeb7de6abdaad903fdf6e7b3f0d Mon Sep 17 00:00:00 2001 From: Jonathan Halmen Date: Thu, 12 Dec 2024 15:01:12 +0100 Subject: [PATCH 16/16] example: install pywisp as editable otherwise it complains about missing .widgets --- examples/generic/pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/generic/pyproject.toml b/examples/generic/pyproject.toml index e968404..ecf0579 100644 --- a/examples/generic/pyproject.toml +++ b/examples/generic/pyproject.toml @@ -10,4 +10,4 @@ dependencies = [ ] [tool.uv.sources] -pywisp = { path = "../../" } +pywisp = { path = "../../", editable = true }