From e159f33aa50547565c1522691e5d2013a6bc3e03 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Wed, 7 May 2025 19:14:02 -0700 Subject: [PATCH 01/47] commented motor calls --- main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/main.cpp b/main.cpp index 56b29ac..159d78d 100644 --- a/main.cpp +++ b/main.cpp @@ -16,9 +16,9 @@ int main() { PID pid(0.017, 0, 1); // No idea if these values work - Motor motor(PA_8, PA_10, PB_2, PB_1, PB_15, PB_14, pid); // test bench - Motor motor1(PB_3, PB_5, PA_11, PA_12, PA_10, PA_9, pid); // these are the mcpcb - Motor motor2(PA_6, PA_5, PB_14, PB_15, PB_13, PA_8, pid); + // Motor motor(PA_8, PA_10, PB_2, PB_1, PB_15, PB_14, pid); // test bench + // Motor motor1(PB_3, PB_5, PA_11, PA_12, PA_10, PA_9, pid); // these are the mcpcb + // Motor motor2(PA_6, PA_5, PB_14, PB_15, PB_13, PA_8, pid); // on stm with breakout: PB_7 PB_8 From befce3e11ea496f000fabf6f8aac6ecca76393ff Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 10 May 2025 10:42:42 -0700 Subject: [PATCH 02/47] motors r spinning --- .gitignore | 1 + EUSBSerial/EUSBSerial.cpp | 123 +++++ EUSBSerial/EUSBSerial.h | 54 +++ I2CSerial-main.cpp | 29 -- TARGET_ARES/PeripheralPinMaps.h | 263 ----------- TARGET_ARES/PeripheralPins.c | 33 -- TARGET_ARES/PinNames.h | 230 --------- .../TOOLCHAIN_ARM/startup_stm32f401xc.S | 335 -------------- TARGET_ARES/TOOLCHAIN_ARM/stm32f401xc.sct | 68 --- .../TOOLCHAIN_GCC_ARM/startup_stm32f401xc.S | 437 ------------------ TARGET_ARES/TOOLCHAIN_GCC_ARM/stm32f401xc.ld | 217 --------- TARGET_ARES/cmsis_nvic.h | 39 -- TARGET_ARES/flash_data.h | 53 --- TARGET_ARES/system_clock.c | 201 -------- main.cpp | 66 +-- 15 files changed, 200 insertions(+), 1949 deletions(-) create mode 100644 EUSBSerial/EUSBSerial.cpp create mode 100644 EUSBSerial/EUSBSerial.h delete mode 100644 I2CSerial-main.cpp delete mode 100644 TARGET_ARES/PeripheralPinMaps.h delete mode 100644 TARGET_ARES/PeripheralPins.c delete mode 100644 TARGET_ARES/PinNames.h delete mode 100644 TARGET_ARES/TOOLCHAIN_ARM/startup_stm32f401xc.S delete mode 100644 TARGET_ARES/TOOLCHAIN_ARM/stm32f401xc.sct delete mode 100644 TARGET_ARES/TOOLCHAIN_GCC_ARM/startup_stm32f401xc.S delete mode 100644 TARGET_ARES/TOOLCHAIN_GCC_ARM/stm32f401xc.ld delete mode 100644 TARGET_ARES/cmsis_nvic.h delete mode 100644 TARGET_ARES/flash_data.h delete mode 100644 TARGET_ARES/system_clock.c diff --git a/.gitignore b/.gitignore index 82e5343..ebdd0e7 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ projectfiles mbed-os BUILD +BLACKPILL_Custom_Target diff --git a/EUSBSerial/EUSBSerial.cpp b/EUSBSerial/EUSBSerial.cpp new file mode 100644 index 0000000..0995bcd --- /dev/null +++ b/EUSBSerial/EUSBSerial.cpp @@ -0,0 +1,123 @@ +#include "EUSBSerial.h" + +//MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE + + +EUSBSerial::EUSBSerial(uint16_t vid, uint16_t pid) : pc(false, vid, pid) { + +} + +EUSBSerial::~EUSBSerial() { + pc.disconnect(); +} + +bool EUSBSerial::printf(const char* format, ...) { + if (!pc.connected()) + return false; + + Thread t1; + Timer t; + + va_list args; + + va_start(args, format); + this->_format = format; + this->_args = args; + va_end(args); + + t1.start([this]() { _printf(); }); + t.start(); + + while (!this->_success) { + if (t.read_ms() > 500) { + pc.disconnect(); + return false; + } + } + + t1.join(); + + return true; +} + + +void EUSBSerial::_printf() { + this->_success = false; + + char buffer[MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE]; + + size_t n = vsnprintf (buffer, MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE, _format, _args); + + if (n > 0 && n < MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE) { + pc.write(buffer, n); + this->_success = true; + } +} + + +bool EUSBSerial::write(const char* buf, size_t size) { + if (!pc.connected()) + return false; + + Thread t1; + Timer t; + + this->_format = buf; + this->_size = size; + + t1.start([this]() { _write(); }); + t.start(); + + while (!this->_success) { + if (t.read_ms() > 500) { + pc.disconnect(); + return false; + } + } + + t1.join(); + return true; +} + +void EUSBSerial::_write() { + this->_success = false; + + pc.write(this->_format, this->_size); + + this->_success = true; +} + +char EUSBSerial::_getc() { + return pc._getc(); +} + +size_t EUSBSerial::available() { + return pc.available(); +} + +bool EUSBSerial::connected() { + return this->write("\n", 1); +} + +bool EUSBSerial::readline(char* buf, size_t size) { + Timer t; + t.start(); + + size_t i = 0; + while (pc.available()) { + if (i == size) + return false; + + buf[i] = this->_getc(); + if (buf[i] == '\n') { + buf[i] = 0; + return true; + } + i ++; + + if (t.read_ms() > 500) + return false; + } + + return false; +} \ No newline at end of file diff --git a/EUSBSerial/EUSBSerial.h b/EUSBSerial/EUSBSerial.h new file mode 100644 index 0000000..c42c5b8 --- /dev/null +++ b/EUSBSerial/EUSBSerial.h @@ -0,0 +1,54 @@ +/* EUSBSerial + * Ethan Armstrong + * warmst@uw.edu + * ------------------------------------------- + * Extended USB Serial (EUSBSerial) is a class that extends + * the functionality of the standard mbed USBSerial class + * found in USBSerial.h. Features as follows: + * formatted print support (printf) + * newline terminated read support (readline) + * Redundant USB Connection monitoring and protection + * + * EUSBSerial implements QOL features while encapsulating + * writing and reading functions within a seperate thread + * This allows write and read operations to return an error + * rather than block the main thread, or crash the STM +*/ +#include "mbed.h" +#include "USBSerial.h" +#include + +#ifndef MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE +#define MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE 2048 +#endif + +#ifndef _E_USB_SERIAL_H_ +#define _E_USB_SERIAL_H_ + +class EUSBSerial { +public: + EUSBSerial(uint16_t vid=0x1F00, uint16_t pid=0x2012); + ~EUSBSerial(); + + bool printf(const char* format, ...); + bool write(const char* buf, size_t size); + + bool readline(char* buf, size_t size); + + size_t available(); + char _getc(); + + bool connected(); + +private: + const char* _format; + va_list _args; + size_t _size; + bool _success; + USBSerial pc; + + void _printf(); + void _write(); +}; + +#endif //_E_USB_SERIAL_H_ \ No newline at end of file diff --git a/I2CSerial-main.cpp b/I2CSerial-main.cpp deleted file mode 100644 index e842404..0000000 --- a/I2CSerial-main.cpp +++ /dev/null @@ -1,29 +0,0 @@ -/* I2CSerial test functionaility - * adjust main() to change between master and slave - */ - -#include "mbed.h" -#include "EUSBSerial.h" - - -EUSBSerial pc(0x3232, 0x1); -I2C i2c(PB_7, PB_8); - -int ack; -int address; -void scanI2C() { - for(address=0;address<255;address++) { - ack = i2c.write(address, "11", 1); - if (ack == 0) { - pc.printf("\tFound at %3d -- %3x\r\n", address,address); - } - } -} - -int main() { - while (true) { - pc.printf("Hello World!\n"); - scanI2C(); - ThisThread::sleep_for(500ms); - } -} \ No newline at end of file diff --git a/TARGET_ARES/PeripheralPinMaps.h b/TARGET_ARES/PeripheralPinMaps.h deleted file mode 100644 index 959a37f..0000000 --- a/TARGET_ARES/PeripheralPinMaps.h +++ /dev/null @@ -1,263 +0,0 @@ -/* mbed Microcontroller Library - ******************************************************************************* - * Copyright (c) 2018, STMicroelectronics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************* - */ - -//============================================================================== -// Notes -// -// - The pins mentioned Px_y_ALTz are alternative possibilities which use other -// HW peripheral instances. You can use them the same way as any other "normal" -// pin (i.e. PwmOut pwm(PA_7_ALT0);). These pins are not displayed on the board -// pinout image on mbed.org. -// -// - The pins which are connected to other components present on the board have -// the comment "Connected to xxx". The pin function may not work properly in this -// case. These pins may not be displayed on the board pinout image on mbed.org. -// Please read the board reference manual and schematic for more information. -// -// - Warning: pins connected to the default STDIO_UART_TX and STDIO_UART_RX pins are commented -// See https://os.mbed.com/teams/ST/wiki/STDIO for more information. -// -//============================================================================== - -#ifndef MBED_PERIPHERALPINMAPS_H -#define MBED_PERIPHERALPINMAPS_H - -#include - -//*** ADC *** - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_ADC[] = { - {PA_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC1_IN0 - {PA_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC1_IN1 -// {PA_2, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC1_IN2 // Connected to STDIO_UART_TX -// {PA_3, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC1_IN3 // Connected to STDIO_UART_RX - {PA_4, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC1_IN4 -// {PA_5, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC1_IN5 // Connected to LD2 [Green Led] - {PA_6, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC1_IN6 - {PA_7, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC1_IN7 - {PB_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC1_IN8 - {PB_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_IN9 - {PC_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC1_IN10 - {PC_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC1_IN11 - {PC_2, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC1_IN12 - {PC_3, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC1_IN13 - {PC_4, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC1_IN14 - {PC_5, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC1_IN15 - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_ADC_Internal[] = { - {ADC_TEMP, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, - {ADC_VREF, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 17, 0)}, - {ADC_VBAT, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, - {NC, NC, 0} -}; - -//*** I2C *** - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_I2C_SDA[] = { - {PB_3, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF9_I2C2)}, // Connected to SWO - {PB_4, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF9_I2C3)}, - {PB_7, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, - {PB_8, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF9_I2C3)}, - {PB_9, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, - {PB_9_ALT0, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF9_I2C2)}, - {PC_9, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_I2C_SCL[] = { - //{PA_8, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, // Connected to MCO - {PB_6, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, - {PB_8, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, - {PB_10, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, - {NC, NC, 0} -}; - -//*** PWM *** - -// TIM5 cannot be used because already used by the us_ticker -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_PWM[] = { - {PA_0, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 -// {PA_0, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 - {PA_1, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 -// {PA_1, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 -// {PA_2, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 // Connected to STDIO_UART_TX -// {PA_2, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 3, 0)}, // TIM5_CH3 // Connected to STDIO_UART_TX -// {PA_2, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 1, 0)}, // TIM9_CH1 // Connected to STDIO_UART_TX -// {PA_3, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 // Connected to STDIO_UART_RX -// {PA_3, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 4, 0)}, // TIM5_CH4 // Connected to STDIO_UART_RX -// {PA_3, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 2, 0)}, // TIM9_CH2 // Connected to STDIO_UART_RX - {PA_5, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 // Connected to LD2 [Green Led] - {PA_6, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 - {PA_7, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N - {PA_7_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 - {PA_8, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 - {PA_9, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 - {PA_10, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 - {PA_11, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 - {PA_15, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 - {PB_0, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N - {PB_0_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 - {PB_1, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N - {PB_1_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 - {PB_3, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 // Connected to SWO - {PB_4, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 - {PB_5, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 - {PB_6, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 - {PB_7, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 - {PB_8, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 - {PB_8_ALT0, PWM_10, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM10, 1, 0)}, // TIM10_CH1 - {PB_9, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 - {PB_9_ALT0, PWM_11, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM11, 1, 0)}, // TIM11_CH1 - {PB_10, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 - {PB_13, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N - {PB_14, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N - {PB_15, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N - {PC_6, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 - {PC_7, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 - {PC_8, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 - {PC_9, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 - {NC, NC, 0} -}; - -//*** SERIAL *** - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_UART_TX[] = { - {PA_2, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, // Connected to STDIO_UART_TX - {PA_9, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {PA_11, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, - {PA_15, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {PB_6, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {PC_6, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_UART_RX[] = { - {PA_3, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, // Connected to STDIO_UART_RX - {PA_10, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {PA_12, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, - {PB_3, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to SWO - {PB_7, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {PC_7, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_UART_RTS[] = { - {PA_1, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, - {PA_12, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_UART_CTS[] = { - {PA_0, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, - {PA_11, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, - {NC, NC, 0} -}; - -//*** SPI *** - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_SPI_MOSI[] = { - {PA_1, SPI_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI4)}, - {PA_7, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, -// {PA_10, SPI_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI5)}, - {PB_5, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, - {PB_5_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, -// {PB_8, SPI_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI5)}, - {PB_15, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PC_3, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PC_12, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_SPI_MISO[] = { - {PA_6, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, -// {PA_11, SPI_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI4)}, -// {PA_12, SPI_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI5)}, - {PB_4, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, - {PB_4_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, - {PB_14, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PC_2, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PC_11, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_SPI_SCLK[] = { - {PA_5, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to LD2 [Green Led] -// {PB_0, SPI_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI5)}, - {PB_3, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to SWO - {PB_3_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, // Connected to SWO - {PB_10, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, -// {PB_12, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF7_SPI3)}, - {PB_13, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, -// {PB_13_ALT0, SPI_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI4)}, - {PC_7, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PC_10, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, - {NC, NC, 0} -}; - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_SPI_SSEL[] = { - {PA_4, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, - {PA_4_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, - {PA_15, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, - {PA_15_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, -// {PB_1, SPI_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI5)}, - {PB_9, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, - {PB_12, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, -// {PB_12_ALT0, SPI_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI4)}, - {NC, NC, 0} -}; - -//*** USBDEVICE *** - -MSTD_CONSTEXPR_OBJ_11 PinMap PinMap_USB_FS[] = { -// {PA_8, USB_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG_FS)}, // USB_OTG_FS_SOF - {PA_9, USB_FS, STM_PIN_DATA(STM_MODE_INPUT, GPIO_NOPULL, GPIO_AF10_OTG_FS)}, // USB_OTG_FS_VBUS - {PA_10, USB_FS, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_PULLUP, GPIO_AF10_OTG_FS)}, // USB_OTG_FS_ID - {PA_11, USB_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG_FS)}, // USB_OTG_FS_DM - {PA_12, USB_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG_FS)}, // USB_OTG_FS_DP - {NC, NC, 0} -}; - -#define PINMAP_ANALOGIN PinMap_ADC -#define PINMAP_ANALOGIN_INTERNAL PinMap_ADC_Internal -#define PINMAP_I2C_SDA PinMap_I2C_SDA -#define PINMAP_I2C_SCL PinMap_I2C_SCL -#define PINMAP_UART_TX PinMap_UART_TX -#define PINMAP_UART_RX PinMap_UART_RX -#define PINMAP_UART_CTS PinMap_UART_CTS -#define PINMAP_UART_RTS PinMap_UART_RTS -#define PINMAP_SPI_SCLK PinMap_SPI_SCLK -#define PINMAP_SPI_MOSI PinMap_SPI_MOSI -#define PINMAP_SPI_MISO PinMap_SPI_MISO -#define PINMAP_SPI_SSEL PinMap_SPI_SSEL -#define PINMAP_PWM PinMap_PWM - -#endif diff --git a/TARGET_ARES/PeripheralPins.c b/TARGET_ARES/PeripheralPins.c deleted file mode 100644 index 67ab048..0000000 --- a/TARGET_ARES/PeripheralPins.c +++ /dev/null @@ -1,33 +0,0 @@ -/* mbed Microcontroller Library - ******************************************************************************* - * Copyright (c) 2018, STMicroelectronics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************* - */ - -#include "PeripheralPins.h" -#include "mbed_toolchain.h" -#include "PeripheralPinMaps.h" diff --git a/TARGET_ARES/PinNames.h b/TARGET_ARES/PinNames.h deleted file mode 100644 index adc0e0a..0000000 --- a/TARGET_ARES/PinNames.h +++ /dev/null @@ -1,230 +0,0 @@ -/* mbed Microcontroller Library - ******************************************************************************* - * Copyright (c) 2018, STMicroelectronics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************* - */ - -#ifndef MBED_PINNAMES_H -#define MBED_PINNAMES_H - -#include "cmsis.h" -#include "PinNamesTypes.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/* If this macro is defined, then constexpr utility functions for pin-map seach can be used. */ -#define STATIC_PINMAP_READY 1 - -typedef enum { - ALT0 = 0x100, - ALT1 = 0x200, - ALT2 = 0x300, - ALT3 = 0x400 -} ALTx; - -typedef enum { - // Not connected - NC = (int)0xFFFFFFFF, - - PA_0 = 0x00, - PA_1 = 0x01, - PA_1_ALT0 = PA_1 | ALT0, - PA_2 = 0x02, - PA_3 = 0x03, - PA_4 = 0x04, - PA_4_ALT0 = PA_4 | ALT0, - PA_5 = 0x05, - PA_6 = 0x06, - PA_7 = 0x07, - PA_7_ALT0 = PA_7 | ALT0, - PA_7_ALT1 = PA_7 | ALT1, - PA_7_ALT2 = PA_7 | ALT2, - PA_8 = 0x08, - PA_9 = 0x09, - PA_10 = 0x0A, - PA_11 = 0x0B, - PA_12 = 0x0C, - PA_13 = 0x0D, - PA_14 = 0x0E, - PA_15 = 0x0F, - PA_15_ALT0 = PA_15 | ALT0, - - PB_0 = 0x10, - PB_0_ALT0 = PB_0 | ALT0, - PB_0_ALT1 = PB_0 | ALT1, - PB_1 = 0x11, - PB_1_ALT0 = PB_1 | ALT0, - PB_1_ALT1 = PB_1 | ALT1, - PB_2 = 0x12, - PB_3 = 0x13, - PB_3_ALT0 = PB_3 | ALT0, - PB_4 = 0x14, - PB_4_ALT0 = PB_4 | ALT0, - PB_4_ALT1 = PB_4 | ALT1, - PB_5 = 0x15, - PB_5_ALT0 = PB_5 | ALT0, - PB_5_ALT1 = PB_5 | ALT1, - PB_6 = 0x16, - PB_7 = 0x17, - PB_8 = 0x18, - PB_8_ALT0 = PB_8 | ALT0, - PB_8_ALT1 = PB_8 | ALT1, - PB_9 = 0x19, - PB_9_ALT0 = PB_9 | ALT0, - PB_9_ALT1 = PB_9 | ALT1, - PB_10 = 0x1A, - PB_12 = 0x1C, - PB_12_ALT0 = PB_12 | ALT0, - PB_13 = 0x1D, - PB_13_ALT0 = PB_13 | ALT0, - PB_14 = 0x1E, - PB_15 = 0x1F, - - PC_0 = 0x20, - PC_1 = 0x21, - PC_2 = 0x22, - PC_3 = 0x23, - PC_4 = 0x24, - PC_5 = 0x25, - PC_6 = 0x26, - PC_7 = 0x27, - PC_8 = 0x28, - PC_9 = 0x29, - PC_10 = 0x2A, - PC_11 = 0x2B, - PC_12 = 0x2C, - PC_13 = 0x2D, - PC_14 = 0x2E, - PC_15 = 0x2F, - - PD_2 = 0x32, - - PH_0 = 0x70, - PH_1 = 0x71, - - // ADC internal channels - ADC_TEMP = 0xF0, - ADC_VREF = 0xF1, - ADC_VBAT = 0xF2, - - // Arduino connector namings - A0 = PA_0, - A1 = PA_1, - A2 = PA_4, - A3 = PB_0, - A4 = NC, - A5 = NC, - D0 = PA_3, - D1 = PA_2, - D2 = PA_10, - D3 = PB_3, - D4 = PB_5, - D5 = PB_4, - D6 = PB_10, - D7 = PA_8, - D8 = PA_9, - D9 = NC, - D10 = PB_6, - D11 = PA_7, - D12 = PA_6, - D13 = PA_5, - D14 = PB_9, - D15 = PB_8, - - // STDIO for console print -#ifdef MBED_CONF_TARGET_STDIO_UART_TX - CONSOLE_TX = MBED_CONF_TARGET_STDIO_UART_TX, -#else - CONSOLE_TX = PA_2, -#endif -#ifdef MBED_CONF_TARGET_STDIO_UART_RX - CONSOLE_RX = MBED_CONF_TARGET_STDIO_UART_RX, -#else - CONSOLE_RX = PA_3, -#endif - - // Generic signals namings - LED1 = PC_13, - LED2 = PC_13, - LED3 = PC_13, - LED4 = PC_13, - LED_RED = PC_13, - USER_BUTTON = PA_0, - - // Standardized button names - BUTTON1 = USER_BUTTON, - - // Serial backword compatibility - STDIO_UART_TX = CONSOLE_TX, - STDIO_UART_RX = CONSOLE_RX, - SERIAL_TX = CONSOLE_TX, - SERIAL_RX = CONSOLE_RX, -#ifndef USBTX - USBTX = CONSOLE_TX, -#endif -#ifndef USBRX - USBRX = CONSOLE_RX, -#endif - - I2C_SCL = PB_8, - I2C_SDA = PB_9, - SPI_MOSI = PA_7, - SPI_MISO = PA_6, - SPI_SCK = PA_5, - SPI_CS = PB_6, - PWM_OUT = PB_3, - - /**** USB FS pins ****/ - USB_OTG_FS_DM = PA_11, - USB_OTG_FS_DP = PA_12, - USB_OTG_FS_ID = PA_10, - USB_OTG_FS_SOF = PA_8, - USB_OTG_FS_VBUS = PA_9, - - /**** OSCILLATOR pins ****/ - RCC_OSC32_IN = PC_14, - RCC_OSC32_OUT = PC_15, - RCC_OSC_IN = PH_0, - RCC_OSC_OUT = PH_1, - - /**** DEBUG pins ****/ - SYS_JTCK_SWCLK = PA_14, - SYS_JTDI = PA_15, - SYS_JTDO_SWO = PB_3, - SYS_JTMS_SWDIO = PA_13, - SYS_JTRST = PB_4, - SYS_WKUP = PA_0, - -} PinName; - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/TARGET_ARES/TOOLCHAIN_ARM/startup_stm32f401xc.S b/TARGET_ARES/TOOLCHAIN_ARM/startup_stm32f401xc.S deleted file mode 100644 index 0f540c3..0000000 --- a/TARGET_ARES/TOOLCHAIN_ARM/startup_stm32f401xc.S +++ /dev/null @@ -1,335 +0,0 @@ -;******************************************************************************* -;* File Name : startup_stm32f411xe.s -;* Author : MCD Application Team -;* Description : STM32F411xExx devices vector table for MDK-ARM toolchain. -;* This module performs: -;* - Set the initial SP -;* - Set the initial PC == Reset_Handler -;* - Set the vector table entries with the exceptions ISR address -;* - Branches to __main in the C library (which eventually -;* calls main()). -;* After Reset the CortexM4 processor is in Thread mode, -;* priority is Privileged, and the Stack is set to Main. -;******************************************************************************** -;* @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 -;* -;******************************************************************************* -;* <<< Use Configuration Wizard in Context Menu >>> -; - PRESERVE8 - THUMB - - -; Vector Table Mapped to Address 0 at Reset - AREA RESET, DATA, READONLY - EXPORT __Vectors - EXPORT __Vectors_End - EXPORT __Vectors_Size - - IMPORT |Image$$ARM_LIB_STACK$$ZI$$Limit| -__Vectors DCD |Image$$ARM_LIB_STACK$$ZI$$Limit| ; Top of Stack - DCD Reset_Handler ; Reset Handler - DCD NMI_Handler ; NMI Handler - DCD HardFault_Handler ; Hard Fault Handler - DCD MemManage_Handler ; MPU Fault Handler - DCD BusFault_Handler ; Bus Fault Handler - DCD UsageFault_Handler ; Usage Fault Handler - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SVC_Handler ; SVCall Handler - DCD DebugMon_Handler ; Debug Monitor Handler - DCD 0 ; Reserved - DCD PendSV_Handler ; PendSV Handler - DCD SysTick_Handler ; SysTick Handler - - ; External Interrupts - DCD WWDG_IRQHandler ; Window WatchDog - DCD PVD_IRQHandler ; PVD through EXTI Line detection - DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line - DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line - DCD FLASH_IRQHandler ; FLASH - DCD RCC_IRQHandler ; RCC - DCD EXTI0_IRQHandler ; EXTI Line0 - DCD EXTI1_IRQHandler ; EXTI Line1 - DCD EXTI2_IRQHandler ; EXTI Line2 - DCD EXTI3_IRQHandler ; EXTI Line3 - DCD EXTI4_IRQHandler ; EXTI Line4 - DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0 - DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1 - DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2 - DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3 - DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4 - DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5 - DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6 - DCD ADC_IRQHandler ; ADC1, ADC2 and ADC3s - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD EXTI9_5_IRQHandler ; External Line[9:5]s - DCD TIM1_BRK_TIM9_IRQHandler ; TIM1 Break and TIM9 - DCD TIM1_UP_TIM10_IRQHandler ; TIM1 Update and TIM10 - DCD TIM1_TRG_COM_TIM11_IRQHandler ; TIM1 Trigger and Commutation and TIM11 - DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare - DCD TIM2_IRQHandler ; TIM2 - DCD TIM3_IRQHandler ; TIM3 - DCD TIM4_IRQHandler ; TIM4 - DCD I2C1_EV_IRQHandler ; I2C1 Event - DCD I2C1_ER_IRQHandler ; I2C1 Error - DCD I2C2_EV_IRQHandler ; I2C2 Event - DCD I2C2_ER_IRQHandler ; I2C2 Error - DCD SPI1_IRQHandler ; SPI1 - DCD SPI2_IRQHandler ; SPI2 - DCD USART1_IRQHandler ; USART1 - DCD USART2_IRQHandler ; USART2 - DCD 0 ; Reserved - DCD EXTI15_10_IRQHandler ; External Line[15:10]s - DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line - DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI line - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7 - DCD 0 ; Reserved - DCD SDIO_IRQHandler ; SDIO - DCD TIM5_IRQHandler ; TIM5 - DCD SPI3_IRQHandler ; SPI3 - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0 - DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1 - DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2 - DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3 - DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4 - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD OTG_FS_IRQHandler ; USB OTG FS - DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5 - DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6 - DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7 - DCD USART6_IRQHandler ; USART6 - DCD I2C3_EV_IRQHandler ; I2C3 event - DCD I2C3_ER_IRQHandler ; I2C3 error - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD FPU_IRQHandler ; FPU - DCD 0 ; Reserved - DCD 0 ; Reserved - DCD SPI4_IRQHandler ; SPI4 - DCD SPI5_IRQHandler ; SPI5 - -__Vectors_End - -__Vectors_Size EQU __Vectors_End - __Vectors - - AREA |.text|, CODE, READONLY - -; Reset handler -Reset_Handler PROC - EXPORT Reset_Handler [WEAK] - IMPORT SystemInit - IMPORT __main - - LDR R0, =SystemInit - BLX R0 - LDR R0, =__main - BX R0 - ENDP - -; Dummy Exception Handlers (infinite loops which can be modified) - -NMI_Handler PROC - EXPORT NMI_Handler [WEAK] - B . - ENDP -HardFault_Handler\ - PROC - EXPORT HardFault_Handler [WEAK] - B . - ENDP -MemManage_Handler\ - PROC - EXPORT MemManage_Handler [WEAK] - B . - ENDP -BusFault_Handler\ - PROC - EXPORT BusFault_Handler [WEAK] - B . - ENDP -UsageFault_Handler\ - PROC - EXPORT UsageFault_Handler [WEAK] - B . - ENDP -SVC_Handler PROC - EXPORT SVC_Handler [WEAK] - B . - ENDP -DebugMon_Handler\ - PROC - EXPORT DebugMon_Handler [WEAK] - B . - ENDP -PendSV_Handler PROC - EXPORT PendSV_Handler [WEAK] - B . - ENDP -SysTick_Handler PROC - EXPORT SysTick_Handler [WEAK] - B . - ENDP - -Default_Handler PROC - - EXPORT WWDG_IRQHandler [WEAK] - EXPORT PVD_IRQHandler [WEAK] - EXPORT TAMP_STAMP_IRQHandler [WEAK] - EXPORT RTC_WKUP_IRQHandler [WEAK] - EXPORT FLASH_IRQHandler [WEAK] - EXPORT RCC_IRQHandler [WEAK] - EXPORT EXTI0_IRQHandler [WEAK] - EXPORT EXTI1_IRQHandler [WEAK] - EXPORT EXTI2_IRQHandler [WEAK] - EXPORT EXTI3_IRQHandler [WEAK] - EXPORT EXTI4_IRQHandler [WEAK] - EXPORT DMA1_Stream0_IRQHandler [WEAK] - EXPORT DMA1_Stream1_IRQHandler [WEAK] - EXPORT DMA1_Stream2_IRQHandler [WEAK] - EXPORT DMA1_Stream3_IRQHandler [WEAK] - EXPORT DMA1_Stream4_IRQHandler [WEAK] - EXPORT DMA1_Stream5_IRQHandler [WEAK] - EXPORT DMA1_Stream6_IRQHandler [WEAK] - EXPORT ADC_IRQHandler [WEAK] - EXPORT EXTI9_5_IRQHandler [WEAK] - EXPORT TIM1_BRK_TIM9_IRQHandler [WEAK] - EXPORT TIM1_UP_TIM10_IRQHandler [WEAK] - EXPORT TIM1_TRG_COM_TIM11_IRQHandler [WEAK] - EXPORT TIM1_CC_IRQHandler [WEAK] - EXPORT TIM2_IRQHandler [WEAK] - EXPORT TIM3_IRQHandler [WEAK] - EXPORT TIM4_IRQHandler [WEAK] - EXPORT I2C1_EV_IRQHandler [WEAK] - EXPORT I2C1_ER_IRQHandler [WEAK] - EXPORT I2C2_EV_IRQHandler [WEAK] - EXPORT I2C2_ER_IRQHandler [WEAK] - EXPORT SPI1_IRQHandler [WEAK] - EXPORT SPI2_IRQHandler [WEAK] - EXPORT USART1_IRQHandler [WEAK] - EXPORT USART2_IRQHandler [WEAK] - EXPORT EXTI15_10_IRQHandler [WEAK] - EXPORT RTC_Alarm_IRQHandler [WEAK] - EXPORT OTG_FS_WKUP_IRQHandler [WEAK] - EXPORT DMA1_Stream7_IRQHandler [WEAK] - EXPORT SDIO_IRQHandler [WEAK] - EXPORT TIM5_IRQHandler [WEAK] - EXPORT SPI3_IRQHandler [WEAK] - EXPORT DMA2_Stream0_IRQHandler [WEAK] - EXPORT DMA2_Stream1_IRQHandler [WEAK] - EXPORT DMA2_Stream2_IRQHandler [WEAK] - EXPORT DMA2_Stream3_IRQHandler [WEAK] - EXPORT DMA2_Stream4_IRQHandler [WEAK] - EXPORT OTG_FS_IRQHandler [WEAK] - EXPORT DMA2_Stream5_IRQHandler [WEAK] - EXPORT DMA2_Stream6_IRQHandler [WEAK] - EXPORT DMA2_Stream7_IRQHandler [WEAK] - EXPORT USART6_IRQHandler [WEAK] - EXPORT I2C3_EV_IRQHandler [WEAK] - EXPORT I2C3_ER_IRQHandler [WEAK] - EXPORT FPU_IRQHandler [WEAK] - EXPORT SPI4_IRQHandler [WEAK] - EXPORT SPI5_IRQHandler [WEAK] - -WWDG_IRQHandler -PVD_IRQHandler -TAMP_STAMP_IRQHandler -RTC_WKUP_IRQHandler -FLASH_IRQHandler -RCC_IRQHandler -EXTI0_IRQHandler -EXTI1_IRQHandler -EXTI2_IRQHandler -EXTI3_IRQHandler -EXTI4_IRQHandler -DMA1_Stream0_IRQHandler -DMA1_Stream1_IRQHandler -DMA1_Stream2_IRQHandler -DMA1_Stream3_IRQHandler -DMA1_Stream4_IRQHandler -DMA1_Stream5_IRQHandler -DMA1_Stream6_IRQHandler -ADC_IRQHandler -EXTI9_5_IRQHandler -TIM1_BRK_TIM9_IRQHandler -TIM1_UP_TIM10_IRQHandler -TIM1_TRG_COM_TIM11_IRQHandler -TIM1_CC_IRQHandler -TIM2_IRQHandler -TIM3_IRQHandler -TIM4_IRQHandler -I2C1_EV_IRQHandler -I2C1_ER_IRQHandler -I2C2_EV_IRQHandler -I2C2_ER_IRQHandler -SPI1_IRQHandler -SPI2_IRQHandler -USART1_IRQHandler -USART2_IRQHandler -EXTI15_10_IRQHandler -RTC_Alarm_IRQHandler -OTG_FS_WKUP_IRQHandler -DMA1_Stream7_IRQHandler -SDIO_IRQHandler -TIM5_IRQHandler -SPI3_IRQHandler -DMA2_Stream0_IRQHandler -DMA2_Stream1_IRQHandler -DMA2_Stream2_IRQHandler -DMA2_Stream3_IRQHandler -DMA2_Stream4_IRQHandler -OTG_FS_IRQHandler -DMA2_Stream5_IRQHandler -DMA2_Stream6_IRQHandler -DMA2_Stream7_IRQHandler -USART6_IRQHandler -I2C3_EV_IRQHandler -I2C3_ER_IRQHandler -FPU_IRQHandler -SPI4_IRQHandler -SPI5_IRQHandler - - B . - - ENDP - - ALIGN - -;******************************************************************************* -; User Stack and Heap initialization -;******************************************************************************* - - END - -;************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE***** diff --git a/TARGET_ARES/TOOLCHAIN_ARM/stm32f401xc.sct b/TARGET_ARES/TOOLCHAIN_ARM/stm32f401xc.sct deleted file mode 100644 index 827b6a9..0000000 --- a/TARGET_ARES/TOOLCHAIN_ARM/stm32f401xc.sct +++ /dev/null @@ -1,68 +0,0 @@ -#! armclang -E --target=arm-arm-none-eabi -x c -mcpu=cortex-m4 -; Scatter-Loading Description File -; -; SPDX-License-Identifier: BSD-3-Clause -;****************************************************************************** -;* @attention -;* -;* Copyright (c) 2016-2020 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 -;* -;****************************************************************************** - -#include "../cmsis_nvic.h" - -#if !defined(MBED_APP_START) - #define MBED_APP_START MBED_ROM_START -#endif - -#if !defined(MBED_APP_SIZE) - #define MBED_APP_SIZE MBED_ROM_SIZE -#endif - -#if !defined(MBED_CONF_TARGET_BOOT_STACK_SIZE) -/* This value is normally defined by the tools to 0x1000 for bare metal and 0x400 for RTOS */ -#if defined(MBED_BOOT_STACK_SIZE) -#define MBED_CONF_TARGET_BOOT_STACK_SIZE MBED_BOOT_STACK_SIZE -#else -#define MBED_CONF_TARGET_BOOT_STACK_SIZE 0x400 -#endif -#endif - -/* Round up VECTORS_SIZE to 8 bytes */ -#define VECTORS_SIZE (((NVIC_NUM_VECTORS * 4) + 7) AND ~7) - -#define MBED_CRASH_REPORT_RAM_SIZE 0x100 - -#define MBED_IRAM1_START (MBED_RAM_START + VECTORS_SIZE + MBED_CRASH_REPORT_RAM_SIZE) -#define MBED_IRAM1_SIZE (MBED_RAM_SIZE - VECTORS_SIZE - MBED_CRASH_REPORT_RAM_SIZE) - - -#define RAM_FIXED_SIZE (MBED_CONF_TARGET_BOOT_STACK_SIZE + VECTORS_SIZE + MBED_CRASH_REPORT_RAM_SIZE) - -LR_IROM1 MBED_APP_START MBED_APP_SIZE { - - ER_IROM1 MBED_APP_START MBED_APP_SIZE { - *.o (RESET, +First) - *(InRoot$$Sections) - .ANY (+RO) - } - - RW_m_crash_data (MBED_RAM_START + VECTORS_SIZE) EMPTY MBED_CRASH_REPORT_RAM_SIZE { ; RW data - } - - RW_IRAM1 MBED_IRAM1_START MBED_IRAM1_SIZE { ; RW data - .ANY (+RW +ZI) - } - - ARM_LIB_HEAP AlignExpr(+0, 16) EMPTY (MBED_IRAM1_START + MBED_RAM_SIZE - RAM_FIXED_SIZE - AlignExpr(ImageLimit(RW_IRAM1), 16)) { ; Heap growing up - } - - ARM_LIB_STACK (MBED_RAM_START + MBED_RAM_SIZE) EMPTY -MBED_CONF_TARGET_BOOT_STACK_SIZE { ; Stack region growing down - } -} diff --git a/TARGET_ARES/TOOLCHAIN_GCC_ARM/startup_stm32f401xc.S b/TARGET_ARES/TOOLCHAIN_GCC_ARM/startup_stm32f401xc.S deleted file mode 100644 index 3214107..0000000 --- a/TARGET_ARES/TOOLCHAIN_GCC_ARM/startup_stm32f401xc.S +++ /dev/null @@ -1,437 +0,0 @@ -/** - ****************************************************************************** - * @file startup_stm32f411xe.s - * @author MCD Application Team - * @brief STM32F411xExx Devices vector table for GCC based toolchains. - * This module performs: - * - Set the initial SP - * - Set the initial PC == Reset_Handler, - * - Set the vector table entries with the exceptions ISR address - * - Branches to main in the C library (which eventually - * calls main()). - * After Reset the Cortex-M4 processor is in Thread mode, - * priority is Privileged, and the Stack is set to Main. - ****************************************************************************** - * @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 - * - ****************************************************************************** - */ - - .syntax unified - .cpu cortex-m4 - .fpu softvfp - .thumb - -.global g_pfnVectors -.global Default_Handler - -/* start address for the initialization values of the .data section. -defined in linker script */ -.word _sidata -/* start address for the .data section. defined in linker script */ -.word _sdata -/* end address for the .data section. defined in linker script */ -.word _edata -/* start address for the .bss section. defined in linker script */ -.word _sbss -/* end address for the .bss section. defined in linker script */ -.word _ebss -/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ - -/** - * @brief This is the code that gets called when the processor first - * starts execution following a reset event. Only the absolutely - * necessary set is performed, after which the application - * supplied main() routine is called. - * @param None - * @retval : None -*/ - - .section .text.Reset_Handler - .weak Reset_Handler - .type Reset_Handler, %function -Reset_Handler: - ldr sp, =_estack /* set stack pointer */ - -/* Copy the data segment initializers from flash to SRAM */ - ldr r0, =_sdata - ldr r1, =_edata - ldr r2, =_sidata - movs r3, #0 - b LoopCopyDataInit - -CopyDataInit: - ldr r4, [r2, r3] - str r4, [r0, r3] - adds r3, r3, #4 - -LoopCopyDataInit: - adds r4, r0, r3 - cmp r4, r1 - bcc CopyDataInit - -/* Zero fill the bss segment. */ - ldr r2, =_sbss - ldr r4, =_ebss - movs r3, #0 - b LoopFillZerobss - -FillZerobss: - str r3, [r2] - adds r2, r2, #4 - -LoopFillZerobss: - cmp r2, r4 - bcc FillZerobss - -/* Call the clock system intitialization function.*/ - bl SystemInit - bl _start - bx lr - bx lr -.size Reset_Handler, .-Reset_Handler - -/** - * @brief This is the code that gets called when the processor receives an - * unexpected interrupt. This simply enters an infinite loop, preserving - * the system state for examination by a debugger. - * @param None - * @retval None -*/ - .section .text.Default_Handler,"ax",%progbits -Default_Handler: -Infinite_Loop: - b Infinite_Loop - .size Default_Handler, .-Default_Handler -/****************************************************************************** -* -* The minimal vector table for a Cortex M3. Note that the proper constructs -* must be placed on this to ensure that it ends up at physical address -* 0x0000.0000. -* -*******************************************************************************/ - .section .isr_vector,"a",%progbits - .type g_pfnVectors, %object - .size g_pfnVectors, .-g_pfnVectors - -g_pfnVectors: - .word _estack - .word Reset_Handler - .word NMI_Handler - .word HardFault_Handler - .word MemManage_Handler - .word BusFault_Handler - .word UsageFault_Handler - .word 0 - .word 0 - .word 0 - .word 0 - .word SVC_Handler - .word DebugMon_Handler - .word 0 - .word PendSV_Handler - .word SysTick_Handler - - /* External Interrupts */ - .word WWDG_IRQHandler /* Window WatchDog */ - .word PVD_IRQHandler /* PVD through EXTI Line detection */ - .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ - .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ - .word FLASH_IRQHandler /* FLASH */ - .word RCC_IRQHandler /* RCC */ - .word EXTI0_IRQHandler /* EXTI Line0 */ - .word EXTI1_IRQHandler /* EXTI Line1 */ - .word EXTI2_IRQHandler /* EXTI Line2 */ - .word EXTI3_IRQHandler /* EXTI Line3 */ - .word EXTI4_IRQHandler /* EXTI Line4 */ - .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ - .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ - .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ - .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ - .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ - .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ - .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ - .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word EXTI9_5_IRQHandler /* External Line[9:5]s */ - .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ - .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ - .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ - .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ - .word TIM2_IRQHandler /* TIM2 */ - .word TIM3_IRQHandler /* TIM3 */ - .word TIM4_IRQHandler /* TIM4 */ - .word I2C1_EV_IRQHandler /* I2C1 Event */ - .word I2C1_ER_IRQHandler /* I2C1 Error */ - .word I2C2_EV_IRQHandler /* I2C2 Event */ - .word I2C2_ER_IRQHandler /* I2C2 Error */ - .word SPI1_IRQHandler /* SPI1 */ - .word SPI2_IRQHandler /* SPI2 */ - .word USART1_IRQHandler /* USART1 */ - .word USART2_IRQHandler /* USART2 */ - .word 0 /* Reserved */ - .word EXTI15_10_IRQHandler /* External Line[15:10]s */ - .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ - .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ - .word 0 /* Reserved */ - .word SDIO_IRQHandler /* SDIO */ - .word TIM5_IRQHandler /* TIM5 */ - .word SPI3_IRQHandler /* SPI3 */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ - .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ - .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ - .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ - .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word OTG_FS_IRQHandler /* USB OTG FS */ - .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ - .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ - .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ - .word USART6_IRQHandler /* USART6 */ - .word I2C3_EV_IRQHandler /* I2C3 event */ - .word I2C3_ER_IRQHandler /* I2C3 error */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word FPU_IRQHandler /* FPU */ - .word 0 /* Reserved */ - .word 0 /* Reserved */ - .word SPI4_IRQHandler /* SPI4 */ - .word SPI5_IRQHandler /* SPI5 */ - -/******************************************************************************* -* -* Provide weak aliases for each Exception handler to the Default_Handler. -* As they are weak aliases, any function with the same name will override -* this definition. -* -*******************************************************************************/ - .weak NMI_Handler - .thumb_set NMI_Handler,Default_Handler - - .weak HardFault_Handler - .thumb_set HardFault_Handler,Default_Handler - - .weak MemManage_Handler - .thumb_set MemManage_Handler,Default_Handler - - .weak BusFault_Handler - .thumb_set BusFault_Handler,Default_Handler - - .weak UsageFault_Handler - .thumb_set UsageFault_Handler,Default_Handler - - .weak SVC_Handler - .thumb_set SVC_Handler,Default_Handler - - .weak DebugMon_Handler - .thumb_set DebugMon_Handler,Default_Handler - - .weak PendSV_Handler - .thumb_set PendSV_Handler,Default_Handler - - .weak SysTick_Handler - .thumb_set SysTick_Handler,Default_Handler - - .weak WWDG_IRQHandler - .thumb_set WWDG_IRQHandler,Default_Handler - - .weak PVD_IRQHandler - .thumb_set PVD_IRQHandler,Default_Handler - - .weak TAMP_STAMP_IRQHandler - .thumb_set TAMP_STAMP_IRQHandler,Default_Handler - - .weak RTC_WKUP_IRQHandler - .thumb_set RTC_WKUP_IRQHandler,Default_Handler - - .weak FLASH_IRQHandler - .thumb_set FLASH_IRQHandler,Default_Handler - - .weak RCC_IRQHandler - .thumb_set RCC_IRQHandler,Default_Handler - - .weak EXTI0_IRQHandler - .thumb_set EXTI0_IRQHandler,Default_Handler - - .weak EXTI1_IRQHandler - .thumb_set EXTI1_IRQHandler,Default_Handler - - .weak EXTI2_IRQHandler - .thumb_set EXTI2_IRQHandler,Default_Handler - - .weak EXTI3_IRQHandler - .thumb_set EXTI3_IRQHandler,Default_Handler - - .weak EXTI4_IRQHandler - .thumb_set EXTI4_IRQHandler,Default_Handler - - .weak DMA1_Stream0_IRQHandler - .thumb_set DMA1_Stream0_IRQHandler,Default_Handler - - .weak DMA1_Stream1_IRQHandler - .thumb_set DMA1_Stream1_IRQHandler,Default_Handler - - .weak DMA1_Stream2_IRQHandler - .thumb_set DMA1_Stream2_IRQHandler,Default_Handler - - .weak DMA1_Stream3_IRQHandler - .thumb_set DMA1_Stream3_IRQHandler,Default_Handler - - .weak DMA1_Stream4_IRQHandler - .thumb_set DMA1_Stream4_IRQHandler,Default_Handler - - .weak DMA1_Stream5_IRQHandler - .thumb_set DMA1_Stream5_IRQHandler,Default_Handler - - .weak DMA1_Stream6_IRQHandler - .thumb_set DMA1_Stream6_IRQHandler,Default_Handler - - .weak ADC_IRQHandler - .thumb_set ADC_IRQHandler,Default_Handler - - .weak EXTI9_5_IRQHandler - .thumb_set EXTI9_5_IRQHandler,Default_Handler - - .weak TIM1_BRK_TIM9_IRQHandler - .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler - - .weak TIM1_UP_TIM10_IRQHandler - .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler - - .weak TIM1_TRG_COM_TIM11_IRQHandler - .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler - - .weak TIM1_CC_IRQHandler - .thumb_set TIM1_CC_IRQHandler,Default_Handler - - .weak TIM2_IRQHandler - .thumb_set TIM2_IRQHandler,Default_Handler - - .weak TIM3_IRQHandler - .thumb_set TIM3_IRQHandler,Default_Handler - - .weak TIM4_IRQHandler - .thumb_set TIM4_IRQHandler,Default_Handler - - .weak I2C1_EV_IRQHandler - .thumb_set I2C1_EV_IRQHandler,Default_Handler - - .weak I2C1_ER_IRQHandler - .thumb_set I2C1_ER_IRQHandler,Default_Handler - - .weak I2C2_EV_IRQHandler - .thumb_set I2C2_EV_IRQHandler,Default_Handler - - .weak I2C2_ER_IRQHandler - .thumb_set I2C2_ER_IRQHandler,Default_Handler - - .weak SPI1_IRQHandler - .thumb_set SPI1_IRQHandler,Default_Handler - - .weak SPI2_IRQHandler - .thumb_set SPI2_IRQHandler,Default_Handler - - .weak USART1_IRQHandler - .thumb_set USART1_IRQHandler,Default_Handler - - .weak USART2_IRQHandler - .thumb_set USART2_IRQHandler,Default_Handler - - .weak EXTI15_10_IRQHandler - .thumb_set EXTI15_10_IRQHandler,Default_Handler - - .weak RTC_Alarm_IRQHandler - .thumb_set RTC_Alarm_IRQHandler,Default_Handler - - .weak OTG_FS_WKUP_IRQHandler - .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler - - .weak DMA1_Stream7_IRQHandler - .thumb_set DMA1_Stream7_IRQHandler,Default_Handler - - .weak SDIO_IRQHandler - .thumb_set SDIO_IRQHandler,Default_Handler - - .weak TIM5_IRQHandler - .thumb_set TIM5_IRQHandler,Default_Handler - - .weak SPI3_IRQHandler - .thumb_set SPI3_IRQHandler,Default_Handler - - .weak DMA2_Stream0_IRQHandler - .thumb_set DMA2_Stream0_IRQHandler,Default_Handler - - .weak DMA2_Stream1_IRQHandler - .thumb_set DMA2_Stream1_IRQHandler,Default_Handler - - .weak DMA2_Stream2_IRQHandler - .thumb_set DMA2_Stream2_IRQHandler,Default_Handler - - .weak DMA2_Stream3_IRQHandler - .thumb_set DMA2_Stream3_IRQHandler,Default_Handler - - .weak DMA2_Stream4_IRQHandler - .thumb_set DMA2_Stream4_IRQHandler,Default_Handler - - .weak OTG_FS_IRQHandler - .thumb_set OTG_FS_IRQHandler,Default_Handler - - .weak DMA2_Stream5_IRQHandler - .thumb_set DMA2_Stream5_IRQHandler,Default_Handler - - .weak DMA2_Stream6_IRQHandler - .thumb_set DMA2_Stream6_IRQHandler,Default_Handler - - .weak DMA2_Stream7_IRQHandler - .thumb_set DMA2_Stream7_IRQHandler,Default_Handler - - .weak USART6_IRQHandler - .thumb_set USART6_IRQHandler,Default_Handler - - .weak I2C3_EV_IRQHandler - .thumb_set I2C3_EV_IRQHandler,Default_Handler - - .weak I2C3_ER_IRQHandler - .thumb_set I2C3_ER_IRQHandler,Default_Handler - - .weak FPU_IRQHandler - .thumb_set FPU_IRQHandler,Default_Handler - - .weak SPI4_IRQHandler - .thumb_set SPI4_IRQHandler,Default_Handler - - .weak SPI5_IRQHandler - .thumb_set SPI5_IRQHandler,Default_Handler - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ - diff --git a/TARGET_ARES/TOOLCHAIN_GCC_ARM/stm32f401xc.ld b/TARGET_ARES/TOOLCHAIN_GCC_ARM/stm32f401xc.ld deleted file mode 100644 index 7acf3c6..0000000 --- a/TARGET_ARES/TOOLCHAIN_GCC_ARM/stm32f401xc.ld +++ /dev/null @@ -1,217 +0,0 @@ -/* Linker script to configure memory regions. */ -/* - * SPDX-License-Identifier: BSD-3-Clause - ****************************************************************************** - * @attention - * - * Copyright (c) 2016-2020 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 - * - ****************************************************************************** -*/ - -#include "../cmsis_nvic.h" - - -#if !defined(MBED_APP_START) - #define MBED_APP_START MBED_ROM_START -#endif - -#if !defined(MBED_APP_SIZE) - #define MBED_APP_SIZE MBED_ROM_SIZE -#endif - -#if !defined(MBED_CONF_TARGET_BOOT_STACK_SIZE) - /* This value is normally defined by the tools - to 0x1000 for bare metal and 0x400 for RTOS */ - #define MBED_CONF_TARGET_BOOT_STACK_SIZE 0x400 -#endif - -/* Round up VECTORS_SIZE to 8 bytes */ -#define VECTORS_SIZE (((NVIC_NUM_VECTORS * 4) + 7) & 0xFFFFFFF8) - -M_CRASH_DATA_RAM_SIZE = 0x100; - -MEMORY -{ - FLASH (rx) : ORIGIN = MBED_APP_START, LENGTH = MBED_APP_SIZE - RAM (rwx) : ORIGIN = MBED_RAM_START + VECTORS_SIZE, LENGTH = MBED_RAM_SIZE - VECTORS_SIZE -} - -/* Linker script to place sections and symbol values. Should be used together - * with other linker script that defines memory regions FLASH and RAM. - * It references following symbols, which must be defined in code: - * Reset_Handler : Entry of reset handler - * - * It defines following symbols, which code can use without definition: - * __exidx_start - * __exidx_end - * __etext - * __data_start__ - * __preinit_array_start - * __preinit_array_end - * __init_array_start - * __init_array_end - * __fini_array_start - * __fini_array_end - * __data_end__ - * __bss_start__ - * __bss_end__ - * __end__ - * end - * __HeapLimit - * __StackLimit - * __StackTop - * __stack - * _estack - */ -ENTRY(Reset_Handler) - -SECTIONS -{ - .text : - { - KEEP(*(.isr_vector)) - *(.text*) - - KEEP(*(.init)) - KEEP(*(.fini)) - - /* .ctors */ - *crtbegin.o(.ctors) - *crtbegin?.o(.ctors) - *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors) - *(SORT(.ctors.*)) - *(.ctors) - - /* .dtors */ - *crtbegin.o(.dtors) - *crtbegin?.o(.dtors) - *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors) - *(SORT(.dtors.*)) - *(.dtors) - - *(.rodata*) - - KEEP(*(.eh_frame*)) - } > FLASH - - .ARM.extab : - { - *(.ARM.extab* .gnu.linkonce.armextab.*) - } > FLASH - - __exidx_start = .; - .ARM.exidx : - { - *(.ARM.exidx* .gnu.linkonce.armexidx.*) - } > FLASH - __exidx_end = .; - - __etext = .; - _sidata = .; - - .crash_data_ram : - { - . = ALIGN(8); - __CRASH_DATA_RAM__ = .; - __CRASH_DATA_RAM_START__ = .; /* Create a global symbol at data start */ - KEEP(*(.keep.crash_data_ram)) - *(.m_crash_data_ram) /* This is a user defined section */ - . += M_CRASH_DATA_RAM_SIZE; - . = ALIGN(8); - __CRASH_DATA_RAM_END__ = .; /* Define a global symbol at data end */ - } > RAM - - .data : AT (__etext) - { - __data_start__ = .; - _sdata = .; - *(vtable) - *(.data*) - - . = ALIGN(8); - /* preinit data */ - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP(*(.preinit_array)) - PROVIDE_HIDDEN (__preinit_array_end = .); - - . = ALIGN(8); - /* init data */ - PROVIDE_HIDDEN (__init_array_start = .); - KEEP(*(SORT(.init_array.*))) - KEEP(*(.init_array)) - PROVIDE_HIDDEN (__init_array_end = .); - - . = ALIGN(8); - /* finit data */ - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP(*(SORT(.fini_array.*))) - KEEP(*(.fini_array)) - PROVIDE_HIDDEN (__fini_array_end = .); - - KEEP(*(.jcr*)) - . = ALIGN(8); - /* All data end */ - __data_end__ = .; - _edata = .; - - } > RAM - - /* Uninitialized data section - * This region is not initialized by the C/C++ library and can be used to - * store state across soft reboots. */ - .uninitialized (NOLOAD): - { - . = ALIGN(32); - __uninitialized_start = .; - *(.uninitialized) - KEEP(*(.keep.uninitialized)) - . = ALIGN(32); - __uninitialized_end = .; - } > RAM - - .bss : - { - . = ALIGN(8); - __bss_start__ = .; - _sbss = .; - *(.bss*) - *(COMMON) - . = ALIGN(8); - __bss_end__ = .; - _ebss = .; - } > RAM - - .heap (COPY): - { - __end__ = .; - PROVIDE(end = .); - *(.heap*) - . = ORIGIN(RAM) + LENGTH(RAM) - MBED_CONF_TARGET_BOOT_STACK_SIZE; - __HeapLimit = .; - } > RAM - - /* .stack_dummy section doesn't contains any symbols. It is only - * used for linker to calculate size of stack sections, and assign - * values to stack symbols later */ - .stack_dummy (COPY): - { - *(.stack*) - } > RAM - - /* Set stack top to end of RAM, and stack limit move down by - * size of stack_dummy section */ - __StackTop = ORIGIN(RAM) + LENGTH(RAM); - _estack = __StackTop; - __StackLimit = __StackTop - MBED_CONF_TARGET_BOOT_STACK_SIZE; - PROVIDE(__stack = __StackTop); - - /* Check if data + heap + stack exceeds RAM limit */ - ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed with stack") -} diff --git a/TARGET_ARES/cmsis_nvic.h b/TARGET_ARES/cmsis_nvic.h deleted file mode 100644 index 25e858f..0000000 --- a/TARGET_ARES/cmsis_nvic.h +++ /dev/null @@ -1,39 +0,0 @@ -/* mbed Microcontroller Library - * SPDX-License-Identifier: BSD-3-Clause - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2016-2020 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 - * - ****************************************************************************** -*/ - -#ifndef MBED_CMSIS_NVIC_H -#define MBED_CMSIS_NVIC_H - -#if !defined(MBED_ROM_START) -#define MBED_ROM_START 0x8000000 -#endif - -#if !defined(MBED_ROM_SIZE) -#define MBED_ROM_SIZE 0x40000 // 256 KB -#endif - -#if !defined(MBED_RAM_START) -#define MBED_RAM_START 0x20000000 -#endif - -#if !defined(MBED_RAM_SIZE) -#define MBED_RAM_SIZE 0x10000 // 64 KB -#endif - -#define NVIC_NUM_VECTORS 102 -#define NVIC_RAM_VECTOR_ADDRESS MBED_RAM_START - -#endif diff --git a/TARGET_ARES/flash_data.h b/TARGET_ARES/flash_data.h deleted file mode 100644 index f64bfd2..0000000 --- a/TARGET_ARES/flash_data.h +++ /dev/null @@ -1,53 +0,0 @@ -/* mbed Microcontroller Library - ******************************************************************************* - * Copyright (c) 2016, STMicroelectronics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************* - */ -#ifndef MBED_FLASH_DATA_H -#define MBED_FLASH_DATA_H - -#include "device.h" -#include - -#if DEVICE_FLASH - -/* Exported types ------------------------------------------------------------*/ -/* Exported constants --------------------------------------------------------*/ -/* Exported macro ------------------------------------------------------------*/ -/* FLASH SIZE */ -#define FLASH_SIZE (uint32_t) 0x40000 - -/* Base address of the Flash sectors Bank 1 */ -#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) /* Base @ of Sector 0, 16 Kbytes */ -#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08004000) /* Base @ of Sector 1, 16 Kbytes */ -#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08008000) /* Base @ of Sector 2, 16 Kbytes */ -#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x0800C000) /* Base @ of Sector 3, 16 Kbytes */ -#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08010000) /* Base @ of Sector 4, 64 Kbytes */ -#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x08020000) /* Base @ of Sector 5, 128 Kbytes */ - -#endif -#endif diff --git a/TARGET_ARES/system_clock.c b/TARGET_ARES/system_clock.c deleted file mode 100644 index 99aed16..0000000 --- a/TARGET_ARES/system_clock.c +++ /dev/null @@ -1,201 +0,0 @@ -/* mbed Microcontroller Library - * SPDX-License-Identifier: BSD-3-Clause - ****************************************************************************** - * - * Copyright (c) 2015-2021 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 - * - ****************************************************************************** - */ - -/** - * This file configures the system clock as follows: - *---------------------------------------------------------------------- - * System clock source | 1- USE_PLL_HSE_XTAL | 2- USE_PLL_HSI - * (external 25 MHz xtal) | (internal 16 MHz) - *---------------------------------------------------------------------- - * USB enabled | NO | YES | NO | YES - *---------------------------------------------------------------------- - * SYSCLK(MHz) | 100 | 96 | 100 | 96 - * AHBCLK (MHz) | 100 | 96 | 100 | 96 - * APB1CLK (MHz) | 50 | 48 | 50 | 48 - * APB2CLK (MHz) | 100 | 96 | 100 | 96 - *---------------------------------------------------------------------- -**/ - -#include "stm32f4xx.h" -#include "mbed_error.h" - -// clock source is selected with CLOCK_SOURCE in json config -#define USE_PLL_HSE_EXTC 0x8 // Use external clock (ST Link MCO) -#define USE_PLL_HSE_XTAL 0x4 // Use external xtal (X3 on board - not provided by default) -#define USE_PLL_HSI 0x2 // Use HSI internal clock - -#if ( ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) ) -uint8_t SetSysClock_PLL_HSE(uint8_t bypass); -#endif /* ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) */ - -#if ((CLOCK_SOURCE) & USE_PLL_HSI) -uint8_t SetSysClock_PLL_HSI(void); -#endif /* ((CLOCK_SOURCE) & USE_PLL_HSI) */ - - -/** -* @brief Configures the System clock source, PLL Multiplier and Divider factors, -* AHB/APBx prescalers and Flash settings -* @note This function should be called only once the RCC clock configuration -* is reset to the default reset state (done in SystemInit() function). - * @param None - * @retval None - */ - -MBED_WEAK void SetSysClock(void) -{ -#if ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) - /* 1- Try to start with HSE and external clock */ - if (SetSysClock_PLL_HSE(1) == 0) -#endif - { -#if ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) - /* 2- If fail try to start with HSE and external xtal */ - if (SetSysClock_PLL_HSE(0) == 0) -#endif - { -#if ((CLOCK_SOURCE) & USE_PLL_HSI) - /* 3- If fail start with HSI clock */ - if (SetSysClock_PLL_HSI() == 0) -#endif - { - { - error("SetSysClock failed\n"); - } - } - } - } - - /* Output clock on MCO2 pin(PC9) for debugging purpose */ - //HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_SYSCLK, RCC_MCODIV_4); -} - -#if ( ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) ) -/******************************************************************************/ -/* PLL (clocked by HSE) used as System clock source */ -/******************************************************************************/ -MBED_WEAK uint8_t SetSysClock_PLL_HSE(uint8_t bypass) -{ - RCC_OscInitTypeDef RCC_OscInitStruct; - RCC_ClkInitTypeDef RCC_ClkInitStruct; - - /* The voltage scaling allows optimizing the power consumption when the device is - clocked below the maximum system frequency, to update the voltage scaling value - regarding system frequency refer to product datasheet. */ - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); - - /* Get the Clocks configuration according to the internal RCC registers */ - HAL_RCC_GetOscConfig(&RCC_OscInitStruct); - - /* PLL could be already configured by bootlader */ - if (RCC_OscInitStruct.PLL.PLLState != RCC_PLL_ON) { - - // Enable HSE oscillator and activate PLL with HSE as source - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; - if (bypass == 0) { - RCC_OscInitStruct.HSEState = RCC_HSE_ON; // External 25 MHz xtal on OSC_IN/OSC_OUT - } else { - RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; // External 25 MHz clock on OSC_IN - } - - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; - #if (CLOCK_SOURCE_USB) - RCC_OscInitStruct.PLL.PLLM = 25; // VCO input clock = 1 MHz (25 MHz / 25) - RCC_OscInitStruct.PLL.PLLN = 192; // VCO output clock = 192 MHz (1 MHz * 192) - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;// PLLCLK = 96 MHz (192 / 2) - RCC_OscInitStruct.PLL.PLLQ = 4; // USB clock = 48 MHz (192 / 4, CLOCK_SOURCE_USB = 1) - #else - RCC_OscInitStruct.PLL.PLLM = 12; // VCO input clock = 2.0833 MHz (25 MHz / 12) - RCC_OscInitStruct.PLL.PLLN = 96; // VCO output clock = 200 MHz (2.0833 MHz * 96) - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;// PLLCLK = 100 MHz (200 / 2) - RCC_OscInitStruct.PLL.PLLQ = 4; // USB clock = 50 MHz (200 / 4 -> USB not available) - #endif - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - return 0; // FAIL - } - } - - // Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // 100/96 MHz - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // 100/96 MHz - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; // 50/48 MHz - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; // 100/96 MHz - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { - return 0; // FAIL - } - - /* Output clock on MCO1 pin(PA8) for debugging purpose */ - //if (bypass == 0) - // HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSE, RCC_MCODIV_2); // 4 MHz with xtal - //else - // HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSE, RCC_MCODIV_1); // 8 MHz with external clock - - return 1; // OK -} -#endif /* ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) */ - -#if ((CLOCK_SOURCE) & USE_PLL_HSI) -/******************************************************************************/ -/* PLL (clocked by HSI) used as System clock source */ -/******************************************************************************/ -uint8_t SetSysClock_PLL_HSI(void) -{ - RCC_OscInitTypeDef RCC_OscInitStruct; - RCC_ClkInitTypeDef RCC_ClkInitStruct; - - /* The voltage scaling allows optimizing the power consumption when the device is - clocked below the maximum system frequency, to update the voltage scaling value - regarding system frequency refer to product datasheet. */ - __HAL_RCC_PWR_CLK_ENABLE(); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); - - // Enable HSI oscillator and activate PLL with HSI as source - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_HSE; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.HSEState = RCC_HSE_OFF; - RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; - RCC_OscInitStruct.PLL.PLLM = 8; // VCO input clock = 2 MHz (16 MHz / 8) -#if (DEVICE_USBDEVICE) - RCC_OscInitStruct.PLL.PLLN = 192; // VCO output clock = 384 MHz (2 MHz * 192) -#else /* DEVICE_USBDEVICE */ - RCC_OscInitStruct.PLL.PLLN = 200; // VCO output clock = 400 MHz (2 MHz * 200) -#endif /* DEVICE_USBDEVICE */ - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; // PLLCLK = 100 MHz or 96 MHz (depending on DEVICE_USBDEVICE) - RCC_OscInitStruct.PLL.PLLQ = 8; // USB clock = 48 MHz (DEVICE_USBDEVICE=1) - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - return 0; // FAIL - } - - /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */ - RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { - return 0; // FAIL - } - - /* Output clock on MCO1 pin(PA8) for debugging purpose */ - //HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1); // 16 MHz - - return 1; // OK -} -#endif /* ((CLOCK_SOURCE) & USE_PLL_HSI) */ diff --git a/main.cpp b/main.cpp index 159d78d..913e2b6 100644 --- a/main.cpp +++ b/main.cpp @@ -1,48 +1,26 @@ #include "mbed.h" -#include -#include "I2CSerial.h" -#include "Motor.h" -#include "PID.h" -#include "Distributor.h" +int main() { + DigitalOut led(PC_13); + DigitalOut led_ext(PB_0); + DigitalOut motor1(PA_0); + DigitalOut motor2(PA_1); + + bool m1 = true; + bool m2 = false; + + while (1) { + // ThisThread::sleep_for(500ms); + led = !led; + led_ext = !led_ext; + motor1.write(0); + motor2.write(0); + ThisThread::sleep_for(50ms); + motor1.write(m1); + motor2.write(m2); + m1 = !m1; + m2 = !m2; + ThisThread::sleep_for(2s); - -int leftExtension = 0; -int rightExtension = 0; - - -// main() runs in its own thread in the OS -int main() -{ - - PID pid(0.017, 0, 1); // No idea if these values work - // Motor motor(PA_8, PA_10, PB_2, PB_1, PB_15, PB_14, pid); // test bench - // Motor motor1(PB_3, PB_5, PA_11, PA_12, PA_10, PA_9, pid); // these are the mcpcb - // Motor motor2(PA_6, PA_5, PB_14, PB_15, PB_13, PA_8, pid); - - - // on stm with breakout: PB_7 PB_8 - I2CSerial ser(PB_7, PB_8, 0x32, true); - - - Distributor distributor(&ser); - - - // DEBUG // - // printf("start :)\n"); - // DigitalOut led1(PC_14); - - while (true) { - ThisThread::sleep_for(10ms); - // get distributed pair - std::pair spoolExtensions = distributor.getMotorOutputs(); - // if the first float is a NAN, keep each extension value the same - leftExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.first : leftExtension; - rightExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.second : rightExtension; - - // go to position - motor1.lineTo(leftExtension, 10); - motor2.lineTo(rightExtension, 10); } -} - +} \ No newline at end of file From 6b48ba0cab240f7cf8a968cef2599d9c521e47e6 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Sat, 10 May 2025 12:14:14 -0700 Subject: [PATCH 03/47] Motor PWM control working --- Motor.cpp | 169 ------------------------------------------------------ Motor.h | 71 ----------------------- main.cpp | 21 +++---- 3 files changed, 11 insertions(+), 250 deletions(-) delete mode 100644 Motor.cpp delete mode 100644 Motor.h diff --git a/Motor.cpp b/Motor.cpp deleted file mode 100644 index 7f1feb7..0000000 --- a/Motor.cpp +++ /dev/null @@ -1,169 +0,0 @@ -#include "mbed.h" -#include -#include "Motor.h" -#include -#include "PID.h" - - -void Motor::updateGlobals() { - position = encoderCounter; - angle = (encoderCounter * 360.0) / totalCounts; - rotations = angle / 360.0; - -} - -void Motor::aRiseCallback() { - aUp = true; - - if (!bUp) { - encoderCounter++; - } else { - encoderCounter--; - } -} - - -void Motor::bRiseCallback() { - bUp = true; - - if (aUp) { - encoderCounter++; - } else { - encoderCounter--; - } -} - -void Motor::aFallCallback() { - aUp = false; - - if (bUp) { - encoderCounter++; - } else { - encoderCounter--; - } -} - -void Motor::bFallCallback() { - bUp = false; - - if (!aUp) { - encoderCounter++; - } else { - encoderCounter--; - } -} - - -Motor::Motor(PinName PIN_A, PinName PIN_B, PinName MOTOR_1, PinName MOTOR_2, PinName MOTOR_3, - PinName MOTOR_4, const PID& pidObject) : encoderA(PIN_A), encoderB(PIN_B), motorPin1(MOTOR_1), - motorPin3(MOTOR_3), motorPin2(MOTOR_2), motorPin4(MOTOR_4), motorPID(pidObject) { - - // Init pins and set pin modes for encoders - encoderA.mode(PullDown); - encoderB.mode(PullDown); - - if (encoderA.read() == 1) { - - aUp = true; - } - if (encoderB.read() == 1) { - bUp = true; - } - - powerPositive = true; - - // Attach the address of the encoderCallback function to the each edge - encoderA.rise([this]() {aRiseCallback();}); - encoderA.fall([this]() {aFallCallback();}); - encoderB.rise([this]() {bRiseCallback();}); - encoderB.fall([this]() {bFallCallback();}); - -} - -void Motor::motorPower(float power) { - printf("motor power\n"); - if (power > 0) { - if (!powerPositive) { - motorPin1.write(0); - motorPin2.write(0); - motorPin3.write(0); - motorPin4.write(0); - - ThisThread::sleep_for(2ms); - } - powerPositive = true; - //set forward pins proportional to powe - motorPin1.write(1); - motorPin4.write(power); - } else if (power < 0) { - if (powerPositive) { - motorPin1.write(0); - motorPin2.write(0); - motorPin3.write(0); - motorPin4.write(0); - - ThisThread::sleep_for(2ms); - } - powerPositive = false; - //set backwards pins prop to |power| - this->motorPin3.write(1); - this->motorPin2.write(-power); - - } else { - //set all pins to off - motorPin1.write(0); - motorPin2.write(0); - motorPin3.write(0); - motorPin4.write(0); - } -} - -// Takes an integer for degrees to spin from current position -void Motor::spinDegrees(int degrees) { - printf("spin degrees\n"); - int initial = getDegrees(); - if (degrees > 0) { - printf("positive\n"); - motorPower(1); - printf("on\n"); - while (angle < (initial + degrees)) { - printf("a: %d\n", (int)encoderA.read()); - printf("b: %d\n", (int)encoderB.read()); - ThisThread::sleep_for(10ms); - updateGlobals(); - } - printf("off\n"); - motorPower(0); - } else if (degrees < 0) { - printf("negative motor power\n"); - motorPower(-1); - ThisThread::sleep_for(100ms); - while (angle > (initial + degrees)) { - ThisThread::sleep_for(10ms); - updateGlobals(); - } - printf("off"); - motorPower(0); - } -} - - - -int Motor::getDegrees() { - updateGlobals(); - return (int)angle; -} - -// Gets the current displacement of the line as opposed to degrees -// Returns inches because spool diameter is in inches -long Motor::getDisplacement() { - updateGlobals(); - return angle * spoolDiameter * PI / 360; -} - -void Motor::lineTo(float retraction, int delay) { - float inches = MAX_DEFLECTION * retraction; - float displacement = getDisplacement(); - float power = motorPID.compute(displacement, inches, delay); - motorPower(power); -} \ No newline at end of file diff --git a/Motor.h b/Motor.h deleted file mode 100644 index 9bcf7e7..0000000 --- a/Motor.h +++ /dev/null @@ -1,71 +0,0 @@ -// Class which controls DC motor with ethan's mosftet motor board -#ifndef _MOTOR_H_ -#define _MOTOR_H_ - - -#include "mbed.h" -#include "PID.h" - -class Motor { - private: - // Defining constants from arduino - const int LOW = 0; - const int HIGH = 1; - - DigitalOut motorPin1; - DigitalOut motorPin3; - PwmOut motorPin2; - PwmOut motorPin4; - InterruptIn encoderA; - InterruptIn encoderB; - - // Encoder state boolean - bool aUp = false; - bool bUp = false; - - const float PI = 3.1415926535; - - volatile int encoderCounter = 0; // Counter for the encoder ticks - const int countsPerRev = 64; - const float gearRatio = 150; // Might need to be updated for different motors - const float spoolDiameter = 0.5; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 36; // INCHES - - const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft - long position; - float angle; - float rotations; - bool powerPositive; - - PID motorPID; - - - void aRiseCallback(); - void bRiseCallback(); - void aFallCallback(); - void bFallCallback(); - - - public: - - void updateGlobals(); - - - - - Motor(PinName PIN_A, PinName PIN_B, PinName MOTOR_1, PinName MOTOR_2, PinName MOTOR_3, - PinName MOTOR_4, const PID& pidObject); - - void motorPower(float power); - - void spinDegrees(int degrees); - - int getDegrees(); - - long getDisplacement(); - - void lineTo(float retraction, int delay); - -}; - -#endif //_MOTOR_H_ \ No newline at end of file diff --git a/main.cpp b/main.cpp index 913e2b6..6ce6eb9 100644 --- a/main.cpp +++ b/main.cpp @@ -3,24 +3,25 @@ int main() { DigitalOut led(PC_13); DigitalOut led_ext(PB_0); - DigitalOut motor1(PA_0); - DigitalOut motor2(PA_1); + PwmOut motorPower(PA_1); + DigitalOut motorIn1(PA_6); + DigitalOut motorIn2(PA_5); + + motorIn1.write(1); + motorIn2.write(0); + motorPower.write(0.5); bool m1 = true; bool m2 = false; + float power = 0; while (1) { // ThisThread::sleep_for(500ms); led = !led; led_ext = !led_ext; - motor1.write(0); - motor2.write(0); - ThisThread::sleep_for(50ms); - motor1.write(m1); - motor2.write(m2); - m1 = !m1; - m2 = !m2; ThisThread::sleep_for(2s); - + power += 0.25; + if (power > 1) {power -= 1;} + motorPower.write(power); } } \ No newline at end of file From 988b65b097c3f094a5e9397f4d8b045b921ff41e Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Sun, 11 May 2025 16:40:55 -0700 Subject: [PATCH 04/47] New motor class wrote MotorCOTS cpp and headder files, compiled them --- MotorCOTS.cpp | 106 ++++++++++++++++++++++++++++++++++++++++++++++++++ MotorCOTS.h | 54 +++++++++++++++++++++++++ 2 files changed, 160 insertions(+) create mode 100644 MotorCOTS.cpp create mode 100644 MotorCOTS.h diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp new file mode 100644 index 0000000..f2dbc23 --- /dev/null +++ b/MotorCOTS.cpp @@ -0,0 +1,106 @@ +#include "mbed.h" +#include "Motor.h" + +void Motor::updateGlobals() { + position = encoderCounter; + angle = (encoderCounter * 360.0) / totalCounts; + rotations = angle / 360.0; + +} + +void Motor::aRiseCallback() { + aUp = true; + + if (!bUp) { + encoderCounter++; + } else { + encoderCounter--; + } +} + + +void Motor::bRiseCallback() { + bUp = true; + + if (aUp) { + encoderCounter++; + } else { + encoderCounter--; + } +} + +void Motor::aFallCallback() { + aUp = false; + + if (bUp) { + encoderCounter++; + } else { + encoderCounter--; + } +} + +void Motor::bFallCallback() { + bUp = false; + + if (!aUp) { + encoderCounter++; + } else { + encoderCounter--; + } +} + +Motor::Motor(PinName directionOne, PinNAme directionTwo, PinName powerThrottle, PinName encoderA, PinName encoderB) : encoderA(PIN_A), encoderB(PIN_B), + directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle) { + + // Init pins and set pin modes for encoders + encoderA.mode(PullDown); + encoderB.mode(PullDown); + + if (encoderA.read() == 1) { + + aUp = true; + } + if (encoderB.read() == 1) { + bUp = true; + } + + powerPositive = true; + + // Attach the address of the encoderCallback function to the each edge + encoderA.rise([this]() {aRiseCallback();}); + encoderA.fall([this]() {aFallCallback();}); + encoderB.rise([this]() {bRiseCallback();}); + encoderB.fall([this]() {bFallCallback();}); + +} + +void Motor::direction(int direction) { + if (direction == 1) { + ThisThread::sleep_for(2ms); + directionTwo.write(0); + directionOne.write(1); + powerPositive = true; + } else if (direction == -1) { + ThisThread::sleep_for(2ms); + directionOne.write(0); + directionTwo.write(1); + powerPositive = false; + } +} + +void Motor::motorPower(float power) { + if (power > 0) { + direction(1); + powerThrottle.write(power); + } else if (power < 0) { + direction(-1); + powerThrottle.write(-power); + } else { + powerThrottle.write(0); + } +} + +int Motor::getDegrees() { + updateGlobals(); + return (int)angle; +} \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h new file mode 100644 index 0000000..0527bc6 --- /dev/null +++ b/MotorCOTS.h @@ -0,0 +1,54 @@ +#ifndef _MOTOR_H_ +#define _MOTOR_H_ + + +#include "mbed.h" +#include "PID.h" + +class Motor { + private: + DigitalOut directionOne; + DigitalOut directionTwo; + PwmOut powerThrottle; + InterruptIn encoderA; + InterruptIn encoderB; + + // Encoder state boolean + bool aUp = false; + bool bUp = false; + + const float PI = 3.1415926535; + + volatile int encoderCounter = 0; // Counter for the encoder ticks + const int countsPerRev = 64; + const float gearRatio = 150; // Might need to be updated for different motors + const float spoolDiameter = 0.5; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const + const float MAX_DEFLECTION = 36; // INCHES + + const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft + long position; + float angle; + float rotations; + bool powerPositive; + + void aRiseCallback(); + void bRiseCallback(); + void aFallCallback(); + void bFallCallback(); + + public: + + void updateGlobals(); + + + + + Motor(PinName directionOne, PinNAme directionTwo, PinName powerThrottle, PinName encoderA, PinName encoderB); + + void direction(int direction); + void motorPower(float power); + + int getDegrees(); +}; + +#endif //_MOTOR_H_ \ No newline at end of file From 873c8009f36543ecec55ba404c026149ae6f487d Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Wed, 14 May 2025 18:25:50 -0700 Subject: [PATCH 05/47] MotorCOTS fixes, implementation in main --- MotorCOTS.cpp | 20 ++++++++++---------- MotorCOTS.h | 10 +++++----- main.cpp | 7 ++++--- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index f2dbc23..54c4762 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -1,14 +1,14 @@ #include "mbed.h" -#include "Motor.h" +#include "MotorCOTS.h" -void Motor::updateGlobals() { +void MotorCOTS::updateGlobals() { position = encoderCounter; angle = (encoderCounter * 360.0) / totalCounts; rotations = angle / 360.0; } -void Motor::aRiseCallback() { +void MotorCOTS::aRiseCallback() { aUp = true; if (!bUp) { @@ -19,7 +19,7 @@ void Motor::aRiseCallback() { } -void Motor::bRiseCallback() { +void MotorCOTS::bRiseCallback() { bUp = true; if (aUp) { @@ -29,7 +29,7 @@ void Motor::bRiseCallback() { } } -void Motor::aFallCallback() { +void MotorCOTS::aFallCallback() { aUp = false; if (bUp) { @@ -39,7 +39,7 @@ void Motor::aFallCallback() { } } -void Motor::bFallCallback() { +void MotorCOTS::bFallCallback() { bUp = false; if (!aUp) { @@ -49,7 +49,7 @@ void Motor::bFallCallback() { } } -Motor::Motor(PinName directionOne, PinNAme directionTwo, PinName powerThrottle, PinName encoderA, PinName encoderB) : encoderA(PIN_A), encoderB(PIN_B), +MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB) : encoderA(PINA), encoderB(PINB), directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle) { // Init pins and set pin modes for encoders @@ -74,7 +74,7 @@ Motor::Motor(PinName directionOne, PinNAme directionTwo, PinName powerThrottle, } -void Motor::direction(int direction) { +void MotorCOTS::direction(int direction) { if (direction == 1) { ThisThread::sleep_for(2ms); directionTwo.write(0); @@ -88,7 +88,7 @@ void Motor::direction(int direction) { } } -void Motor::motorPower(float power) { +void MotorCOTS::motorPower(float power) { if (power > 0) { direction(1); powerThrottle.write(power); @@ -100,7 +100,7 @@ void Motor::motorPower(float power) { } } -int Motor::getDegrees() { +int MotorCOTS::getDegrees() { updateGlobals(); return (int)angle; } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index 0527bc6..cd6f3ff 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -1,11 +1,11 @@ -#ifndef _MOTOR_H_ -#define _MOTOR_H_ +#ifndef _MOTORCOTS_H_ +#define _MOTORCOTS_H_ #include "mbed.h" #include "PID.h" -class Motor { +class MotorCOTS { private: DigitalOut directionOne; DigitalOut directionTwo; @@ -43,7 +43,7 @@ class Motor { - Motor(PinName directionOne, PinNAme directionTwo, PinName powerThrottle, PinName encoderA, PinName encoderB); + MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB); void direction(int direction); void motorPower(float power); @@ -51,4 +51,4 @@ class Motor { int getDegrees(); }; -#endif //_MOTOR_H_ \ No newline at end of file +#endif //_MOTORCOTS_H_ \ No newline at end of file diff --git a/main.cpp b/main.cpp index 6ce6eb9..77e1ec5 100644 --- a/main.cpp +++ b/main.cpp @@ -1,11 +1,12 @@ #include "mbed.h" +#include "MotorCOTS.h" int main() { DigitalOut led(PC_13); DigitalOut led_ext(PB_0); - PwmOut motorPower(PA_1); - DigitalOut motorIn1(PA_6); - DigitalOut motorIn2(PA_5); + + // direction 1, direction 2, throttle, encoder a, b + MotorCOTS motor(PB_8, PB_9, PA_1, PA_8, PA_9); motorIn1.write(1); motorIn2.write(0); From f2353d32570ffc4972a9e2435ba236e135992e73 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Wed, 14 May 2025 18:54:54 -0700 Subject: [PATCH 06/47] MotorCOTS motorPower working --- main.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/main.cpp b/main.cpp index 77e1ec5..562b114 100644 --- a/main.cpp +++ b/main.cpp @@ -8,10 +8,6 @@ int main() { // direction 1, direction 2, throttle, encoder a, b MotorCOTS motor(PB_8, PB_9, PA_1, PA_8, PA_9); - motorIn1.write(1); - motorIn2.write(0); - motorPower.write(0.5); - bool m1 = true; bool m2 = false; @@ -20,9 +16,9 @@ int main() { // ThisThread::sleep_for(500ms); led = !led; led_ext = !led_ext; - ThisThread::sleep_for(2s); + ThisThread::sleep_for(5s); power += 0.25; if (power > 1) {power -= 1;} - motorPower.write(power); + motor.motorPower(power); } } \ No newline at end of file From d8b4dae40b461f045b6d55ede33d88ae4b26d620 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Wed, 14 May 2025 23:44:04 -0700 Subject: [PATCH 07/47] motor PID control works --- main.cpp | 88 ++++++++++++++++++++++++++++++++++++++++----------- mbed_app.json | 10 +++++- 2 files changed, 79 insertions(+), 19 deletions(-) diff --git a/main.cpp b/main.cpp index 562b114..68e8db4 100644 --- a/main.cpp +++ b/main.cpp @@ -1,24 +1,76 @@ #include "mbed.h" #include "MotorCOTS.h" +#include "EUSBSerial.h" +#include "PID.h" + int main() { - DigitalOut led(PC_13); - DigitalOut led_ext(PB_0); - - // direction 1, direction 2, throttle, encoder a, b - MotorCOTS motor(PB_8, PB_9, PA_1, PA_8, PA_9); - - bool m1 = true; - bool m2 = false; - - float power = 0; - while (1) { - // ThisThread::sleep_for(500ms); - led = !led; - led_ext = !led_ext; - ThisThread::sleep_for(5s); - power += 0.25; - if (power > 1) {power -= 1;} - motor.motorPower(power); + // DigitalOut led(PC_13); + DigitalOut led_ext(PB_8); + PID pid(4, 0.1, 2); + EUSBSerial pc; + ThisThread::sleep_for(1s); + pc.printf("\nSerial Port Connected!\n"); + + // direction 1, direction 2, throttle, encoder a, encoder b + + // // ollie motor + // MotorCOTS motor(PB_8, PB_9, PA_1, PA_8, PA_9); + + // jimmy motor: + MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); + + float dt = 0.1; + Timer t; + t.start(); + + float degrees = 0; + float current_angle = 0; + float target_angle = 2000; // degrees + pc.printf("Target Angle: %f\n\n", target_angle); + ThisThread::sleep_for(3s); + + // float power = 0; + while (true) { + ThisThread::sleep_for(10ms); + current_angle = motor.getDegrees(); + float power = pid.compute(current_angle, target_angle, dt); + + if (power < 0.1 && power > -0.1) { // saturation/deadzone + motor.motorPower(0); + } else { + motor.motorPower(power); + } + + pc.printf("Current Angle: %f\n", current_angle); + pc.printf("Motor Power: %f\n\n", power); + + // // NOT USING PWM SO JUST TURN TF OFF + // if (power < 0) { + // motor.motorPower(0); + // break; + // } + + + // motor.direction(1); + // led_ext.write(1); + // for (int i=0; i<50; i++) { // 50 x 100ms = 5s + // degrees = motor.getDegrees(); + // pc.printf("Direction: 1\n"); + // pc.printf("Degrees: %f\n\n", degrees); + // ThisThread::sleep_for(100ms); + // } + + // motor.direction(-1); + // led_ext.write(0); + // for (int i=0; i<50; i++) { + // degrees = motor.getDegrees(); + // pc.printf("Direction: 2\n"); + // pc.printf("Degrees: %f\n\n", degrees); + // ThisThread::sleep_for(100ms); + // } + + + } } \ No newline at end of file diff --git a/mbed_app.json b/mbed_app.json index 2286cc8..4832326 100644 --- a/mbed_app.json +++ b/mbed_app.json @@ -5,7 +5,15 @@ } }, "target_overrides": { - "ARES": { + "ARES_BOOT": { + "target.mbed_app_start": "0x08010000", + "target.mbed_rom_start": "0x08000000", + "target.mbed_rom_size" : "0x40000" + }, + "*": { + "target.printf_lib": "std" + }, + "ARES_BOOT": { "target.mbed_app_start": "0x08010000", "target.mbed_rom_start": "0x08000000", "target.mbed_rom_size" : "0x40000" From a75bdf5843222b7d8c20c1207688c630a3bf4168 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Thu, 15 May 2025 20:50:47 -0700 Subject: [PATCH 08/47] PID included, no hang --- MotorCOTS.cpp | 4 ++-- MotorCOTS.h | 4 +++- main.cpp | 13 +++++++------ 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 54c4762..5b3de72 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -49,8 +49,8 @@ void MotorCOTS::bFallCallback() { } } -MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB) : encoderA(PINA), encoderB(PINB), - directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle) { +MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid) : encoderA(PINA), encoderB(PINB), + directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle), pid(pid) { // Init pins and set pin modes for encoders encoderA.mode(PullDown); diff --git a/MotorCOTS.h b/MotorCOTS.h index cd6f3ff..6467024 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -36,6 +36,8 @@ class MotorCOTS { void aFallCallback(); void bFallCallback(); + PID* pid; + public: void updateGlobals(); @@ -43,7 +45,7 @@ class MotorCOTS { - MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB); + MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid); void direction(int direction); void motorPower(float power); diff --git a/main.cpp b/main.cpp index 68e8db4..7dfde8c 100644 --- a/main.cpp +++ b/main.cpp @@ -5,8 +5,8 @@ int main() { - // DigitalOut led(PC_13); - DigitalOut led_ext(PB_8); + DigitalOut led(PB_0); + led.write(1); PID pid(4, 0.1, 2); EUSBSerial pc; ThisThread::sleep_for(1s); @@ -15,15 +15,16 @@ int main() { // direction 1, direction 2, throttle, encoder a, encoder b // // ollie motor - // MotorCOTS motor(PB_8, PB_9, PA_1, PA_8, PA_9); - + MotorCOTS motor(PB_8, PB_9, PA_1, PA_6, PA_7, &pid); + led.write(0); // jimmy motor: - MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); + // MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); float dt = 0.1; Timer t; t.start(); + float degrees = 0; float current_angle = 0; float target_angle = 2000; // degrees @@ -39,7 +40,7 @@ int main() { if (power < 0.1 && power > -0.1) { // saturation/deadzone motor.motorPower(0); } else { - motor.motorPower(power); + motor.motorPower(-power); } pc.printf("Current Angle: %f\n", current_angle); From e9b991403b8ad6129b877427feed8cb0ca492752 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Thu, 15 May 2025 22:41:47 -0700 Subject: [PATCH 09/47] PID running inside motor and tuned --- MotorCOTS.cpp | 19 +++++++++++++++++-- MotorCOTS.h | 8 +++++++- PID.cpp | 2 +- main.cpp | 18 +++++------------- 4 files changed, 30 insertions(+), 17 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 5b3de72..b301f28 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -1,5 +1,6 @@ #include "mbed.h" #include "MotorCOTS.h" +#include "EUSBSerial.h" void MotorCOTS::updateGlobals() { position = encoderCounter; @@ -49,8 +50,9 @@ void MotorCOTS::bFallCallback() { } } -MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid) : encoderA(PINA), encoderB(PINB), - directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle), pid(pid) { + +MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid, EUSBSerial* pc) : encoderA(PINA), encoderB(PINB), + directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle), pid(pid), pc(pc) { // Init pins and set pin modes for encoders encoderA.mode(PullDown); @@ -103,4 +105,17 @@ void MotorCOTS::motorPower(float power) { int MotorCOTS::getDegrees() { updateGlobals(); return (int)angle; +} + +void MotorCOTS::toPosition(int target, int dt) { + int curr = getDegrees(); + float power = pid->compute(curr, target, dt); + if (curr-target < 10 && curr-target > -10) { + motorPower(0); + } else { + motorPower(-power); + } + pc->printf("Curr: %d\t", curr); + pc->printf("Target: %d\t", target); + pc->printf("Difference: %d\n", curr-target); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index 6467024..98d1233 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -4,6 +4,8 @@ #include "mbed.h" #include "PID.h" +#include "EUSBSerial.h" + class MotorCOTS { private: @@ -37,6 +39,7 @@ class MotorCOTS { void bFallCallback(); PID* pid; + EUSBSerial* pc; public: @@ -45,12 +48,15 @@ class MotorCOTS { - MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid); + // MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid); + MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid, EUSBSerial* pc); void direction(int direction); void motorPower(float power); int getDegrees(); + + void toPosition(int target, int dt); }; #endif //_MOTORCOTS_H_ \ No newline at end of file diff --git a/PID.cpp b/PID.cpp index beba438..80574bb 100644 --- a/PID.cpp +++ b/PID.cpp @@ -21,7 +21,7 @@ float PID::compute(float currAngle, float targetAngle, float dt) { } - float P = -Kp * error; //Simply proportional to error + float P = -Kp * error; float I = -Ki * integralError; float D = -Kd * (error - errorLast) / dt; errorLast = error; diff --git a/main.cpp b/main.cpp index 7dfde8c..6b72abd 100644 --- a/main.cpp +++ b/main.cpp @@ -7,7 +7,7 @@ int main() { DigitalOut led(PB_0); led.write(1); - PID pid(4, 0.1, 2); + PID pid(0.06, 0, 10); EUSBSerial pc; ThisThread::sleep_for(1s); pc.printf("\nSerial Port Connected!\n"); @@ -15,7 +15,7 @@ int main() { // direction 1, direction 2, throttle, encoder a, encoder b // // ollie motor - MotorCOTS motor(PB_8, PB_9, PA_1, PA_6, PA_7, &pid); + MotorCOTS motor(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); led.write(0); // jimmy motor: // MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); @@ -27,24 +27,16 @@ int main() { float degrees = 0; float current_angle = 0; - float target_angle = 2000; // degrees + float target_angle = 720; // degrees pc.printf("Target Angle: %f\n\n", target_angle); - ThisThread::sleep_for(3s); // float power = 0; while (true) { ThisThread::sleep_for(10ms); - current_angle = motor.getDegrees(); - float power = pid.compute(current_angle, target_angle, dt); - if (power < 0.1 && power > -0.1) { // saturation/deadzone - motor.motorPower(0); - } else { - motor.motorPower(-power); - } + motor.toPosition(target_angle, 8); - pc.printf("Current Angle: %f\n", current_angle); - pc.printf("Motor Power: %f\n\n", power); + pc.printf("Current Angle: %f\t", motor.getDegrees()); // // NOT USING PWM SO JUST TURN TF OFF // if (power < 0) { From e350fa3d82422c4dc49b823fa923fc130f89e37d Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Sat, 17 May 2025 00:37:38 -0700 Subject: [PATCH 10/47] To position now takes a float --- MotorCOTS.cpp | 21 ++++++++++++++------- MotorCOTS.h | 5 +++-- main.cpp | 10 +++++----- 3 files changed, 22 insertions(+), 14 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index b301f28..a00d634 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -107,15 +107,22 @@ int MotorCOTS::getDegrees() { return (int)angle; } -void MotorCOTS::toPosition(int target, int dt) { - int curr = getDegrees(); - float power = pid->compute(curr, target, dt); - if (curr-target < 10 && curr-target > -10) { +int MotorCOTS::getPosition() { + return getDegrees() / 360 * PI * spoolDiameter; +} + +void MotorCOTS::toPosition(float pullPercent, int dt) { + int currPos = getPosition(); + int targetPos = pullPercent*MAX_DEFLECTION; + + float power = pid->compute(currPos, targetPos, dt); + + if (currPos-targetPos < 1 && currPos-targetPos > -1) { motorPower(0); } else { motorPower(-power); } - pc->printf("Curr: %d\t", curr); - pc->printf("Target: %d\t", target); - pc->printf("Difference: %d\n", curr-target); + pc->printf("Curr: %d\t", currPos); + pc->printf("Target: %d\t", targetPos); + pc->printf("Difference: %d\n", currPos-targetPos); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index 98d1233..f6e16ba 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -25,7 +25,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.5; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 36; // INCHES + const float MAX_DEFLECTION = 72; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; @@ -55,8 +55,9 @@ class MotorCOTS { void motorPower(float power); int getDegrees(); + int getPosition(); - void toPosition(int target, int dt); + void toPosition(float target, int dt); }; #endif //_MOTORCOTS_H_ \ No newline at end of file diff --git a/main.cpp b/main.cpp index 6b72abd..d6dee94 100644 --- a/main.cpp +++ b/main.cpp @@ -7,7 +7,7 @@ int main() { DigitalOut led(PB_0); led.write(1); - PID pid(0.06, 0, 10); + PID pid(0.5, 0, 10); EUSBSerial pc; ThisThread::sleep_for(1s); pc.printf("\nSerial Port Connected!\n"); @@ -27,16 +27,16 @@ int main() { float degrees = 0; float current_angle = 0; - float target_angle = 720; // degrees - pc.printf("Target Angle: %f\n\n", target_angle); + float target_position = 0.5; // degrees + pc.printf("Target Angle: %f\n\n", target_position); // float power = 0; while (true) { ThisThread::sleep_for(10ms); - motor.toPosition(target_angle, 8); + motor.toPosition(target_position, 10); - pc.printf("Current Angle: %f\t", motor.getDegrees()); + pc.printf("Current Pos: %f\t", motor.getPosition()); // // NOT USING PWM SO JUST TURN TF OFF // if (power < 0) { From 7d7f63a5916cd79eab56d336c029e7f34103a70b Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Sat, 17 May 2025 17:38:19 -0700 Subject: [PATCH 11/47] Two motor funcitonality, working. See for pins --- MotorCOTS.cpp | 17 +++++++++-------- MotorCOTS.h | 2 +- main.cpp | 15 +++++++++------ 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index a00d634..2a20f98 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -107,22 +107,23 @@ int MotorCOTS::getDegrees() { return (int)angle; } -int MotorCOTS::getPosition() { - return getDegrees() / 360 * PI * spoolDiameter; +float MotorCOTS::getPosition() { + return static_cast(getDegrees()) / 360.0 * PI * spoolDiameter; } void MotorCOTS::toPosition(float pullPercent, int dt) { - int currPos = getPosition(); - int targetPos = pullPercent*MAX_DEFLECTION; + + float currPos = getPosition(); + float targetPos = pullPercent*MAX_DEFLECTION; float power = pid->compute(currPos, targetPos, dt); - if (currPos-targetPos < 1 && currPos-targetPos > -1) { + if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) { motorPower(0); } else { motorPower(-power); } - pc->printf("Curr: %d\t", currPos); - pc->printf("Target: %d\t", targetPos); - pc->printf("Difference: %d\n", currPos-targetPos); + pc->printf("Curr: %f\t", currPos); + pc->printf("Target: %f\t", targetPos); + pc->printf("Difference: %f\n", currPos-targetPos); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index f6e16ba..c7e40fd 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -55,7 +55,7 @@ class MotorCOTS { void motorPower(float power); int getDegrees(); - int getPosition(); + float getPosition(); void toPosition(float target, int dt); }; diff --git a/main.cpp b/main.cpp index d6dee94..209b080 100644 --- a/main.cpp +++ b/main.cpp @@ -7,7 +7,7 @@ int main() { DigitalOut led(PB_0); led.write(1); - PID pid(0.5, 0, 10); + PID pid(1, 0, 5); EUSBSerial pc; ThisThread::sleep_for(1s); pc.printf("\nSerial Port Connected!\n"); @@ -15,7 +15,8 @@ int main() { // direction 1, direction 2, throttle, encoder a, encoder b // // ollie motor - MotorCOTS motor(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); + MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); + MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); led.write(0); // jimmy motor: // MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); @@ -27,16 +28,18 @@ int main() { float degrees = 0; float current_angle = 0; - float target_position = 0.5; // degrees + float target_position = 0.1; // degrees pc.printf("Target Angle: %f\n\n", target_position); // float power = 0; while (true) { ThisThread::sleep_for(10ms); - motor.toPosition(target_position, 10); - - pc.printf("Current Pos: %f\t", motor.getPosition()); + motor1.toPosition(target_position, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + + motor2.toPosition(target_position, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); // // NOT USING PWM SO JUST TURN TF OFF // if (power < 0) { From 2f0455082ef85caf8ab65ceb2e90a6b9cb9431d2 Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Mon, 19 May 2025 00:59:21 -0700 Subject: [PATCH 12/47] Manual distributor functionality --- Distributor.cpp | 6 +++++- Distributor.h | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/Distributor.cpp b/Distributor.cpp index 0bf2770..0c9c758 100644 --- a/Distributor.cpp +++ b/Distributor.cpp @@ -5,6 +5,7 @@ Distributor::Distributor(I2CSerial* ser) : ser(ser) {} +Distributor::Distributor() {} std::pair Distributor::getMotorOutputs() { // Integer from -100 to 100 @@ -19,7 +20,10 @@ std::pair Distributor::getMotorOutputs() { } float ctrl = ( (float) ((int) buf) ) / 100.0; - + return getMotorOutputsManual(ctrl); +} + +std::pair Distributor::getMotorOutputsManual(float ctrl) { float leftPull = 0.5 * (-ctrl) + 0.5; float rightPull = 0.5 * ctrl + 0.5; diff --git a/Distributor.h b/Distributor.h index b49bc6f..853c676 100644 --- a/Distributor.h +++ b/Distributor.h @@ -15,8 +15,10 @@ class Distributor { public: Distributor(I2CSerial* ser); + Distributor(); std::pair getMotorOutputs(); + std::pair getMotorOutputsManual(float ctrl); }; From 6860305b084feef4b2da1395f956e674fd1ff808 Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Mon, 19 May 2025 00:59:44 -0700 Subject: [PATCH 13/47] MCP control sequence added --- main.cpp | 93 ++++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 63 insertions(+), 30 deletions(-) diff --git a/main.cpp b/main.cpp index 209b080..a205694 100644 --- a/main.cpp +++ b/main.cpp @@ -2,6 +2,7 @@ #include "MotorCOTS.h" #include "EUSBSerial.h" #include "PID.h" +#include "Distributor.h" int main() { @@ -25,48 +26,80 @@ int main() { Timer t; t.start(); + Distributor dstb; + + // not sure if we need this float degrees = 0; float current_angle = 0; float target_position = 0.1; // degrees pc.printf("Target Angle: %f\n\n", target_position); - // float power = 0; - while (true) { - ThisThread::sleep_for(10ms); - motor1.toPosition(target_position, 10); + std::pair extensions; + int seconds = 30; + + + // CONTROL SEQUENCE + + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(0, 10); pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(0, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + - motor2.toPosition(target_position, 10); + extensions = dstb.getMotorOutputsManual(1); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } - // // NOT USING PWM SO JUST TURN TF OFF - // if (power < 0) { - // motor.motorPower(0); - // break; - // } - - - // motor.direction(1); - // led_ext.write(1); - // for (int i=0; i<50; i++) { // 50 x 100ms = 5s - // degrees = motor.getDegrees(); - // pc.printf("Direction: 1\n"); - // pc.printf("Degrees: %f\n\n", degrees); - // ThisThread::sleep_for(100ms); - // } - - // motor.direction(-1); - // led_ext.write(0); - // for (int i=0; i<50; i++) { - // degrees = motor.getDegrees(); - // pc.printf("Direction: 2\n"); - // pc.printf("Degrees: %f\n\n", degrees); - // ThisThread::sleep_for(100ms); - // } - + extensions = dstb.getMotorOutputsManual(0.5); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + extensions = dstb.getMotorOutputsManual(0); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + extensions = dstb.getMotorOutputsManual(-0.5); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + + extensions = dstb.getMotorOutputsManual(-1); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + + extensions = dstb.getMotorOutputsManual(1); + for (int i = 0; i < seconds*100; i++) { + motor1.toPosition(extensions.first, 10); + pc.printf("Current Pos 1: %f\t", motor1.getPosition()); + motor2.toPosition(extensions.second, 10); + pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + } + + + while (true) { + ThisThread::sleep_for(10ms); } } \ No newline at end of file From 0d24ea6c23e16b55c4700862164b95f4ce849da5 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Tue, 20 May 2025 13:53:36 -0700 Subject: [PATCH 14/47] Control sequence tested --- MotorCOTS.cpp | 2 +- main.cpp | 58 +++++++++++++++++++++++++-------------------------- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 2a20f98..a77278e 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -125,5 +125,5 @@ void MotorCOTS::toPosition(float pullPercent, int dt) { } pc->printf("Curr: %f\t", currPos); pc->printf("Target: %f\t", targetPos); - pc->printf("Difference: %f\n", currPos-targetPos); + // pc->printf("Difference: %f\t", currPos-targetPos); } \ No newline at end of file diff --git a/main.cpp b/main.cpp index a205694..9348c2a 100644 --- a/main.cpp +++ b/main.cpp @@ -23,8 +23,6 @@ int main() { // MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); float dt = 0.1; - Timer t; - t.start(); Distributor dstb; @@ -36,70 +34,72 @@ int main() { pc.printf("Target Angle: %f\n\n", target_position); - std::pair extensions; + std::pair extensions; int seconds = 30; // CONTROL SEQUENCE - for (int i = 0; i < seconds*100; i++) { + Timer t; + t.start(); + + while (t.read_ms() < 1000*seconds) { motor1.toPosition(0, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(0, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + pc.printf("Loop 1\n"); + ThisThread::sleep_for(10ms); } - + t.reset(); extensions = dstb.getMotorOutputsManual(1); - for (int i = 0; i < seconds*100; i++) { + while (t.read_ms() < 1000*seconds) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + pc.printf("Loop 2\n"); + ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(0.5); - for (int i = 0; i < seconds*100; i++) { + t.reset(); + while (t.read_ms() < 1000*seconds) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + pc.printf("Loop 3\n"); + ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(0); - for (int i = 0; i < seconds*100; i++) { + t.reset(); + while (t.read_ms() < 1000*seconds) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + ThisThread::sleep_for(10ms); + pc.printf("Loop 4\n"); } extensions = dstb.getMotorOutputsManual(-0.5); - for (int i = 0; i < seconds*100; i++) { + t.reset(); + while (t.read_ms() < 1000*seconds) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + pc.printf("Loop 5\n"); + ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(-1); - for (int i = 0; i < seconds*100; i++) { + t.reset(); + while (t.read_ms() < 1000*seconds) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); + pc.printf("Loop 6\n"); + ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(1); - for (int i = 0; i < seconds*100; i++) { + while (true) { motor1.toPosition(extensions.first, 10); - pc.printf("Current Pos 1: %f\t", motor1.getPosition()); motor2.toPosition(extensions.second, 10); - pc.printf("Current Pos 2: %f\t", motor2.getPosition()); - } - - - while (true) { + pc.printf("Loop 7\n"); ThisThread::sleep_for(10ms); } } \ No newline at end of file From 4bb9920a91fbaf21493ad1e725fff6ddaee1bb14 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Tue, 20 May 2025 16:37:50 -0700 Subject: [PATCH 15/47] Ctrl sequence tested TEST CONSTANTS --- MotorCOTS.h | 4 ++-- main.cpp | 7 +++++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index c7e40fd..c59303d 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -24,8 +24,8 @@ class MotorCOTS { volatile int encoderCounter = 0; // Counter for the encoder ticks const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors - const float spoolDiameter = 0.5; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 72; // INCHES + const float spoolDiameter = 0.24; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const + const float MAX_DEFLECTION = 10; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; diff --git a/main.cpp b/main.cpp index 9348c2a..546762e 100644 --- a/main.cpp +++ b/main.cpp @@ -44,6 +44,7 @@ int main() { t.start(); while (t.read_ms() < 1000*seconds) { + pc.printf("State: full open"); motor1.toPosition(0, 10); motor2.toPosition(0, 10); pc.printf("Loop 1\n"); @@ -53,6 +54,7 @@ int main() { t.reset(); extensions = dstb.getMotorOutputsManual(1); while (t.read_ms() < 1000*seconds) { + pc.printf("State: 1"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); pc.printf("Loop 2\n"); @@ -62,6 +64,7 @@ int main() { extensions = dstb.getMotorOutputsManual(0.5); t.reset(); while (t.read_ms() < 1000*seconds) { + pc.printf("State: 0.5"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); pc.printf("Loop 3\n"); @@ -71,6 +74,7 @@ int main() { extensions = dstb.getMotorOutputsManual(0); t.reset(); while (t.read_ms() < 1000*seconds) { + pc.printf("State: 0"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); ThisThread::sleep_for(10ms); @@ -80,6 +84,7 @@ int main() { extensions = dstb.getMotorOutputsManual(-0.5); t.reset(); while (t.read_ms() < 1000*seconds) { + pc.printf("State: -0.5"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); pc.printf("Loop 5\n"); @@ -89,6 +94,7 @@ int main() { extensions = dstb.getMotorOutputsManual(-1); t.reset(); while (t.read_ms() < 1000*seconds) { + pc.printf("State: -1"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); pc.printf("Loop 6\n"); @@ -97,6 +103,7 @@ int main() { extensions = dstb.getMotorOutputsManual(1); while (true) { + pc.printf("State: 1"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); pc.printf("Loop 7\n"); From fd9e06057ad96e875900e9045598cff30a89ec49 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Wed, 21 May 2025 12:05:56 -0700 Subject: [PATCH 16/47] changed spool diam and added more tabs to output --- MotorCOTS.cpp | 4 +- MotorCOTS.h | 4 +- main.cpp | 110 +++++++++++++++++++++++++++++++++++++++++--------- 3 files changed, 95 insertions(+), 23 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index a77278e..41e5322 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -123,7 +123,7 @@ void MotorCOTS::toPosition(float pullPercent, int dt) { } else { motorPower(-power); } - pc->printf("Curr: %f\t", currPos); - pc->printf("Target: %f\t", targetPos); + pc->printf("\t\tCurrent: %f", currPos); + pc->printf("\tTarget: %f", targetPos); // pc->printf("Difference: %f\t", currPos-targetPos); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index c59303d..4908059 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -24,8 +24,8 @@ class MotorCOTS { volatile int encoderCounter = 0; // Counter for the encoder ticks const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors - const float spoolDiameter = 0.24; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 10; // INCHES + const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const + const float MAX_DEFLECTION = 25.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; diff --git a/main.cpp b/main.cpp index 546762e..f52e47f 100644 --- a/main.cpp +++ b/main.cpp @@ -1,3 +1,4 @@ +#include "ThisThread.h" #include "mbed.h" #include "MotorCOTS.h" #include "EUSBSerial.h" @@ -5,6 +6,7 @@ #include "Distributor.h" + int main() { DigitalOut led(PB_0); led.write(1); @@ -27,11 +29,11 @@ int main() { Distributor dstb; - // not sure if we need this - float degrees = 0; - float current_angle = 0; - float target_position = 0.1; // degrees - pc.printf("Target Angle: %f\n\n", target_position); + // // not sure if we need this + // float degrees = 0; + // float current_angle = 0; + // float target_position = 0.1; // degrees + // pc.printf("Target Angle: %f\n\n", target_position); std::pair extensions; @@ -44,7 +46,8 @@ int main() { t.start(); while (t.read_ms() < 1000*seconds) { - pc.printf("State: full open"); + led = !led; + pc.printf("cmd: 0"); motor1.toPosition(0, 10); motor2.toPosition(0, 10); pc.printf("Loop 1\n"); @@ -54,59 +57,128 @@ int main() { t.reset(); extensions = dstb.getMotorOutputsManual(1); while (t.read_ms() < 1000*seconds) { - pc.printf("State: 1"); + led = !led; + pc.printf("\tcmd: 1"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); - pc.printf("Loop 2\n"); + pc.printf("\tLoop 2\n"); ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(0.5); t.reset(); while (t.read_ms() < 1000*seconds) { - pc.printf("State: 0.5"); + led = !led; + pc.printf("State: 0.5\t"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); - pc.printf("Loop 3\n"); + pc.printf("\tLoop 3\n"); ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(0); t.reset(); while (t.read_ms() < 1000*seconds) { - pc.printf("State: 0"); + led = !led; + pc.printf("State: 0\t"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); ThisThread::sleep_for(10ms); - pc.printf("Loop 4\n"); + pc.printf("\tLoop 4\n"); } extensions = dstb.getMotorOutputsManual(-0.5); t.reset(); while (t.read_ms() < 1000*seconds) { - pc.printf("State: -0.5"); + led = !led; + pc.printf("State: -0.5\t"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); - pc.printf("Loop 5\n"); + pc.printf("\tLoop 5\n"); ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(-1); t.reset(); while (t.read_ms() < 1000*seconds) { - pc.printf("State: -1"); + led = !led; + pc.printf("State: -1\t"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); - pc.printf("Loop 6\n"); + pc.printf("\tLoop 6\n"); ThisThread::sleep_for(10ms); } extensions = dstb.getMotorOutputsManual(1); - while (true) { - pc.printf("State: 1"); + while (t.read_ms() < 1000*seconds) { + led = !led; + pc.printf("State: 1\t"); motor1.toPosition(extensions.first, 10); motor2.toPosition(extensions.second, 10); - pc.printf("Loop 7\n"); + pc.printf("\tLoop 7\n"); ThisThread::sleep_for(10ms); } + + while (true) { + led = !led; + pc.printf("State: full open"); + motor1.toPosition(0, 10); + motor2.toPosition(0, 10); + pc.printf("\tLoop 8 (END)\n"); + ThisThread::sleep_for(10ms); + } + + // =================================== + + // while (t.read_ms() < 1000*seconds) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(1, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + + + // while (t.read_ms() < 1000*seconds) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(0.5f, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + + + // while (t.read_ms() < 1000*seconds) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(0.2, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + + + // while (t.read_ms() < 1000*seconds) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(0, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + + // while (t.read_ms() < 1000*seconds) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(1, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + + // while (true) { + // led = !led; + // pc.printf("State: full open"); + // motor1.toPosition(0, 10); + // pc.printf("Loop 1\n"); + // ThisThread::sleep_for(10ms); + // } + } \ No newline at end of file From 0bcf93ab02ef495d813e0b79f63c2304d13e1715 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Wed, 21 May 2025 18:52:05 -0700 Subject: [PATCH 17/47] less prints --- MotorCOTS.cpp | 6 ++++-- MotorCOTS.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 41e5322..a89c0a6 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -123,7 +123,9 @@ void MotorCOTS::toPosition(float pullPercent, int dt) { } else { motorPower(-power); } - pc->printf("\t\tCurrent: %f", currPos); - pc->printf("\tTarget: %f", targetPos); + pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); + // pc->printf("\tTarget pull percent: %f", pullPercent); + // pc-printf("\tMax deflection: %f", MAX_DEFLECTION); + // pc->printf("Difference: %f\t", currPos-targetPos); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index 4908059..6c3fe10 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -25,7 +25,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 25.0f; // INCHES + const float MAX_DEFLECTION = 54.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; From 7f94572716a488839799d0e830a2e120c3ccd683 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Wed, 21 May 2025 18:52:28 -0700 Subject: [PATCH 18/47] cleaned up main - READY FOR FINAL TEST --- main.cpp | 198 ++++++++++++------------------------------------------- 1 file changed, 43 insertions(+), 155 deletions(-) diff --git a/main.cpp b/main.cpp index f52e47f..efea999 100644 --- a/main.cpp +++ b/main.cpp @@ -7,178 +7,66 @@ -int main() { - DigitalOut led(PB_0); - led.write(1); - PID pid(1, 0, 5); - EUSBSerial pc; - ThisThread::sleep_for(1s); - pc.printf("\nSerial Port Connected!\n"); - - // direction 1, direction 2, throttle, encoder a, encoder b - - // // ollie motor - MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); - MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); - led.write(0); - // jimmy motor: - // MotorCOTS motor(PA_2, PA_3, PB_1, PA_6, PA_7); - - float dt = 0.1; - - Distributor dstb; - - - // // not sure if we need this - // float degrees = 0; - // float current_angle = 0; - // float target_position = 0.1; // degrees - // pc.printf("Target Angle: %f\n\n", target_position); - - - std::pair extensions; - int seconds = 30; - - - // CONTROL SEQUENCE - +void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distributor* dstb, EUSBSerial* pc) { Timer t; t.start(); - - while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("cmd: 0"); - motor1.toPosition(0, 10); - motor2.toPosition(0, 10); - pc.printf("Loop 1\n"); - ThisThread::sleep_for(10ms); - } - - t.reset(); - extensions = dstb.getMotorOutputsManual(1); - while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("\tcmd: 1"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - pc.printf("\tLoop 2\n"); - ThisThread::sleep_for(10ms); - } - - extensions = dstb.getMotorOutputsManual(0.5); - t.reset(); - while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("State: 0.5\t"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - pc.printf("\tLoop 3\n"); - ThisThread::sleep_for(10ms); - } - - extensions = dstb.getMotorOutputsManual(0); - t.reset(); - while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("State: 0\t"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - ThisThread::sleep_for(10ms); - pc.printf("\tLoop 4\n"); - } - - extensions = dstb.getMotorOutputsManual(-0.5); - t.reset(); - while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("State: -0.5\t"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - pc.printf("\tLoop 5\n"); - ThisThread::sleep_for(10ms); - } - - extensions = dstb.getMotorOutputsManual(-1); - t.reset(); + std::pair extensions; + extensions = dstb->getMotorOutputsManual(cmd); while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("State: -1\t"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - pc.printf("\tLoop 6\n"); + pc->printf("\tcmd: %f", cmd); + motor1->toPosition(extensions.first, 10); + motor2->toPosition(extensions.second, 10); + pc->printf("\n"); ThisThread::sleep_for(10ms); } +} - extensions = dstb.getMotorOutputsManual(1); +void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, EUSBSerial* pc) { + Timer t; + t.start(); while (t.read_ms() < 1000*seconds) { - led = !led; - pc.printf("State: 1\t"); - motor1.toPosition(extensions.first, 10); - motor2.toPosition(extensions.second, 10); - pc.printf("\tLoop 7\n"); - ThisThread::sleep_for(10ms); - } - - while (true) { - led = !led; - pc.printf("State: full open"); - motor1.toPosition(0, 10); - motor2.toPosition(0, 10); - pc.printf("\tLoop 8 (END)\n"); + pc->printf("cmd: full open"); + motor1->toPosition(cmd1, 10); + motor2->toPosition(cmd2, 10); + pc->printf("\n"); ThisThread::sleep_for(10ms); } +} - // =================================== - // while (t.read_ms() < 1000*seconds) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(1, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); - // } - - - // while (t.read_ms() < 1000*seconds) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(0.5f, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); - // } +int main() { + DigitalOut led(PC_13); + led.write(0); + PID pid(1, 0, 5); + EUSBSerial pc; + ThisThread::sleep_for(1s); + pc.printf("\nSerial Port Connected!\n"); - // while (t.read_ms() < 1000*seconds) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(0.2, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); - // } + // direction 1, direction 2, throttle, encoder a, encoder b + MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); + MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); + float dt = 0.1; + Distributor dstb; + std::pair extensions; - // while (t.read_ms() < 1000*seconds) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(0, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); + // tools: + // ctrl_manual(0, 0, 30, &motor1, &motor2, &pc); // cmd1, cmd2, duration (seconds) + // while (true) { + // motor1.motorPower(-1); + // // motor2.motorPower(1); // } + // ctrl_manual(1, 0, 300, &motor1, &motor2, &pc); - // while (t.read_ms() < 1000*seconds) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(1, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); - // } - // while (true) { - // led = !led; - // pc.printf("State: full open"); - // motor1.toPosition(0, 10); - // pc.printf("Loop 1\n"); - // ThisThread::sleep_for(10ms); - // } + // CONTROL SEQUENCE: + // ctrl(cmd1, cmd2, ...) + ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); + ctrl(1, 120, &motor1, &motor2, &dstb, &pc); + ctrl(0, 60, &motor1, &motor2, &dstb, &pc); + ctrl(-0.5, 60, &motor1, &motor2, &dstb, &pc); + ctrl(-1, 60, &motor1, &motor2, &dstb, &pc); + ctrl(1, 999999999, &motor1, &motor2, &dstb, &pc); } \ No newline at end of file From e79423689f51ff0790ca66aac6636dcf05079c19 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Thu, 22 May 2025 02:29:07 -0700 Subject: [PATCH 19/47] fixed integer overflow--FINAL FLIGHT CODE --- main.cpp | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/main.cpp b/main.cpp index efea999..7d588ea 100644 --- a/main.cpp +++ b/main.cpp @@ -1,3 +1,4 @@ +#include "DigitalOut.h" #include "ThisThread.h" #include "mbed.h" #include "MotorCOTS.h" @@ -37,11 +38,23 @@ void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCO int main() { DigitalOut led(PC_13); - led.write(0); - PID pid(1, 0, 5); EUSBSerial pc; ThisThread::sleep_for(1s); pc.printf("\nSerial Port Connected!\n"); + led.write(1); + DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) + + while(ctrl_trigger.read() == 1) { + ThisThread::sleep_for(10ms); + } + + pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); + led.write(0); + PID pid(1, 0, 5); + + ThisThread::sleep_for(10s); + + // direction 1, direction 2, throttle, encoder a, encoder b MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); @@ -62,11 +75,15 @@ int main() { // CONTROL SEQUENCE: // ctrl(cmd1, cmd2, ...) + ctrl(0.25, 60, &motor1, &motor2, &dstb, &pc); ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); + ctrl(0.75, 60, &motor1, &motor2, &dstb, &pc); ctrl(1, 120, &motor1, &motor2, &dstb, &pc); ctrl(0, 60, &motor1, &motor2, &dstb, &pc); ctrl(-0.5, 60, &motor1, &motor2, &dstb, &pc); ctrl(-1, 60, &motor1, &motor2, &dstb, &pc); - ctrl(1, 999999999, &motor1, &motor2, &dstb, &pc); + while (true) { + ctrl(1, 3600, &motor1, &motor2, &dstb, &pc); + } } \ No newline at end of file From 9f7d57a2eb997e90039017faa87f5ddd1e512fe6 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Wed, 22 Oct 2025 21:21:55 -0700 Subject: [PATCH 20/47] Cleaned and commented code --- Distributor.cpp | 6 ++++++ Distributor.h | 4 ++-- MotorCOTS.cpp | 22 +++++++++++++++++++--- MotorCOTS.h | 2 ++ PID.cpp | 7 ++++++- main.cpp | 47 ++++++++++++++++++++++++----------------------- 6 files changed, 59 insertions(+), 29 deletions(-) diff --git a/Distributor.cpp b/Distributor.cpp index 0c9c758..a31d2cc 100644 --- a/Distributor.cpp +++ b/Distributor.cpp @@ -7,6 +7,9 @@ Distributor::Distributor(I2CSerial* ser) : ser(ser) {} Distributor::Distributor() {} + +// Reads sereal to get flight computer steering inputs +// Returns a tuple of floats representing the retraction of the left motor and right motor respectively std::pair Distributor::getMotorOutputs() { // Integer from -100 to 100 char buf[DISTRIBUTOR_BUFFER_SIZE]; @@ -23,6 +26,9 @@ std::pair Distributor::getMotorOutputs() { return getMotorOutputsManual(ctrl); } + +// Takes a flot [-1, 1] indicatng full left to full right steering respectively +// Returns a tuple of floats representing the retraction of the left motor and right motor respectively std::pair Distributor::getMotorOutputsManual(float ctrl) { float leftPull = 0.5 * (-ctrl) + 0.5; float rightPull = 0.5 * ctrl + 0.5; diff --git a/Distributor.h b/Distributor.h index 853c676..9996704 100644 --- a/Distributor.h +++ b/Distributor.h @@ -1,5 +1,5 @@ -// Class which translates a float input (from the flight controller) into two floats for each -// motors linear displacement +// Class which translates a float inputs (ideally from the flight controller) into two floats +// for each motors linear displacement #ifndef _DISTRIBUTOR_H_ #define _DISTRIBUTOR_H_ diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index a89c0a6..c2b959a 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -2,13 +2,14 @@ #include "MotorCOTS.h" #include "EUSBSerial.h" +// Updates class variables for motor state void MotorCOTS::updateGlobals() { position = encoderCounter; angle = (encoderCounter * 360.0) / totalCounts; rotations = angle / 360.0; - } +// Tickes encoder count forward or backward on encoder a rise callback void MotorCOTS::aRiseCallback() { aUp = true; @@ -19,7 +20,7 @@ void MotorCOTS::aRiseCallback() { } } - +// Tickes encoder count forward or backward on encoder b rise callback void MotorCOTS::bRiseCallback() { bUp = true; @@ -30,6 +31,7 @@ void MotorCOTS::bRiseCallback() { } } +// Tickes encoder count forward or backward on encoder a fall callback void MotorCOTS::aFallCallback() { aUp = false; @@ -40,6 +42,7 @@ void MotorCOTS::aFallCallback() { } } +// Tickes encoder count forward or backward on encoder b fall callback void MotorCOTS::bFallCallback() { bUp = false; @@ -51,6 +54,9 @@ void MotorCOTS::bFallCallback() { } +// Initialize motor object, set encoder states +// Takes: PinName for direction one control line, PinName for direction two control line, PinName for power throttle (PWM), +// PinName for encoder A, PinName for encoder B, PID pointer for motor PID controller, EUSBSerial pointer for debugging serial MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid, EUSBSerial* pc) : encoderA(PINA), encoderB(PINB), directionOne(directionOne), directionTwo(directionTwo), powerThrottle(powerThrottle), pid(pid), pc(pc) { @@ -76,6 +82,9 @@ MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerTh } + +// Set motor direction +// Takes: int direction (1 for forward, -1 for backward) void MotorCOTS::direction(int direction) { if (direction == 1) { ThisThread::sleep_for(2ms); @@ -90,6 +99,8 @@ void MotorCOTS::direction(int direction) { } } +// Set motor power +// Takes: float power (0 to 1 for forward, 0 to -1 for backward) void MotorCOTS::motorPower(float power) { if (power > 0) { direction(1); @@ -102,15 +113,19 @@ void MotorCOTS::motorPower(float power) { } } +// Returns motor angle in degrees int MotorCOTS::getDegrees() { updateGlobals(); return (int)angle; } +// Returns motor linear position in inches float MotorCOTS::getPosition() { return static_cast(getDegrees()) / 360.0 * PI * spoolDiameter; } +// Moves motor to target position using PID controller +// Takes: float target position in inches, int dt in milliseconds since last call void MotorCOTS::toPosition(float pullPercent, int dt) { float currPos = getPosition(); @@ -123,9 +138,10 @@ void MotorCOTS::toPosition(float pullPercent, int dt) { } else { motorPower(-power); } + + // Debug print statements pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); // pc->printf("\tTarget pull percent: %f", pullPercent); // pc-printf("\tMax deflection: %f", MAX_DEFLECTION); - // pc->printf("Difference: %f\t", currPos-targetPos); } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index 6c3fe10..b656ebb 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -1,3 +1,5 @@ +// Class which controls a COTS motor with encoder feedback + #ifndef _MOTORCOTS_H_ #define _MOTORCOTS_H_ diff --git a/PID.cpp b/PID.cpp index 80574bb..00b4086 100644 --- a/PID.cpp +++ b/PID.cpp @@ -1,6 +1,8 @@ #include "PID.h" - +// Initialize PID controller +// Takes: float for proportional constant, float for integral constant +// float derivative constant PID::PID(float Kp, float Ki, float Kd) { positiveLast = true; errorLast = 0; @@ -10,6 +12,9 @@ PID::PID(float Kp, float Ki, float Kd) { this->Kd = Kd; } +// Compute PID output +// Takes: float current angle, float target angle, float dt in milliseconds since last call +// Returns: float output power (-1 to 1) float PID::compute(float currAngle, float targetAngle, float dt) { float error = targetAngle - currAngle; integralError += error*dt; // todo: solve integral windup later diff --git a/main.cpp b/main.cpp index 7d588ea..b1cfaa4 100644 --- a/main.cpp +++ b/main.cpp @@ -7,7 +7,9 @@ #include "Distributor.h" - +// Steers ARES in a set turn angle for a set ammount of time +// Takes: float for steering (-1 full left 1 full right), an int of seconds to hold for, two motor pointer +// for the left and right motors, a distributor pointer, and a USB serial pointer void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distributor* dstb, EUSBSerial* pc) { Timer t; t.start(); @@ -22,6 +24,9 @@ void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distribu } } +// Pulls each control line set ammounts for a set ammount of time +// Takes: float for left motor retraction, float for right motor retraction, an int of seconds to hold for, +// two motor pointer for the left and right motors, a distributor pointer, and a USB serial pointer void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, EUSBSerial* pc) { Timer t; t.start(); @@ -37,44 +42,40 @@ void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCO int main() { + // Debug LED DigitalOut led(PC_13); + + // USB serial init EUSBSerial pc; ThisThread::sleep_for(1s); pc.printf("\nSerial Port Connected!\n"); led.write(1); - DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) - - while(ctrl_trigger.read() == 1) { - ThisThread::sleep_for(10ms); - } - - pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); - led.write(0); - PID pid(1, 0, 5); - ThisThread::sleep_for(10s); - - - // direction 1, direction 2, throttle, encoder a, encoder b + // Object init + PID pid(1, 0, 5); + // Direction 1, direction 2, throttle, encoder a, encoder b MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); - float dt = 0.1; Distributor dstb; std::pair extensions; - // tools: - // ctrl_manual(0, 0, 30, &motor1, &motor2, &pc); // cmd1, cmd2, duration (seconds) - // while (true) { - // motor1.motorPower(-1); - // // motor2.motorPower(1); - // } - // ctrl_manual(1, 0, 300, &motor1, &motor2, &pc); + + // Control Trigger + DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) + while(ctrl_trigger.read() == 1) { + ThisThread::sleep_for(10ms); + } + pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); + led.write(0); + + // CONTROL SEQUENCE: - // ctrl(cmd1, cmd2, ...) + // ctrl(cmd1, time (seconds), ...) + // cmd = -1 is full left, 1 is full right ctrl(0.25, 60, &motor1, &motor2, &dstb, &pc); ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); ctrl(0.75, 60, &motor1, &motor2, &dstb, &pc); From d68abebab5cd667ee60e05b8bbb44a76ce8cb0ff Mon Sep 17 00:00:00 2001 From: Peterb063 Date: Fri, 24 Oct 2025 20:02:08 -0700 Subject: [PATCH 21/47] Update motor pinout and disable control trigger Changed MotorCOTS initialization to use new driver board pinout. Commented out the control trigger logic for now. --- main.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/main.cpp b/main.cpp index b1cfaa4..317b408 100644 --- a/main.cpp +++ b/main.cpp @@ -55,20 +55,23 @@ int main() { // Object init PID pid(1, 0, 5); // Direction 1, direction 2, throttle, encoder a, encoder b - MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); - MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); + // MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); + // MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); + //New Driver board pinout + MotorCOTS motor1(PA_6, PA_5, PA_7, PC_15, PA_1, &pid, &pc); + MotorCOTS motor2(PA_4, PA_3, PA_2, PB_9, PC_14, &pid, &pc); Distributor dstb; std::pair extensions; - // Control Trigger - DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) - while(ctrl_trigger.read() == 1) { - ThisThread::sleep_for(10ms); - } - pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); - led.write(0); + // // Control Trigger + // DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) + // while(ctrl_trigger.read() == 1) { + // ThisThread::sleep_for(10ms); + // } + // pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); + // led.write(0); From 26e4de870729e58f85753fc25d6ecd4e6e44d295 Mon Sep 17 00:00:00 2001 From: Peterb063 Date: Thu, 30 Oct 2025 00:01:08 -0700 Subject: [PATCH 22/47] Update motor pin assignments for new driver board These are the most recent pin assignments. --- main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/main.cpp b/main.cpp index 317b408..e49e7c7 100644 --- a/main.cpp +++ b/main.cpp @@ -57,9 +57,9 @@ int main() { // Direction 1, direction 2, throttle, encoder a, encoder b // MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); // MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); - //New Driver board pinout - MotorCOTS motor1(PA_6, PA_5, PA_7, PC_15, PA_1, &pid, &pc); - MotorCOTS motor2(PA_4, PA_3, PA_2, PB_9, PC_14, &pid, &pc); + // New Driver board pinout + MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A + MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B Distributor dstb; std::pair extensions; From 96d61b6a397f76f9d2f0a07c4b5740b88844088e Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 8 Jan 2026 18:52:54 -0800 Subject: [PATCH 23/47] Moved comms i2c handler into test branch --- main.cpp | 181 ++++++++++++++++++++++++++++++++++--------------------- 1 file changed, 111 insertions(+), 70 deletions(-) diff --git a/main.cpp b/main.cpp index e49e7c7..3ec99f7 100644 --- a/main.cpp +++ b/main.cpp @@ -1,93 +1,134 @@ -#include "DigitalOut.h" -#include "ThisThread.h" #include "mbed.h" -#include "MotorCOTS.h" -#include "EUSBSerial.h" +#include +#include "I2CSerial.h" + +#include "Motor.h" #include "PID.h" #include "Distributor.h" +#include "EUSBSerial.h" -// Steers ARES in a set turn angle for a set ammount of time -// Takes: float for steering (-1 full left 1 full right), an int of seconds to hold for, two motor pointer -// for the left and right motors, a distributor pointer, and a USB serial pointer -void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distributor* dstb, EUSBSerial* pc) { - Timer t; - t.start(); - std::pair extensions; - extensions = dstb->getMotorOutputsManual(cmd); - while (t.read_ms() < 1000*seconds) { - pc->printf("\tcmd: %f", cmd); - motor1->toPosition(extensions.first, 10); - motor2->toPosition(extensions.second, 10); - pc->printf("\n"); - ThisThread::sleep_for(10ms); - } -} +DigitalOut led(PC_13); +EUSBSerial pc; -// Pulls each control line set ammounts for a set ammount of time -// Takes: float for left motor retraction, float for right motor retraction, an int of seconds to hold for, -// two motor pointer for the left and right motors, a distributor pointer, and a USB serial pointer -void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, EUSBSerial* pc) { - Timer t; - t.start(); - while (t.read_ms() < 1000*seconds) { - pc->printf("cmd: full open"); - motor1->toPosition(cmd1, 10); - motor2->toPosition(cmd2, 10); - pc->printf("\n"); - ThisThread::sleep_for(10ms); - } +int leftExtension = 0; +int rightExtension = 0; + +Thread i2cThread; // setup thread + +char i2c_tx_buf[32]; +char i2c_rx_buf[32]; + +#define MCPS_ADDR 0x02 << 1 + +Mutex mutex; +float cmd_ctrl = 0; + +struct { + float leftDegrees; + float rightDegrees; + float leftPower; + float rightPower; +} motorPacket; + +void update_struct(float leftDegrees, float rightDegrees, float leftPower, float rightPower) { + ScopedLock lock(mutex); + motorPacket.leftDegrees = leftDegrees; + motorPacket.rightDegrees = rightDegrees; + motorPacket.leftPower = leftPower; + motorPacket.rightPower = rightPower; } +// SDA, SCL +I2CSlave slave(PB_7, PB_6); +void i2c_handler(void) { + slave.address(MCPS_ADDR); -int main() { - // Debug LED - DigitalOut led(PC_13); - - // USB serial init - EUSBSerial pc; - ThisThread::sleep_for(1s); - pc.printf("\nSerial Port Connected!\n"); - led.write(1); + while(true) { + int event = slave.receive(); + + switch(event) { + + case I2CSlave::WriteAddressed: { + int err = slave.read(i2c_rx_buf, sizeof(float)); - // Object init - PID pid(1, 0, 5); - // Direction 1, direction 2, throttle, encoder a, encoder b - // MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); - // MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); - // New Driver board pinout - MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A - MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B - Distributor dstb; - std::pair extensions; + { + ScopedLock lock(); + memcpy(&cmd_ctrl, i2c_rx_buf, sizeof(float)); + } + break; + } + case I2CSlave::ReadAddressed: { - // // Control Trigger - // DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) - // while(ctrl_trigger.read() == 1) { + { + ScopedLock lock(); + memcpy(i2c_tx_buf, &motorPacket, sizeof(motorPacket)); + } + + slave.write(i2c_tx_buf, sizeof(motorPacket)); + + break; + } + + default: break; + } + } +} + +int main() +{ + + // PID pid(0.017, 0, 1); // No idea if these values work + // Motor motor(PA_8, PA_10, PB_2, PB_1, PB_15, PB_14, pid); // test bench + // Motor motor1(PB_3, PB_5, PA_11, PA_12, PA_10, PA_9, pid); // these are the mcpcb + // Motor motor2(PA_6, PA_5, PB_14, PB_15, PB_13, PA_8, pid); + + Distributor distributor; + + i2cThread.start(i2c_handler); + + // while (true) { // ThisThread::sleep_for(10ms); + // // get distributed pair + // mutex.lock(); + // float ctrl = f; + // mutex.unlock(); + // std::pair spoolExtensions = distributor.getMotorOutputs(ctrl); + + // // if the first float is a NAN, keep each extension value the same + // leftExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.first : leftExtension; + // rightExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.second : rightExtension; + + // // go to position + // float power1 = motor1.lineTo(leftExtension, 10); + // float power2 = motor2.lineTo(rightExtension, 10); + + // update_struct(motor1.getDegrees(), motor2.getDegrees(), power1, power2); // } - // pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); - // led.write(0); + while (true) { + float ctrl; + { + ScopedLock lock(); + ctrl = cmd_ctrl; + } + + std::pair spoolExtensions = distributor.getMotorOutputs(ctrl); + float extension = spoolExtensions.first; + + if (extension > 0.5) led.write(1); + else led.write(0); + + update_struct(1.0, 2.0, 3.0, 4.0); + + ThisThread::sleep_for(100ms); - // CONTROL SEQUENCE: - // ctrl(cmd1, time (seconds), ...) - // cmd = -1 is full left, 1 is full right - ctrl(0.25, 60, &motor1, &motor2, &dstb, &pc); - ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); - ctrl(0.75, 60, &motor1, &motor2, &dstb, &pc); - ctrl(1, 120, &motor1, &motor2, &dstb, &pc); - ctrl(0, 60, &motor1, &motor2, &dstb, &pc); - ctrl(-0.5, 60, &motor1, &motor2, &dstb, &pc); - ctrl(-1, 60, &motor1, &motor2, &dstb, &pc); - while (true) { - ctrl(1, 3600, &motor1, &motor2, &dstb, &pc); } +} -} \ No newline at end of file From 83453de007772e326486f22cab8bbf04263b2940 Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 8 Jan 2026 18:54:29 -0800 Subject: [PATCH 24/47] Deleted i2c and related functions --- Distributor.cpp | 22 ----- Distributor.h | 5 -- I2CSerial/I2CSerial.cpp | 180 ---------------------------------------- I2CSerial/I2CSerial.h | 121 --------------------------- main.cpp | 123 ++++++++++++++++----------- 5 files changed, 76 insertions(+), 375 deletions(-) delete mode 100644 I2CSerial/I2CSerial.cpp delete mode 100644 I2CSerial/I2CSerial.h diff --git a/Distributor.cpp b/Distributor.cpp index a31d2cc..52421e6 100644 --- a/Distributor.cpp +++ b/Distributor.cpp @@ -3,30 +3,8 @@ #include "Distributor.h" #include - -Distributor::Distributor(I2CSerial* ser) : ser(ser) {} Distributor::Distributor() {} - -// Reads sereal to get flight computer steering inputs -// Returns a tuple of floats representing the retraction of the left motor and right motor respectively -std::pair Distributor::getMotorOutputs() { - // Integer from -100 to 100 - char buf[DISTRIBUTOR_BUFFER_SIZE]; - if ( !ser->readline(buf, DISTRIBUTOR_BUFFER_SIZE) ) { - return std::make_pair(NAN, 0); // whats' the differnece bewteen a nan("1") and a different nan - } - int input; - int res = sscanf(buf, "%d", &input); - if (res != 1) { - return std::make_pair(NAN, 1); - } - - float ctrl = ( (float) ((int) buf) ) / 100.0; - return getMotorOutputsManual(ctrl); -} - - // Takes a flot [-1, 1] indicatng full left to full right steering respectively // Returns a tuple of floats representing the retraction of the left motor and right motor respectively std::pair Distributor::getMotorOutputsManual(float ctrl) { diff --git a/Distributor.h b/Distributor.h index 9996704..ed77cad 100644 --- a/Distributor.h +++ b/Distributor.h @@ -5,19 +5,14 @@ #include "mbed.h" -#include "I2CSerial.h" #define DISTRIBUTOR_BUFFER_SIZE 8 class Distributor { - private: - I2CSerial* ser; public: - Distributor(I2CSerial* ser); Distributor(); - std::pair getMotorOutputs(); std::pair getMotorOutputsManual(float ctrl); }; diff --git a/I2CSerial/I2CSerial.cpp b/I2CSerial/I2CSerial.cpp deleted file mode 100644 index 6a8025a..0000000 --- a/I2CSerial/I2CSerial.cpp +++ /dev/null @@ -1,180 +0,0 @@ -#include "I2CSerial.h" -#include - - -I2CSerial::I2CSerial(PinName SDA, PinName SCL, int address, bool slave) { - this->own_i2c = true; - - this->is_slave = slave; - this->address = address; - - if (is_slave) { - this->slave = new I2CSlave(SDA, SCL); - this->slave->address(address); - } - else { - this->i2c = new I2C(SDA, SCL); - } - - // clear buffers and pointers - msgs_pointer = 0; - memset(msgs_buf, 0, MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE); - write_pointer = 0; - memset(write_buf, 0, MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - - this->running = true; - t.set_priority(osPriorityBelowNormal1); - t.start([this]() {loop();}); -} - -I2CSerial::I2CSerial(I2C* i2c, int address) { - this->own_i2c = false; - this->i2c = i2c; - this->is_slave = false; - this->address = address; - - // clear buffers and pointers - msgs_pointer = 0; - memset(msgs_buf, 0, MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE); - write_pointer = 0; - memset(write_buf, 0, MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - - this->running = true; - t.set_priority(osPriorityBelowNormal1); - t.start([this]() {loop();}); -} - -I2CSerial::~I2CSerial() { - if (!own_i2c) { - delete this->i2c; - } - - if (is_slave) { - delete this->slave; - } - - this->running = false; - t.join(); -} - -void I2CSerial::loop() { - // slave code, listen for i2c events and respond as requested - if (is_slave) { - while (true) { - int i = slave->receive(); - switch (i) { - case I2CSlave::ReadAddressed: - if (write_pointer > 0) { - slave->write(write_buf, MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - write_pointer = 0; - } - break; - case I2CSlave::WriteGeneral: - break; - case I2CSlave::WriteAddressed: - slave->read(&msgs_buf[msgs_pointer], MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - size_t len = strlen(&msgs_buf[msgs_pointer]); - msgs_pointer += (len > 0) ? len+1 : len; - - break; - } - ThisThread::yield(); - } - } - - // master code polls slave for new messages - else { - while (true) { - i2c->read(this->address, &msgs_buf[msgs_pointer], MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - size_t len = strlen(&msgs_buf[msgs_pointer]); - msgs_pointer += (len > 0) ? len+1 : len; - - if (write_pointer > 0) { - i2c->write(this->address, write_buf, MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE); - write_pointer = 0; - } - - ThisThread::sleep_for(MBED_CONF_I2CSERIAL_MSG_POLL_MS); - } - } -} - -I2CSerialError I2CSerial::printf(const char* format, ...) { - va_list args; - - char buf[MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE] = {0}; - - va_start(args, format); - - size_t n = vsnprintf(buf, MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE-write_pointer, format, args); - n += 1; // null terminator - - va_end(args); - - if (n > MBED_CONF_I2CSERIAL_MAX_PACKET_SIZE) { - return I2CSerialError::I2CS_MSG_TOO_LARGE; - } - - return write(buf, n); -} - -I2CSerialError I2CSerial::write(const char* buf, size_t size) { - if (size > MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE-write_pointer) { - return I2CSerialError::I2CS_WRITE_BUFFER_FULL; - } - - memcpy(&write_buf[write_pointer], buf, size); - write_pointer += size; - - return I2CSerialError::I2CS_OK; -} - -bool I2CSerial::read(char* buf, size_t size) { - if (msgs_pointer < size) { - return false; - } - - size = (size > MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE) ? MBED_CONF_I2CSERIAL_MSG_BUFFER_SIZE : size; - - memcpy(buf, msgs_buf, size); - for (int i = size; i -#include "I2CSerial.h" - -#include "Motor.h" +#include "MotorCOTS.h" +#include "EUSBSerial.h" #include "PID.h" #include "Distributor.h" -#include "EUSBSerial.h" - DigitalOut led(PC_13); EUSBSerial pc; @@ -79,56 +77,87 @@ void i2c_handler(void) { } } -int main() -{ +// Steers ARES in a set turn angle for a set ammount of time +// Takes: float for steering (-1 full left 1 full right), an int of seconds to hold for, two motor pointer +// for the left and right motors, a distributor pointer, and a USB serial pointer +void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distributor* dstb, EUSBSerial* pc) { + Timer t; + t.start(); + std::pair extensions; + extensions = dstb->getMotorOutputsManual(cmd); + while (t.read_ms() < 1000*seconds) { + pc->printf("\tcmd: %f", cmd); + motor1->toPosition(extensions.first, 10); + motor2->toPosition(extensions.second, 10); + pc->printf("\n"); + ThisThread::sleep_for(10ms); + } +} + +// Pulls each control line set ammounts for a set ammount of time +// Takes: float for left motor retraction, float for right motor retraction, an int of seconds to hold for, +// two motor pointer for the left and right motors, a distributor pointer, and a USB serial pointer +void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, EUSBSerial* pc) { + Timer t; + t.start(); + while (t.read_ms() < 1000*seconds) { + pc->printf("cmd: full open"); + motor1->toPosition(cmd1, 10); + motor2->toPosition(cmd2, 10); + pc->printf("\n"); + ThisThread::sleep_for(10ms); + } +} + - // PID pid(0.017, 0, 1); // No idea if these values work - // Motor motor(PA_8, PA_10, PB_2, PB_1, PB_15, PB_14, pid); // test bench - // Motor motor1(PB_3, PB_5, PA_11, PA_12, PA_10, PA_9, pid); // these are the mcpcb - // Motor motor2(PA_6, PA_5, PB_14, PB_15, PB_13, PA_8, pid); + +int main() { + // Debug LED + DigitalOut led(PC_13); - Distributor distributor; + // USB serial init + EUSBSerial pc; + ThisThread::sleep_for(1s); + pc.printf("\nSerial Port Connected!\n"); + led.write(1); - i2cThread.start(i2c_handler); - // while (true) { - // ThisThread::sleep_for(10ms); - // // get distributed pair - // mutex.lock(); - // float ctrl = f; - // mutex.unlock(); - // std::pair spoolExtensions = distributor.getMotorOutputs(ctrl); - - // // if the first float is a NAN, keep each extension value the same - // leftExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.first : leftExtension; - // rightExtension = (isnan(spoolExtensions.first)) ? spoolExtensions.second : rightExtension; - - // // go to position - // float power1 = motor1.lineTo(leftExtension, 10); - // float power2 = motor2.lineTo(rightExtension, 10); - - // update_struct(motor1.getDegrees(), motor2.getDegrees(), power1, power2); - // } + // Object init + PID pid(1, 0, 5); + // Direction 1, direction 2, throttle, encoder a, encoder b + // MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); + // MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); + // New Driver board pinout + MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A + MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B + Distributor dstb; + std::pair extensions; - while (true) { - float ctrl; - { - ScopedLock lock(); - ctrl = cmd_ctrl; - } + // // Control Trigger + // DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) + // while(ctrl_trigger.read() == 1) { + // ThisThread::sleep_for(10ms); + // } + // pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); + // led.write(0); + - std::pair spoolExtensions = distributor.getMotorOutputs(ctrl); - float extension = spoolExtensions.first; - if (extension > 0.5) led.write(1); - else led.write(0); - - update_struct(1.0, 2.0, 3.0, 4.0); - - ThisThread::sleep_for(100ms); + // CONTROL SEQUENCE: + // ctrl(cmd1, time (seconds), ...) + // cmd = -1 is full left, 1 is full right + ctrl(0.25, 60, &motor1, &motor2, &dstb, &pc); + ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); + ctrl(0.75, 60, &motor1, &motor2, &dstb, &pc); + ctrl(1, 120, &motor1, &motor2, &dstb, &pc); + ctrl(0, 60, &motor1, &motor2, &dstb, &pc); + ctrl(-0.5, 60, &motor1, &motor2, &dstb, &pc); + ctrl(-1, 60, &motor1, &motor2, &dstb, &pc); + while (true) { + ctrl(1, 3600, &motor1, &motor2, &dstb, &pc); } -} +} \ No newline at end of file From 707e3008a60c84b3b3ab44104b0e1d429aa46c3a Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 8 Jan 2026 18:57:13 -0800 Subject: [PATCH 25/47] Cleaned up motorCOTScpp --- MotorCOTS.cpp | 47 +++++++++++++---------------------------------- 1 file changed, 13 insertions(+), 34 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index c2b959a..eeaf500 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -13,44 +13,32 @@ void MotorCOTS::updateGlobals() { void MotorCOTS::aRiseCallback() { aUp = true; - if (!bUp) { - encoderCounter++; - } else { - encoderCounter--; - } + if (!bUp) encoderCounter++; + else encoderCounter--; } // Tickes encoder count forward or backward on encoder b rise callback void MotorCOTS::bRiseCallback() { bUp = true; - if (aUp) { - encoderCounter++; - } else { - encoderCounter--; - } + if (aUp) encoderCounter++; + else encoderCounter--; } // Tickes encoder count forward or backward on encoder a fall callback void MotorCOTS::aFallCallback() { aUp = false; - if (bUp) { - encoderCounter++; - } else { - encoderCounter--; - } + if (bUp) encoderCounter++; + else encoderCounter--; } // Tickes encoder count forward or backward on encoder b fall callback void MotorCOTS::bFallCallback() { bUp = false; - if (!aUp) { - encoderCounter++; - } else { - encoderCounter--; - } + if (!aUp) encoderCounter++; + else encoderCounter--; } @@ -64,13 +52,8 @@ MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerTh encoderA.mode(PullDown); encoderB.mode(PullDown); - if (encoderA.read() == 1) { - - aUp = true; - } - if (encoderB.read() == 1) { - bUp = true; - } + if (encoderA.read() == 1) aUp = true; + if (encoderB.read() == 1) bUp = true; powerPositive = true; @@ -82,7 +65,6 @@ MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerTh } - // Set motor direction // Takes: int direction (1 for forward, -1 for backward) void MotorCOTS::direction(int direction) { @@ -133,14 +115,11 @@ void MotorCOTS::toPosition(float pullPercent, int dt) { float power = pid->compute(currPos, targetPos, dt); - if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) { - motorPower(0); - } else { - motorPower(-power); - } + if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) motorPower(0); + else motorPower(-power); // Debug print statements - pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); + // pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); // pc->printf("\tTarget pull percent: %f", pullPercent); // pc-printf("\tMax deflection: %f", MAX_DEFLECTION); // pc->printf("Difference: %f\t", currPos-targetPos); From b2d4d4d31eba3a3539726a438e0c92a6bc9dff74 Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 8 Jan 2026 19:44:23 -0800 Subject: [PATCH 26/47] changed control logic --- Distributor.cpp | 4 +- Distributor.h | 5 +-- MotorCOTS.cpp | 13 +++--- MotorCOTS.h | 8 ++-- main.cpp | 107 +++++++++++++++++++++++++----------------------- 5 files changed, 69 insertions(+), 68 deletions(-) diff --git a/Distributor.cpp b/Distributor.cpp index 52421e6..104adf8 100644 --- a/Distributor.cpp +++ b/Distributor.cpp @@ -5,9 +5,9 @@ Distributor::Distributor() {} -// Takes a flot [-1, 1] indicatng full left to full right steering respectively +// Takes a float [-1, 1] indicating full left to full right steering respectively // Returns a tuple of floats representing the retraction of the left motor and right motor respectively -std::pair Distributor::getMotorOutputsManual(float ctrl) { +std::pair Distributor::getMotorOutputs(float ctrl) { float leftPull = 0.5 * (-ctrl) + 0.5; float rightPull = 0.5 * ctrl + 0.5; diff --git a/Distributor.h b/Distributor.h index ed77cad..96968f9 100644 --- a/Distributor.h +++ b/Distributor.h @@ -3,17 +3,14 @@ #ifndef _DISTRIBUTOR_H_ #define _DISTRIBUTOR_H_ - #include "mbed.h" -#define DISTRIBUTOR_BUFFER_SIZE 8 - class Distributor { public: Distributor(); - std::pair getMotorOutputsManual(float ctrl); + std::pair getMotorOutputs(float ctrl); }; diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index eeaf500..a8f14d7 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -83,7 +83,7 @@ void MotorCOTS::direction(int direction) { // Set motor power // Takes: float power (0 to 1 for forward, 0 to -1 for backward) -void MotorCOTS::motorPower(float power) { +void MotorCOTS::motorPower() { if (power > 0) { direction(1); powerThrottle.write(power); @@ -108,19 +108,22 @@ float MotorCOTS::getPosition() { // Moves motor to target position using PID controller // Takes: float target position in inches, int dt in milliseconds since last call -void MotorCOTS::toPosition(float pullPercent, int dt) { +float MotorCOTS::toPosition(float pullPercent, int dt) { float currPos = getPosition(); float targetPos = pullPercent*MAX_DEFLECTION; - float power = pid->compute(currPos, targetPos, dt); + power = pid->compute(currPos, targetPos, dt); - if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) motorPower(0); - else motorPower(-power); + if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) power = 0; + + motorPower(); // Debug print statements // pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); // pc->printf("\tTarget pull percent: %f", pullPercent); // pc-printf("\tMax deflection: %f", MAX_DEFLECTION); // pc->printf("Difference: %f\t", currPos-targetPos); + + return power; } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index b656ebb..92ce2b6 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -34,6 +34,7 @@ class MotorCOTS { float angle; float rotations; bool powerPositive; + float power; void aRiseCallback(); void bRiseCallback(); @@ -47,19 +48,16 @@ class MotorCOTS { void updateGlobals(); - - - // MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid); MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid, EUSBSerial* pc); void direction(int direction); - void motorPower(float power); + void motorPower(); int getDegrees(); float getPosition(); - void toPosition(float target, int dt); + float toPosition(float target, int dt); }; #endif //_MOTORCOTS_H_ \ No newline at end of file diff --git a/main.cpp b/main.cpp index 4401a97..843b36b 100644 --- a/main.cpp +++ b/main.cpp @@ -6,19 +6,31 @@ #include "PID.h" #include "Distributor.h" +#define MCPS_ADDR 0x02 << 1 +#define LOOP_PERIOD_MS 20 + +// Debug Led DigitalOut led(PC_13); + +// Serial EUSBSerial pc; + +// Object init +PID pid(1, 0, 5); + +// New Driver board pinout +MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A +MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B +Distributor dstb; -int leftExtension = 0; -int rightExtension = 0; +// Defleciton cmds +std::pair extensions; Thread i2cThread; // setup thread char i2c_tx_buf[32]; char i2c_rx_buf[32]; -#define MCPS_ADDR 0x02 << 1 - Mutex mutex; float cmd_ctrl = 0; @@ -29,7 +41,7 @@ struct { float rightPower; } motorPacket; -void update_struct(float leftDegrees, float rightDegrees, float leftPower, float rightPower) { +void update_motorPacket(float leftDegrees, float rightDegrees, float leftPower, float rightPower) { ScopedLock lock(mutex); motorPacket.leftDegrees = leftDegrees; motorPacket.rightDegrees = rightDegrees; @@ -53,7 +65,7 @@ void i2c_handler(void) { int err = slave.read(i2c_rx_buf, sizeof(float)); { - ScopedLock lock(); + ScopedLock lock(mutex); memcpy(&cmd_ctrl, i2c_rx_buf, sizeof(float)); } @@ -63,12 +75,11 @@ void i2c_handler(void) { case I2CSlave::ReadAddressed: { { - ScopedLock lock(); + ScopedLock lock(mutex); memcpy(i2c_tx_buf, &motorPacket, sizeof(motorPacket)); } slave.write(i2c_tx_buf, sizeof(motorPacket)); - break; } @@ -77,6 +88,8 @@ void i2c_handler(void) { } } +/* -------- DEBUG --------- */ + // Steers ARES in a set turn angle for a set ammount of time // Takes: float for steering (-1 full left 1 full right), an int of seconds to hold for, two motor pointer // for the left and right motors, a distributor pointer, and a USB serial pointer @@ -84,7 +97,7 @@ void ctrl(float cmd, int seconds, MotorCOTS* motor1, MotorCOTS* motor2, Distribu Timer t; t.start(); std::pair extensions; - extensions = dstb->getMotorOutputsManual(cmd); + extensions = dstb->getMotorOutputs(cmd); while (t.read_ms() < 1000*seconds) { pc->printf("\tcmd: %f", cmd); motor1->toPosition(extensions.first, 10); @@ -109,55 +122,45 @@ void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCO } } +void led_if_deflection_pos(float ctrl) { + if (ctrl > 0) led.write(1); + else led.write(0); +} +/* ------------------------- */ int main() { - // Debug LED - DigitalOut led(PC_13); - - // USB serial init - EUSBSerial pc; - ThisThread::sleep_for(1s); - pc.printf("\nSerial Port Connected!\n"); - led.write(1); - - - // Object init - PID pid(1, 0, 5); - // Direction 1, direction 2, throttle, encoder a, encoder b - // MotorCOTS motor1(PB_8, PB_9, PA_1, PA_6, PA_7, &pid, &pc); - // MotorCOTS motor2(PA_10, PA_9, PA_8, PA_15, PB_3, &pid, &pc); - // New Driver board pinout - MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A - MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B - Distributor dstb; - std::pair extensions; - - - - // // Control Trigger - // DigitalIn ctrl_trigger(PB_7); // SDA1 (pulled high by default, trigger is low) - // while(ctrl_trigger.read() == 1) { - // ThisThread::sleep_for(10ms); - // } - // pc.printf("\nTHE THING WAS TRIGGERED...\n STARTING CONTROL SEQUENCE..."); - // led.write(0); - - + Timer t; - // CONTROL SEQUENCE: - // ctrl(cmd1, time (seconds), ...) - // cmd = -1 is full left, 1 is full right - ctrl(0.25, 60, &motor1, &motor2, &dstb, &pc); - ctrl(0.5, 60, &motor1, &motor2, &dstb, &pc); - ctrl(0.75, 60, &motor1, &motor2, &dstb, &pc); - ctrl(1, 120, &motor1, &motor2, &dstb, &pc); - ctrl(0, 60, &motor1, &motor2, &dstb, &pc); - ctrl(-0.5, 60, &motor1, &motor2, &dstb, &pc); - ctrl(-1, 60, &motor1, &motor2, &dstb, &pc); while (true) { - ctrl(1, 3600, &motor1, &motor2, &dstb, &pc); + t.start(); + + float ctrl; + + { // Safely grab commanded ctrl + ScopedLock lock(mutex); + ctrl = cmd_ctrl; + } + + // RUN COMMANDS + /* ext.first -> left : ext.second -> right */ + extensions = dstb.getMotorOutputs(ctrl); + float lpower = motor1.toPosition(extensions.first, 10); // Left cmd + float rpower = motor2.toPosition(extensions.second, 10); // Right cmd + + led_if_deflection_pos(ctrl); + + // UPDATE FLASH DATA + update_motorPacket( + motor1.getDegrees(), // Left Motor + motor2.getDegrees(), // Right Motor + lpower, + rpower + ); + + // Event Scheduling + if(t.read_ms() < LOOP_PERIOD_MS) ThisThread::sleep_for(LOOP_PERIOD_MS - t.read_ms()); } } \ No newline at end of file From 853f5ba2c2bbb4e3b1146551d12f44b5d5b9e9d7 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Fri, 9 Jan 2026 17:03:59 -0800 Subject: [PATCH 27/47] i2c thread tweaks --- main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/main.cpp b/main.cpp index 843b36b..498d51d 100644 --- a/main.cpp +++ b/main.cpp @@ -132,6 +132,7 @@ void led_if_deflection_pos(float ctrl) { int main() { Timer t; + i2cThread.start(i2c_handler); while (true) { t.start(); From aeea7c2f71f92e6676e1daf37c3c587a7f8e531e Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Fri, 9 Jan 2026 17:53:44 -0800 Subject: [PATCH 28/47] Sigma --- MotorCOTS.cpp | 4 ++-- MotorCOTS.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index a8f14d7..73cd6bd 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -96,9 +96,9 @@ void MotorCOTS::motorPower() { } // Returns motor angle in degrees -int MotorCOTS::getDegrees() { +float MotorCOTS::getDegrees() { updateGlobals(); - return (int)angle; + return angle; } // Returns motor linear position in inches diff --git a/MotorCOTS.h b/MotorCOTS.h index 92ce2b6..a7a6737 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -54,7 +54,7 @@ class MotorCOTS { void direction(int direction); void motorPower(); - int getDegrees(); + float getDegrees(); float getPosition(); float toPosition(float target, int dt); From b6fb433d19482593af5f4a1c47bb6b4e22917fc2 Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Mon, 12 Jan 2026 19:58:34 -0800 Subject: [PATCH 29/47] Message --- MotorCOTS.cpp | 2 +- PID.cpp | 7 ++----- main.cpp | 12 +++++++----- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 73cd6bd..36b3d4a 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -113,7 +113,7 @@ float MotorCOTS::toPosition(float pullPercent, int dt) { float currPos = getPosition(); float targetPos = pullPercent*MAX_DEFLECTION; - power = pid->compute(currPos, targetPos, dt); + power = -pid->compute(currPos, targetPos, dt); if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) power = 0; diff --git a/PID.cpp b/PID.cpp index 00b4086..e266a99 100644 --- a/PID.cpp +++ b/PID.cpp @@ -31,10 +31,7 @@ float PID::compute(float currAngle, float targetAngle, float dt) { float D = -Kd * (error - errorLast) / dt; errorLast = error; int output = P + I + D; - if (output > 1) { - return 1; - } else if (output < -1) { - return -1; - } + if (output > 1) return 1; + else if (output < -1) return -1; return output; } \ No newline at end of file diff --git a/main.cpp b/main.cpp index 498d51d..481b77c 100644 --- a/main.cpp +++ b/main.cpp @@ -137,6 +137,8 @@ int main() { while (true) { t.start(); + + // COMMS float ctrl; { // Safely grab commanded ctrl @@ -146,7 +148,7 @@ int main() { // RUN COMMANDS /* ext.first -> left : ext.second -> right */ - extensions = dstb.getMotorOutputs(ctrl); + extensions = dstb.getMotorOutputs(0.0); float lpower = motor1.toPosition(extensions.first, 10); // Left cmd float rpower = motor2.toPosition(extensions.second, 10); // Right cmd @@ -154,10 +156,10 @@ int main() { // UPDATE FLASH DATA update_motorPacket( - motor1.getDegrees(), // Left Motor - motor2.getDegrees(), // Right Motor - lpower, - rpower + motor1.getPosition(), // Left Motor + motor2.getPosition(), // Right Motor + extensions.first, + extensions.second ); // Event Scheduling From 2ec9dddcaacb7128dbf24c7577eae9b4ac5bcfa5 Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 15 Jan 2026 22:48:47 -0800 Subject: [PATCH 30/47] lighter mutexes: --- PID.cpp | 2 +- main.cpp | 31 +++++++++++-------------------- 2 files changed, 12 insertions(+), 21 deletions(-) diff --git a/PID.cpp b/PID.cpp index e266a99..a266f31 100644 --- a/PID.cpp +++ b/PID.cpp @@ -31,7 +31,7 @@ float PID::compute(float currAngle, float targetAngle, float dt) { float D = -Kd * (error - errorLast) / dt; errorLast = error; int output = P + I + D; - if (output > 1) return 1; + if (output > 1) return 1; else if (output < -1) return -1; return output; } \ No newline at end of file diff --git a/main.cpp b/main.cpp index 481b77c..e872fe7 100644 --- a/main.cpp +++ b/main.cpp @@ -29,10 +29,9 @@ std::pair extensions; Thread i2cThread; // setup thread char i2c_tx_buf[32]; -char i2c_rx_buf[32]; Mutex mutex; -float cmd_ctrl = 0; +volatile float cmd_ctrl; struct { float leftDegrees; @@ -61,24 +60,17 @@ void i2c_handler(void) { switch(event) { case I2CSlave::WriteAddressed: { + + char temp [sizeof(float)]; + int err = slave.read(temp, sizeof(float)); - int err = slave.read(i2c_rx_buf, sizeof(float)); - - { - ScopedLock lock(mutex); - memcpy(&cmd_ctrl, i2c_rx_buf, sizeof(float)); - } + if(err == 0) memcpy((void*)&cmd_ctrl, temp, sizeof(float)); break; } case I2CSlave::ReadAddressed: { - - { - ScopedLock lock(mutex); - memcpy(i2c_tx_buf, &motorPacket, sizeof(motorPacket)); - } - + memcpy(i2c_tx_buf, &motorPacket, sizeof(motorPacket)); slave.write(i2c_tx_buf, sizeof(motorPacket)); break; } @@ -137,18 +129,17 @@ int main() { while (true) { t.start(); - // COMMS float ctrl; - { // Safely grab commanded ctrl + { ScopedLock lock(mutex); - ctrl = cmd_ctrl; - } - + ctrl = cmd_ctrl; + } + // RUN COMMANDS /* ext.first -> left : ext.second -> right */ - extensions = dstb.getMotorOutputs(0.0); + extensions = dstb.getMotorOutputs(ctrl); float lpower = motor1.toPosition(extensions.first, 10); // Left cmd float rpower = motor2.toPosition(extensions.second, 10); // Right cmd From 0fe443ff0e40f7c551647a9a856cee0c1071a9aa Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Thu, 15 Jan 2026 23:39:25 -0800 Subject: [PATCH 31/47] I fixed it --- MotorCOTS.cpp | 6 +++--- PID.cpp | 6 +++--- main.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 36b3d4a..8bcde3a 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -69,12 +69,12 @@ MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerTh // Takes: int direction (1 for forward, -1 for backward) void MotorCOTS::direction(int direction) { if (direction == 1) { - ThisThread::sleep_for(2ms); + ThisThread::sleep_for(20ms); directionTwo.write(0); directionOne.write(1); powerPositive = true; } else if (direction == -1) { - ThisThread::sleep_for(2ms); + ThisThread::sleep_for(20ms); directionOne.write(0); directionTwo.write(1); powerPositive = false; @@ -115,7 +115,7 @@ float MotorCOTS::toPosition(float pullPercent, int dt) { power = -pid->compute(currPos, targetPos, dt); - if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) power = 0; + if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) power = 0.0f; motorPower(); diff --git a/PID.cpp b/PID.cpp index a266f31..e065e90 100644 --- a/PID.cpp +++ b/PID.cpp @@ -30,8 +30,8 @@ float PID::compute(float currAngle, float targetAngle, float dt) { float I = -Ki * integralError; float D = -Kd * (error - errorLast) / dt; errorLast = error; - int output = P + I + D; - if (output > 1) return 1; - else if (output < -1) return -1; + float output = P + I + D; + if (output > 1.0f) return 1.0f; + else if (output < -1.0f) return -1.0f; return output; } \ No newline at end of file diff --git a/main.cpp b/main.cpp index e872fe7..15bb8b8 100644 --- a/main.cpp +++ b/main.cpp @@ -127,15 +127,16 @@ int main() { i2cThread.start(i2c_handler); while (true) { + t.reset(); t.start(); // COMMS float ctrl; - { - ScopedLock lock(mutex); - ctrl = cmd_ctrl; - } + // { + // ScopedLock lock(mutex); + ctrl = cmd_ctrl; + // } // RUN COMMANDS /* ext.first -> left : ext.second -> right */ From cd54b3a5aa5a09928d24fa135fd19036b1a6af5c Mon Sep 17 00:00:00 2001 From: LukeVerlan Date: Fri, 16 Jan 2026 00:13:19 -0800 Subject: [PATCH 32/47] made it atomic, fixes deadlocking --- main.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/main.cpp b/main.cpp index 15bb8b8..cfb0ab6 100644 --- a/main.cpp +++ b/main.cpp @@ -5,6 +5,7 @@ #include "EUSBSerial.h" #include "PID.h" #include "Distributor.h" +#include #define MCPS_ADDR 0x02 << 1 #define LOOP_PERIOD_MS 20 @@ -31,7 +32,7 @@ Thread i2cThread; // setup thread char i2c_tx_buf[32]; Mutex mutex; -volatile float cmd_ctrl; +std::atomic cmd_ctrl{0.0f}; struct { float leftDegrees; @@ -61,11 +62,12 @@ void i2c_handler(void) { case I2CSlave::WriteAddressed: { - char temp [sizeof(float)]; - int err = slave.read(temp, sizeof(float)); - - if(err == 0) memcpy((void*)&cmd_ctrl, temp, sizeof(float)); - + char temp[sizeof(float)]; + if(slave.read(temp, sizeof(float)) == 0) { + float val; + memcpy(&val, temp, sizeof(float)); + cmd_ctrl.store(val); // Atomic non-blocking store + } break; } @@ -131,12 +133,7 @@ int main() { t.start(); // COMMS - float ctrl; - - // { - // ScopedLock lock(mutex); - ctrl = cmd_ctrl; - // } + float ctrl = cmd_ctrl.load(); // RUN COMMANDS /* ext.first -> left : ext.second -> right */ From bf27a3ca7fb52b6f0e6b6e7e2118ab1bcde6e868 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Mon, 2 Feb 2026 19:56:21 -0800 Subject: [PATCH 33/47] finished commenting --- MotorCOTS.cpp | 9 +++------ MotorCOTS.h | 3 +-- PID.h | 2 +- main.cpp | 20 ++++++++++++-------- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/MotorCOTS.cpp b/MotorCOTS.cpp index 8bcde3a..7da2c12 100644 --- a/MotorCOTS.cpp +++ b/MotorCOTS.cpp @@ -42,6 +42,8 @@ void MotorCOTS::bFallCallback() { } + + // Initialize motor object, set encoder states // Takes: PinName for direction one control line, PinName for direction two control line, PinName for power throttle (PWM), // PinName for encoder A, PinName for encoder B, PID pointer for motor PID controller, EUSBSerial pointer for debugging serial @@ -65,6 +67,7 @@ MotorCOTS::MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerTh } + // Set motor direction // Takes: int direction (1 for forward, -1 for backward) void MotorCOTS::direction(int direction) { @@ -118,12 +121,6 @@ float MotorCOTS::toPosition(float pullPercent, int dt) { if (currPos-targetPos < 0.5 && currPos-targetPos > -0.5) power = 0.0f; motorPower(); - - // Debug print statements - // pc->printf("\t\tCurrent: %f\tTarget: %f", currPos, targetPos); - // pc->printf("\tTarget pull percent: %f", pullPercent); - // pc-printf("\tMax deflection: %f", MAX_DEFLECTION); - // pc->printf("Difference: %f\t", currPos-targetPos); return power; } \ No newline at end of file diff --git a/MotorCOTS.h b/MotorCOTS.h index a7a6737..0dc5869 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,7 +27,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 54.0f; // INCHES + const float MAX_DEFLECTION = 12.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; @@ -48,7 +48,6 @@ class MotorCOTS { void updateGlobals(); - // MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid); MotorCOTS(PinName directionOne, PinName directionTwo, PinName powerThrottle, PinName PINA, PinName PINB, PID* pid, EUSBSerial* pc); void direction(int direction); diff --git a/PID.h b/PID.h index 3a00100..fd2df49 100644 --- a/PID.h +++ b/PID.h @@ -15,7 +15,7 @@ class PID { float Kp; float Ki; float Kd; - float setpoint; + float setpoint; float errorLast; float integralError; bool positiveLast; diff --git a/main.cpp b/main.cpp index cfb0ab6..72c9805 100644 --- a/main.cpp +++ b/main.cpp @@ -16,19 +16,21 @@ DigitalOut led(PC_13); // Serial EUSBSerial pc; -// Object init -PID pid(1, 0, 5); +// Initializing the PID controller for both motors +PID pid(0.1, 0, 5); -// New Driver board pinout -MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); //motor A -MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); //motor B +MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A +MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B Distributor dstb; -// Defleciton cmds +// Relative extensions for both motors +// 0 = retracted, 1 = fully exteded std::pair extensions; -Thread i2cThread; // setup thread +Thread i2cThread; +// Buffer for I2C transmission +// 32 bytes for 4 floats (motor positions and powers) char i2c_tx_buf[32]; Mutex mutex; @@ -52,6 +54,7 @@ void update_motorPacket(float leftDegrees, float rightDegrees, float leftPower, // SDA, SCL I2CSlave slave(PB_7, PB_6); +// I2C handler thread takes motor control inputs and upon request sends motor status packet void i2c_handler(void) { slave.address(MCPS_ADDR); @@ -82,7 +85,6 @@ void i2c_handler(void) { } } -/* -------- DEBUG --------- */ // Steers ARES in a set turn angle for a set ammount of time // Takes: float for steering (-1 full left 1 full right), an int of seconds to hold for, two motor pointer @@ -116,11 +118,13 @@ void ctrl_manual(float cmd1, float cmd2, int seconds, MotorCOTS* motor1, MotorCO } } +// LED debug indicator method void led_if_deflection_pos(float ctrl) { if (ctrl > 0) led.write(1); else led.write(0); } + /* ------------------------- */ int main() { From 41cdfe20d0809222e1485c7e5af426a055e533eb Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Tue, 10 Feb 2026 09:21:00 -0800 Subject: [PATCH 34/47] Removed EUSBSerial --- EUSBSerial/EUSBSerial.cpp | 123 -------------------------------------- EUSBSerial/EUSBSerial.h | 54 ----------------- 2 files changed, 177 deletions(-) delete mode 100644 EUSBSerial/EUSBSerial.cpp delete mode 100644 EUSBSerial/EUSBSerial.h diff --git a/EUSBSerial/EUSBSerial.cpp b/EUSBSerial/EUSBSerial.cpp deleted file mode 100644 index 0995bcd..0000000 --- a/EUSBSerial/EUSBSerial.cpp +++ /dev/null @@ -1,123 +0,0 @@ -#include "EUSBSerial.h" - -//MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE - - -EUSBSerial::EUSBSerial(uint16_t vid, uint16_t pid) : pc(false, vid, pid) { - -} - -EUSBSerial::~EUSBSerial() { - pc.disconnect(); -} - -bool EUSBSerial::printf(const char* format, ...) { - if (!pc.connected()) - return false; - - Thread t1; - Timer t; - - va_list args; - - va_start(args, format); - this->_format = format; - this->_args = args; - va_end(args); - - t1.start([this]() { _printf(); }); - t.start(); - - while (!this->_success) { - if (t.read_ms() > 500) { - pc.disconnect(); - return false; - } - } - - t1.join(); - - return true; -} - - -void EUSBSerial::_printf() { - this->_success = false; - - char buffer[MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE]; - - size_t n = vsnprintf (buffer, MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE, _format, _args); - - if (n > 0 && n < MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE) { - pc.write(buffer, n); - this->_success = true; - } -} - - -bool EUSBSerial::write(const char* buf, size_t size) { - if (!pc.connected()) - return false; - - Thread t1; - Timer t; - - this->_format = buf; - this->_size = size; - - t1.start([this]() { _write(); }); - t.start(); - - while (!this->_success) { - if (t.read_ms() > 500) { - pc.disconnect(); - return false; - } - } - - t1.join(); - return true; -} - -void EUSBSerial::_write() { - this->_success = false; - - pc.write(this->_format, this->_size); - - this->_success = true; -} - -char EUSBSerial::_getc() { - return pc._getc(); -} - -size_t EUSBSerial::available() { - return pc.available(); -} - -bool EUSBSerial::connected() { - return this->write("\n", 1); -} - -bool EUSBSerial::readline(char* buf, size_t size) { - Timer t; - t.start(); - - size_t i = 0; - while (pc.available()) { - if (i == size) - return false; - - buf[i] = this->_getc(); - if (buf[i] == '\n') { - buf[i] = 0; - return true; - } - i ++; - - if (t.read_ms() > 500) - return false; - } - - return false; -} \ No newline at end of file diff --git a/EUSBSerial/EUSBSerial.h b/EUSBSerial/EUSBSerial.h deleted file mode 100644 index c42c5b8..0000000 --- a/EUSBSerial/EUSBSerial.h +++ /dev/null @@ -1,54 +0,0 @@ -/* EUSBSerial - * Ethan Armstrong - * warmst@uw.edu - * ------------------------------------------- - * Extended USB Serial (EUSBSerial) is a class that extends - * the functionality of the standard mbed USBSerial class - * found in USBSerial.h. Features as follows: - * formatted print support (printf) - * newline terminated read support (readline) - * Redundant USB Connection monitoring and protection - * - * EUSBSerial implements QOL features while encapsulating - * writing and reading functions within a seperate thread - * This allows write and read operations to return an error - * rather than block the main thread, or crash the STM -*/ -#include "mbed.h" -#include "USBSerial.h" -#include - -#ifndef MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE -#define MBED_CONF_EUSBSERIAL_MAX_PACKET_SIZE 2048 -#endif - -#ifndef _E_USB_SERIAL_H_ -#define _E_USB_SERIAL_H_ - -class EUSBSerial { -public: - EUSBSerial(uint16_t vid=0x1F00, uint16_t pid=0x2012); - ~EUSBSerial(); - - bool printf(const char* format, ...); - bool write(const char* buf, size_t size); - - bool readline(char* buf, size_t size); - - size_t available(); - char _getc(); - - bool connected(); - -private: - const char* _format; - va_list _args; - size_t _size; - bool _success; - USBSerial pc; - - void _printf(); - void _write(); -}; - -#endif //_E_USB_SERIAL_H_ \ No newline at end of file From 230f575ec5bdd67d119102a1858842469485757f Mon Sep 17 00:00:00 2001 From: ollieb393 <62722139+ollieb393@users.noreply.github.com> Date: Tue, 10 Feb 2026 09:23:31 -0800 Subject: [PATCH 35/47] add EUSBSerial as submodule --- .gitmodules | 3 +++ EUSBSerial | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 EUSBSerial diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..ecf90b0 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "EUSBSerial"] + path = EUSBSerial + url = https://github.com/SARP-ARES/EUSBSerial diff --git a/EUSBSerial b/EUSBSerial new file mode 160000 index 0000000..f4c7009 --- /dev/null +++ b/EUSBSerial @@ -0,0 +1 @@ +Subproject commit f4c7009c5f146661f623f2f1e9a7c40e378e105b From 01cfbb4e1a095337fc987f7229d05772073cdc63 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Tue, 24 Feb 2026 01:01:18 -0800 Subject: [PATCH 36/47] Added deadzone --- PID.cpp | 6 +++++- PID.h | 3 ++- main.cpp | 2 +- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/PID.cpp b/PID.cpp index e065e90..cdb728c 100644 --- a/PID.cpp +++ b/PID.cpp @@ -3,13 +3,14 @@ // Initialize PID controller // Takes: float for proportional constant, float for integral constant // float derivative constant -PID::PID(float Kp, float Ki, float Kd) { +PID::PID(float Kp, float Ki, float Kd, float deadzone) { positiveLast = true; errorLast = 0; integralError = 0; this->Kp = Kp; this->Ki = Ki; this->Kd = Kd; + this->deadzone = deadzone; } // Compute PID output @@ -33,5 +34,8 @@ float PID::compute(float currAngle, float targetAngle, float dt) { float output = P + I + D; if (output > 1.0f) return 1.0f; else if (output < -1.0f) return -1.0f; + if (output < this->deadzone) { + output = 0; + } return output; } \ No newline at end of file diff --git a/PID.h b/PID.h index fd2df49..fc91885 100644 --- a/PID.h +++ b/PID.h @@ -8,7 +8,7 @@ class PID { public: - PID(float Kp, float Ki, float Kd); + PID(float Kp, float Ki, float Kd, float deadzone); float compute(float currAngle, float targetAngle, float dt); private: @@ -19,6 +19,7 @@ class PID { float errorLast; float integralError; bool positiveLast; + float deadzone; }; diff --git a/main.cpp b/main.cpp index 72c9805..5a7c6a4 100644 --- a/main.cpp +++ b/main.cpp @@ -17,7 +17,7 @@ DigitalOut led(PC_13); EUSBSerial pc; // Initializing the PID controller for both motors -PID pid(0.1, 0, 5); +PID pid(0.1, 0, 5, 0.05); MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B From 9b88f613ce714fe4b01edc156019c11b62260a07 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Fri, 27 Feb 2026 14:56:12 -0800 Subject: [PATCH 37/47] changed max defleciton to 40in --- MotorCOTS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index 0dc5869..e9c13d3 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,7 +27,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 12.0f; // INCHES + const float MAX_DEFLECTION = 40.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; From 6687f4406e2e8f6d271b8e35083fbc3470c217e7 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 01:58:21 -0800 Subject: [PATCH 38/47] caught default ctrl value to avoid motors spinning on startup --- MotorCOTS.h | 4 ++-- main.cpp | 23 ++++++++++++++++++----- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index e9c13d3..7c02213 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,14 +27,14 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 40.0f; // INCHES + const float MAX_DEFLECTION = 30.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; float angle; float rotations; bool powerPositive; - float power; + float power; void aRiseCallback(); void bRiseCallback(); diff --git a/main.cpp b/main.cpp index 5a7c6a4..cb25bef 100644 --- a/main.cpp +++ b/main.cpp @@ -23,6 +23,9 @@ MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B Distributor dstb; + +const float DEFAULT_CTRL_VALUE = 999.0f; + // Relative extensions for both motors // 0 = retracted, 1 = fully exteded std::pair extensions; @@ -34,7 +37,7 @@ Thread i2cThread; char i2c_tx_buf[32]; Mutex mutex; -std::atomic cmd_ctrl{0.0f}; +std::atomic cmd_ctrl{DEFAULT_CTRL_VALUE}; struct { float leftDegrees; @@ -43,6 +46,14 @@ struct { float rightPower; } motorPacket; + +bool is_nan_safe(float f) { + uint32_t i; + memcpy(&i, &f, sizeof(i)); + return (i & 0x7F800000) == 0x7F800000 && (i & 0x007FFFFF) != 0; +} + + void update_motorPacket(float leftDegrees, float rightDegrees, float leftPower, float rightPower) { ScopedLock lock(mutex); motorPacket.leftDegrees = leftDegrees; @@ -141,9 +152,11 @@ int main() { // RUN COMMANDS /* ext.first -> left : ext.second -> right */ - extensions = dstb.getMotorOutputs(ctrl); - float lpower = motor1.toPosition(extensions.first, 10); // Left cmd - float rpower = motor2.toPosition(extensions.second, 10); // Right cmd + if (ctrl != DEFAULT_CTRL_VALUE) { + extensions = dstb.getMotorOutputs(ctrl); + float lpower = motor1.toPosition(extensions.first, 10); // Left cmd + float rpower = motor2.toPosition(extensions.second, 10); // Right cmd + } led_if_deflection_pos(ctrl); @@ -157,6 +170,6 @@ int main() { // Event Scheduling if(t.read_ms() < LOOP_PERIOD_MS) ThisThread::sleep_for(LOOP_PERIOD_MS - t.read_ms()); - } + } } \ No newline at end of file From de6815a429a0c405fd6b1cd86e709514131a6ec4 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 02:05:32 -0800 Subject: [PATCH 39/47] changing max deflection for car test --- MotorCOTS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index 7c02213..2f56a5c 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,7 +27,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 30.0f; // INCHES + const float MAX_DEFLECTION = 10.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; From a623adda5a7bc12901c12d95644eaebf75419d47 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 13:37:22 -0800 Subject: [PATCH 40/47] swapped motor 1 and motor 2, now correctly executes right and left turns --- main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/main.cpp b/main.cpp index cb25bef..1833ebe 100644 --- a/main.cpp +++ b/main.cpp @@ -19,8 +19,9 @@ EUSBSerial pc; // Initializing the PID controller for both motors PID pid(0.1, 0, 5, 0.05); -MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A -MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B +MotorCOTS motor1(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B () +MotorCOTS motor2(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A () + Distributor dstb; From 37b38dfb9429c14888148f1ac21ee7578dc6f714 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 13:38:52 -0800 Subject: [PATCH 41/47] swapped motors 1 and 2, now correctly executes left and right turns --- main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/main.cpp b/main.cpp index cb25bef..4071bf0 100644 --- a/main.cpp +++ b/main.cpp @@ -19,8 +19,9 @@ EUSBSerial pc; // Initializing the PID controller for both motors PID pid(0.1, 0, 5, 0.05); -MotorCOTS motor1(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A -MotorCOTS motor2(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B +MotorCOTS motor1(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B +MotorCOTS motor2(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A + Distributor dstb; From 4ac5c59134086e8cf59eead316b2c7f5a21db6f8 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 13:40:35 -0800 Subject: [PATCH 42/47] comments --- main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/main.cpp b/main.cpp index 1833ebe..e15a157 100644 --- a/main.cpp +++ b/main.cpp @@ -153,6 +153,7 @@ int main() { // RUN COMMANDS /* ext.first -> left : ext.second -> right */ + // check for the default value so that motors don't spin until a command is received from the flight computer if (ctrl != DEFAULT_CTRL_VALUE) { extensions = dstb.getMotorOutputs(ctrl); float lpower = motor1.toPosition(extensions.first, 10); // Left cmd From 08d515eab1b1d0f7136b9bd23deb83b89ecf1a6e Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Sat, 28 Feb 2026 13:41:10 -0800 Subject: [PATCH 43/47] clarifying comment in main loop --- main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/main.cpp b/main.cpp index 4071bf0..093a8cd 100644 --- a/main.cpp +++ b/main.cpp @@ -153,6 +153,7 @@ int main() { // RUN COMMANDS /* ext.first -> left : ext.second -> right */ + // check for the default value so that motors don't spin until a command is received from the flight computer if (ctrl != DEFAULT_CTRL_VALUE) { extensions = dstb.getMotorOutputs(ctrl); float lpower = motor1.toPosition(extensions.first, 10); // Left cmd From e60a23b0c558a01e420d428f8918dce7e01a778c Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Mon, 2 Mar 2026 14:38:17 -0800 Subject: [PATCH 44/47] added const params for PID --- main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index e15a157..ff9d4ac 100644 --- a/main.cpp +++ b/main.cpp @@ -16,8 +16,13 @@ DigitalOut led(PC_13); // Serial EUSBSerial pc; +const float Kp = 0.2; +const float Ki = 0; +const float Kd = 0.1; +const float DEADZONE = 0.05; + // Initializing the PID controller for both motors -PID pid(0.1, 0, 5, 0.05); +PID pid(Kp, Ki, Kd, DEADZONE); MotorCOTS motor1(PA_6, PA_5, PA_1, PB_8, PB_9, &pid, &pc); // Motor B () MotorCOTS motor2(PB_0, PA_7, PB_1, PC_14, PC_15, &pid, &pc); // Motor A () From cc1ebfe09b2525ad77d0f59cf3cce90a7d24d195 Mon Sep 17 00:00:00 2001 From: Jimmy Fowler <118411457+jimmyfowler@users.noreply.github.com> Date: Mon, 2 Mar 2026 14:40:54 -0800 Subject: [PATCH 45/47] changed max deflection to 24in --- MotorCOTS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index 2f56a5c..b7d23a6 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,7 +27,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 10.0f; // INCHES + const float MAX_DEFLECTION = 24.0f; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position; From 8bfed78a1e738532fda8bd4432b3faeb4a155921 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Mon, 2 Mar 2026 23:11:56 -0800 Subject: [PATCH 46/47] Fixed large deadzone error Just changed P coefficient and some readability lol --- .mbedignore | 1 + PID.cpp | 4 ++-- main.cpp | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) create mode 100644 .mbedignore diff --git a/.mbedignore b/.mbedignore new file mode 100644 index 0000000..909ebcf --- /dev/null +++ b/.mbedignore @@ -0,0 +1 @@ +BLACKPILL_Custom_Target/* \ No newline at end of file diff --git a/PID.cpp b/PID.cpp index cdb728c..3b642bb 100644 --- a/PID.cpp +++ b/PID.cpp @@ -16,8 +16,8 @@ PID::PID(float Kp, float Ki, float Kd, float deadzone) { // Compute PID output // Takes: float current angle, float target angle, float dt in milliseconds since last call // Returns: float output power (-1 to 1) -float PID::compute(float currAngle, float targetAngle, float dt) { - float error = targetAngle - currAngle; +float PID::compute(float curr, float target, float dt) { + float error = target - curr; integralError += error*dt; // todo: solve integral windup later // Reset integralError if the error switches sign diff --git a/main.cpp b/main.cpp index ff9d4ac..766cf77 100644 --- a/main.cpp +++ b/main.cpp @@ -15,8 +15,8 @@ DigitalOut led(PC_13); // Serial EUSBSerial pc; - -const float Kp = 0.2; + +const float Kp = 1.2; const float Ki = 0; const float Kd = 0.1; const float DEADZONE = 0.05; From 2ec1c7dbaa1fb31d437408c916e57da51d3f3940 Mon Sep 17 00:00:00 2001 From: ollieb393 Date: Tue, 3 Mar 2026 00:19:37 -0800 Subject: [PATCH 47/47] Corrected motor MAX_DEFLECTION for 36 inch max extenctions --- MotorCOTS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MotorCOTS.h b/MotorCOTS.h index b7d23a6..08ff99a 100644 --- a/MotorCOTS.h +++ b/MotorCOTS.h @@ -27,7 +27,7 @@ class MotorCOTS { const int countsPerRev = 64; const float gearRatio = 150; // Might need to be updated for different motors const float spoolDiameter = 0.6345; // Spool Diameter IN INCHES. Sidenote in realidad this will not be const - const float MAX_DEFLECTION = 24.0f; // INCHES + const float MAX_DEFLECTION = 30.0; // INCHES const float totalCounts = countsPerRev * gearRatio; // Total counts per rotation of motor output shaft long position;