diff --git a/.gitignore b/.gitignore index 75fe0a3..7468554 100644 --- a/.gitignore +++ b/.gitignore @@ -31,7 +31,6 @@ dist/ downloads/ eggs/ .eggs/ -lib/ lib64/ parts/ sdist/ diff --git a/lib/Messages/src/Messages.hpp b/lib/Messages/src/Messages.hpp new file mode 100644 index 0000000..cff03f7 --- /dev/null +++ b/lib/Messages/src/Messages.hpp @@ -0,0 +1,29 @@ +#ifndef MESSAGES_HPP +#define MESSAGES_HPP + +#include + +#include "pb_encode.h" +#include "pb_decode.h" +#include "urc.pb.h" + +namespace protobuf { + +class Messages { + +public: + static bool decodeRequest(uint8_t *buffer, size_t bufferLen, RequestMessage &requestMessage) { + pb_istream_t istream = pb_istream_from_buffer(buffer, bufferLen); + return pb_decode(&istream, RequestMessage_fields, &requestMessage); + } + + static size_t encodeResponse(uint8_t *buffer, size_t bufferLen, DriveEncodersMessage &driveEncodersMessage) { + pb_ostream_t ostream = pb_ostream_from_buffer(buffer, bufferLen); + pb_encode(&ostream, DriveEncodersMessage_fields, &driveEncodersMessage); + return ostream.bytes_written; + } +}; + +} // namespace protobuf + +#endif \ No newline at end of file diff --git a/lib/SoloCAN/src/SoloCAN.cpp b/lib/SoloCAN/src/SoloCAN.cpp new file mode 100644 index 0000000..dbdbcfb --- /dev/null +++ b/lib/SoloCAN/src/SoloCAN.cpp @@ -0,0 +1,105 @@ +#include "SoloCAN.hpp" + +namespace solo_can { + +SoloCan::SoloCan(FlexCAN_T4 &_can) : can(_can) {} + +void SoloCan::GetPositionFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = POSITION_FEEDBACK_CODE, + .payload = 0 + }; + can.write(createMessage(data)); + delayMicroseconds(200); +} + +void SoloCan::GetSpeedFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = SPEED_FEEDBACK_CODE, + .payload = 0 + }; + can.write(createMessage(data)); + delayMicroseconds(200); +} + +void SoloCan::SetSpeedReferenceCommand(int soloID, int speedRef) { + uint32_t dir = (uint32_t)(speedRef < 0 ? 1 : 0); + uint32_t speedMag = (uint32_t)abs(speedRef); + struct CanOpenData data; + + Serial.print("Speed ref: "); + Serial.println(speedMag); + + // set speed + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = SPEED_REF_CODE, + .payload = speedMag + }; + + can.write(createMessage(data)); + delayMicroseconds(200); + + // set direction + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = MOTOR_DIRECTION_CODE, + .payload = dir + }; + + can.write(createMessage(data)); + delayMicroseconds(200); +} + +void SoloCan::GetBoardTemperatureCommand(int soloID) { + struct CanOpenData data = (struct CanOpenData){ + .id = 0x0600 + soloID, + .type = SDO_READ_COMMAND, + .code = TEMP_CODE, + .payload = 0 + }; + can.write(createMessage(data)); + delayMicroseconds(200); +} + +CAN_message_t createMessage(struct CanOpenData data) { + CAN_message_t msg; + msg.len = 8; + msg.id = 0x7FF & data.id; + msg.buf[0] = 0x00FF & data.type; + msg.buf[1] = 0x00FF & data.code; + msg.buf[2] = (0xFF00 & data.code) >> 8; + msg.buf[3] = 0; + msg.buf[4] = 0x000000FF & data.payload; + msg.buf[5] = (0x0000FF00 & data.payload) >> 8; + msg.buf[6] = (0x00FF0000 & data.payload) >> 16; + msg.buf[7] = (0xFF000000 & data.payload) >> 24; + + return msg; +} + +struct CanOpenData parseMessage(CAN_message_t msg) { + struct CanOpenData data; + data.id = msg.id; + data.type = msg.buf[0]; + data.code = (msg.buf[2] << 8) | msg.buf[1]; + data.payload = (msg.buf[7] << 24) | (msg.buf[6] << 16) | (msg.buf[5] << 8) | msg.buf[4]; + + return data; +} + +float toFloat(uint32_t payload) { + if (payload <= 0x7FFE0000) { + return payload / 131072.0; + } else { + return (0xFFFFFFFF - payload + 0x1) / -131072.0; + } +} + +} \ No newline at end of file diff --git a/lib/SoloCAN/src/SoloCAN.hpp b/lib/SoloCAN/src/SoloCAN.hpp new file mode 100644 index 0000000..7341960 --- /dev/null +++ b/lib/SoloCAN/src/SoloCAN.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include + +namespace solo_can { + + const int SDO_READ_COMMAND = 0x40; + const int SDO_READ_RESPONSE = 0x42; + const int SDO_WRITE_COMMAND = 0x22; + const int SDO_WRITE_RESPONSE = 0x60; + + const int TEMP_CODE = 0x3039; + const int SPEED_REF_CODE = 0x3005; + const int SPEED_FEEDBACK_CODE = 0x3036; + const int POSITION_FEEDBACK_CODE = 0x3037; + const int MOTOR_DIRECTION_CODE = 0x300C; + + struct CanOpenData { + uint16_t id; + uint8_t type; + uint16_t code; + uint32_t payload; + }; + + CAN_message_t createMessage(struct CanOpenData data); + struct CanOpenData parseMessage(CAN_message_t msg); + + float toFloat(uint32_t payload); + + class SoloCan { + public: + SoloCan(FlexCAN_T4 &_can); + + void GetBoardTemperatureCommand(int soloID); + void GetSpeedFeedbackCommand(int soloID); + void GetPositionFeedbackCommand(int soloID); + void SetSpeedReferenceCommand(int soloID, int speedRef); + + private: + FlexCAN_T4 &can; + }; + +} \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 1802892..4cd2a21 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,16 +8,47 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html -[env:teensy41] +[platformio] +default_envs = Stepper + +[env] platform = teensy board = teensy41 upload_protocol = teensy-cli -check_tool = cppcheck framework = arduino + +[env:main] +check_tool = cppcheck +build_src_filter = + +
lib_deps = Nanopb https://github.com/basicmicro/roboclaw_arduino_library.git ; https://github.com/Solo-FL/SOLO-motor-controllers-ARDUINO-library.git#v3.0 +custom_nanopb_protos = + + + +[env:MotorTest] +build_src_filter = + + + +[env:ReadTemp] +build_src_filter = + + + +[env:SoloComputerControl] +build_src_filter = + + +lib_deps = + Nanopb + QNEthernet + +[env:Stepper] +build_src_filter = + + +lib_deps = + https://github.com/janelia-arduino/TMC2209.git + custom_nanopb_protos = + \ No newline at end of file diff --git a/scripts/ArduinoSketches/ArduinoCanRead/ArduinoCanRead.ino b/scripts/ArduinoSketches/ArduinoCanRead/ArduinoCanRead.ino new file mode 100644 index 0000000..799bf12 --- /dev/null +++ b/scripts/ArduinoSketches/ArduinoCanRead/ArduinoCanRead.ino @@ -0,0 +1,43 @@ +#include +#include "mcp_can.h" + +#define SPI_CS_PIN 10 + +MCP_CAN CAN(SPI_CS_PIN); // Set CS pin + +void setup() { + Serial.begin(115200); + while(!Serial); + + while (CAN_OK != CAN.begin(CAN_1000KBPS)) { + Serial.println("CAN BUS FAIL!"); + delay(100); + } + + Serial.println("CAN BUS OK!"); +} + + +void loop() { + unsigned char len = 0; + unsigned char buf[8]; + + if(CAN_MSGAVAIL == CAN.checkReceive()) { + CAN.readMsgBuf(&len, buf); + + unsigned long canId = CAN.getCanId(); + + Serial.println("-----------------------------"); + Serial.print("Get data from ID: "); + Serial.println(canId, HEX); + + for(int i = 0; i < len; i++) { + Serial.print(buf[i], HEX); + Serial.print("\t"); + } + + Serial.println(); + } +} + +// END FILE diff --git a/scripts/ArduinoSketches/ArduinoCanWrite/ArduinoCanWrite.ino b/scripts/ArduinoSketches/ArduinoCanWrite/ArduinoCanWrite.ino new file mode 100644 index 0000000..a2a37af --- /dev/null +++ b/scripts/ArduinoSketches/ArduinoCanWrite/ArduinoCanWrite.ino @@ -0,0 +1,26 @@ +#include +#include + +#define SPI_CS_PIN 10 + +MCP_CAN CAN(SPI_CS_PIN); + +void setup() { + Serial.begin(115200); + while(!Serial); + + while (CAN_OK != CAN.begin(CAN_500KBPS)) { + Serial.println("CAN BUS FAIL!"); + delay(100); + } + Serial.println("CAN BUS OK!"); +} + +unsigned char stmp[8] = {0, 1, 2, 3, 4, 5, 6, 7}; + +void loop() { + CAN.sendMsgBuf(0x0A, 0, 8, stmp); + delay(100); +} + +// END FILE diff --git a/scripts/ArduinoSketches/ArduinoUnoSoloUnoCanTest/ArduinoUnoSoloUnoCanTest.ino b/scripts/ArduinoSketches/ArduinoUnoSoloUnoCanTest/ArduinoUnoSoloUnoCanTest.ino new file mode 100644 index 0000000..ccbc139 --- /dev/null +++ b/scripts/ArduinoSketches/ArduinoUnoSoloUnoCanTest/ArduinoUnoSoloUnoCanTest.ino @@ -0,0 +1,38 @@ +// EXAMPLE of how read the SOLO board temperature, +// every second we print the value of the temperature + + +//Importing SOLO Arduino library +#include "SOLOMotorControllersCanopen.h" + +// Solo Object +SOLOMotorControllers *SOLO_Obj1; + +// Variables +float Temperature=0; +int fwVersion; +int error; + +// definitions +const int SOLOdeviceAddress = 0; // set CAN ID using Motion Studio +const int chipSelectPin = 10; // inland CAN shield uses CS pin 10 + +void setup() { + Serial.begin(115200); + SOLO_Obj1 = new SOLOMotorControllersCanopen(SOLOdeviceAddress, chipSelectPin); +} + +void loop() { + //Reading + Temperature = SOLO_Obj1->GetBoardTemperature(error); + fwVersion = SOLO_Obj1->GetDeviceFirmwareVersion(error); + + //Print + Serial.println("Read from SOLO"); + Serial.println(Temperature,7); + Serial.println(fwVersion, HEX); + Serial.println("Error"); + Serial.println(error); + + delay(1000); +} diff --git a/scripts/ArduinoSketches/ArduinoUnoSoloUnoSpeedTest/ArduinoUnoSoloUnoSpeedTest.ino b/scripts/ArduinoSketches/ArduinoUnoSoloUnoSpeedTest/ArduinoUnoSoloUnoSpeedTest.ino new file mode 100644 index 0000000..814c95a --- /dev/null +++ b/scripts/ArduinoSketches/ArduinoUnoSoloUnoSpeedTest/ArduinoUnoSoloUnoSpeedTest.ino @@ -0,0 +1,128 @@ +// Copyright: (c) 2021, SOLO motor controllers project +// GNU General Public License v3.0+ (see COPYING or https://www.gnu.org/licenses/gpl-3.0.txt) + +/* +* Title: Speed Control of PMSM equipped with Incremental Encoders using Arduino and SOLO +* Author: SOLOMOTORCONTROLLERS +* Date: 2022 +* Code version: 4.0.0 +* Availability: https://github.com/Solo-FL/SOLO-motor-controllers-ARDUINO-library +* Please make sure you are applying the right wiring between SOLO and your ARDUINO +* The Code below has been tested on Arduino UNO +* The Motor used for Testings: teknic m-2310P-LN-04K +*/ + +#include "SOLOMotorControllersCanopen.h" + +//For this Test, make sure you have calibrated your Motor and Incremental Encoders before +//to know more please read: https://www.solomotorcontrollers.com/how-to-connect-calibrate-incremental-encoder-with-solo/ + +//instanciate a SOLO object: +SOLOMotorControllers *SOLO_Obj1; + +//the device address of SOLO: +unsigned char SOLO_address1 = 0; + +//Desired Switching or PWM Frequency at Output +long pwmFrequency = 20; + +//Motor's Number of Poles +long numberOfPoles = 8; + +// Current Limit of the Motor +float currentLimit = 7.0; + +//Motor's Number of Encoder Lines (PPR pre-quad) +long numberOfEncoderLines = 1000; + +//Speed controller Kp +float speedControllerKp = 0.15; + +//Speed controller Ki +float speedControllerKi = 0.005; + +// Battery or Bus Voltage +float busVoltage = 0; + +// Motor Torque feedback +float actualMotorTorque = 0; + +// Motor speed feedback +long actualMotorSpeed = 0; + +// Motor position feedback +long actualMotorPosition = 0; + +void setup() { + //In this example, make sure you put SOLO into Closed-Loop Mode + + //used for Monitoring in Arduino Only + Serial.begin(115200); + + //Initialize the SOLO object + int SOLOdeviceAddress = 0; + int chipSelectPin = 9; //SPI CS pin for CANshield + SOLO_Obj1 = new SOLOMotorControllersCanopen(SOLOdeviceAddress, chipSelectPin); + delay(1000); + + Serial.println("\n Trying to Connect To SOLO"); + delay(1000); + //wait here till communication is established + while(SOLO_Obj1->CommunicationIsWorking() == false) + { + delay(500); + } + Serial.println("\n Communication Established succuessfully!"); + + // Initial Configuration of the device and the Motor + SOLO_Obj1->SetOutputPwmFrequencyKhz(pwmFrequency); + SOLO_Obj1->SetCurrentLimit(currentLimit); + SOLO_Obj1->SetMotorPolesCounts(numberOfPoles); + SOLO_Obj1->SetIncrementalEncoderLines(numberOfEncoderLines); + SOLO_Obj1->SetCommandMode(SOLOMotorControllers::CommandMode::digital); + SOLO_Obj1->SetMotorType(SOLOMotorControllers::MotorType::bldcPmsm); + SOLO_Obj1->SetFeedbackControlMode(SOLOMotorControllers::FeedbackControlMode::encoders); + SOLO_Obj1->SetSpeedControllerKp(speedControllerKp); + SOLO_Obj1->SetSpeedControllerKi(speedControllerKi); + SOLO_Obj1->SetControlMode(SOLOMotorControllers::ControlMode::speedMode); + + //run the motor identification to Auto-tune the current controller gains Kp and Ki needed for Torque Loop + //run ID. always after selecting the Motor Type! + //ID. doesn't need to be called everytime, only one time after wiring up the Motor will be enough + //the ID. values will be remembered by SOLO after power recycling + SOLO_Obj1->MotorParametersIdentification(SOLOMotorControllers::Action::start); + Serial.println("\n Identifying the Motor"); + //wait at least for 2sec till ID. is done + delay(2000); +} + +void loop() { + + //set the Direction on C.C.W. + SOLO_Obj1->SetMotorDirection(SOLOMotorControllers::Direction::counterclockwise); + //set an arbitrary Positive speed reference[RPM] + SOLO_Obj1->SetSpeedReference(1500); + // wait till motor reaches to the reference + delay(300); + actualMotorSpeed = SOLO_Obj1->GetSpeedFeedback(); + Serial.println("\n Measured Speed[RPM]: "); + Serial.println(actualMotorSpeed); + actualMotorTorque = SOLO_Obj1->GetQuadratureCurrentIqFeedback(); + Serial.println("\n Measured Iq/Torque[A]: "); + Serial.println(actualMotorTorque); + delay(3000); + + //set the Direction on C.W. + SOLO_Obj1->SetMotorDirection(SOLOMotorControllers::Direction::clockwise); + //set an arbitrary Positive speed reference[RPM] + SOLO_Obj1->SetSpeedReference(3000); + // wait till motor reaches to the reference + delay(300); + actualMotorSpeed = SOLO_Obj1->GetSpeedFeedback(); + Serial.println("\n Measured Speed[RPM]: "); + Serial.println(actualMotorSpeed); + actualMotorTorque = SOLO_Obj1->GetQuadratureCurrentIqFeedback(); + Serial.println("\n Measured Iq/Torque[A]: "); + Serial.println(actualMotorTorque); + delay(3000); +} diff --git a/scripts/MOTOR_CONFIG.md b/scripts/MOTOR_CONFIG.md new file mode 100644 index 0000000..15c839f --- /dev/null +++ b/scripts/MOTOR_CONFIG.md @@ -0,0 +1,28 @@ +# How to configure motor controllers + +0. Unplug motor from Solo Uno. +1. Turn on the Solo Uno. Set the following settings: +* Command Mode: Digital +* Control Type: Torque (Maybe its safe?) +* Device Address: 161-166 (i.e. A1 - A6) +* Motor Type: BLDC-PMSM +* PWM Frequency: 20 kHz +* Number of Poles: 14 +* Current Limit: 7.5 +* Feedback Control Mode: Sensor Less +2. For the following settings, click the lock to edit the values. If these values are out of range / incorrect, the motor controller could explode! +* Current Controller Kp: 0.20 +* Current Controller Ki: 0.00165 +* Motor Inductance: 0.00022 +* Motor Resistance: 0.024 +3. Turn off the Solo Uno, plug in the motor. +4. Run "Motor Identification" in Motion Terminal. This will set the correct values for Current Controller Kp, Current Controller Ki, Motor Inductance, and Motor Resistance. +5. Set the following settings: +* Feedback Control Mode: Using Encoders +* Encoder Lines: 2048 +6. Run "Encoder Calibration". +7. Set the following settings: +* Control Type: Speed +* Speed Controller Kp: 0.12 +* Speed Controller Ki: 0.005 +8. The Solo is now ready for regular use. diff --git a/scripts/SoloConfig/encoder_calibration.py b/scripts/SoloConfig/encoder_calibration.py new file mode 100644 index 0000000..44245fd --- /dev/null +++ b/scripts/SoloConfig/encoder_calibration.py @@ -0,0 +1,42 @@ +import SoloPy as solo +import time +import argparse + +parser = argparse.ArgumentParser(description="Run SoloUNO encoder calibration") +parser.add_argument('port', type=str, help="USB port") +args = parser.parse_args() + +# instanciate a SOLO object: +mySolo = solo.SoloMotorControllerUart(args.port, 0, solo.UART_BAUD_RATE.RATE_937500) +print("Start") + +while True: + time.sleep(1) + + commandMode, error = mySolo.get_command_mode() + if error == solo.ERROR.NO_ERROR_DETECTED: + break + else: + print("ERROR: " + str(error)) + +print("Identifying") +mySolo.sensor_calibration(solo.POSITION_SENSOR_CALIBRATION_ACTION.INCREMENTAL_ENCODER_START_CALIBRATION) +time.sleep(20) +result = mySolo.sensor_calibration(solo.POSITION_SENSOR_CALIBRATION_ACTION.STOP_CALIBRATION) + + +time.sleep(1) + +if result[0]: + print("CCW Offset:", mySolo.get_encoder_hall_ccw_offset()) + print("CW Offset:", mySolo.get_encoder_hall_cw_offset()) + mySolo.set_control_mode(solo.CONTROL_MODE.SPEED_MODE) +else: + print("ERROR:", result[1]) + +time.sleep(1) + +print("Done, disconnecting") +#ensure close the serial +mySolo.serial_close() +print("Disconnected") \ No newline at end of file diff --git a/scripts/SoloConfig/parameter_identification.py b/scripts/SoloConfig/parameter_identification.py new file mode 100644 index 0000000..7995f50 --- /dev/null +++ b/scripts/SoloConfig/parameter_identification.py @@ -0,0 +1,40 @@ +import SoloPy as solo +import time +import argparse + +parser = argparse.ArgumentParser(description="Run SoloUNO parameter identification routine") +parser.add_argument('port', type=str, help="USB port") +args = parser.parse_args() + +# instanciate a SOLO object: +mySolo = solo.SoloMotorControllerUart(args.port, 0, solo.UART_BAUD_RATE.RATE_937500) +print("Start") + +while True: + time.sleep(1) + + commandMode, error = mySolo.get_command_mode() + if error == solo.ERROR.NO_ERROR_DETECTED: + break + else: + print("ERROR: " + str(error)) + +print("Identifying") +result = mySolo.motor_parameters_identification(solo.ACTION.START) + +time.sleep(1) + +if result[0]: + print("KI:", mySolo.get_current_controller_ki()) + print("KP:", mySolo.get_current_controller_kp()) + print("Inductance:", mySolo.get_motor_inductance()) + print("Resistance:", mySolo.get_motor_resistance()) +else: + print("ERROR:", result[1]) + +time.sleep(1) + +print("Done, disconnecting") +#ensure close the serial +mySolo.serial_close() +print("Disconnected") \ No newline at end of file diff --git a/scripts/SoloConfig/set_constants.py b/scripts/SoloConfig/set_constants.py new file mode 100644 index 0000000..e89a5f6 --- /dev/null +++ b/scripts/SoloConfig/set_constants.py @@ -0,0 +1,42 @@ +import SoloPy as solo +import time +import argparse + +parser = argparse.ArgumentParser(description="Set SoloUNO constants") +parser.add_argument('port', type=str, help="USB port") +args = parser.parse_args() + +# instanciate a SOLO object: +mySolo = solo.SoloMotorControllerUart(args.port, 0, solo.UART_BAUD_RATE.RATE_937500) +print("Start") + +while True: + time.sleep(1) + + commandMode, error = mySolo.get_command_mode() + if error == solo.ERROR.NO_ERROR_DETECTED: + print("Configuring") + else: + print("ERROR: " + str(error)) + + if error == solo.ERROR.NO_ERROR_DETECTED: + success_list = [] + success_list.append(mySolo.set_command_mode(solo.COMMAND_MODE.DIGITAL)) + success_list.append(mySolo.set_feedback_control_mode(solo.FEEDBACK_CONTROL_MODE.ENCODERS)) + success_list.append(mySolo.set_control_mode(solo.CONTROL_MODE.SPEED_MODE)) + success_list.append(mySolo.set_output_pwm_frequency_khz(20)) + success_list.append(mySolo.set_incremental_encoder_lines(2048)) + success_list.append(mySolo.set_current_limit(7.5)) + success_list.append(mySolo.set_motor_poles_counts(14)) + success_list.append(mySolo.set_motor_type(solo.MOTOR_TYPE.BLDC_PMSM)) + if all([e[0] for e in success_list]): + break + else: + print("\n".join([error for bool,error in success_list])) + print("\n\n\n\n\n") + +print("Done, disconnecting") +#ensure close the serial +mySolo.serial_close() +print("Disconnected") + diff --git a/scripts/SoloConfig/setparams.py b/scripts/SoloConfig/setparams.py new file mode 100644 index 0000000..8f8c478 --- /dev/null +++ b/scripts/SoloConfig/setparams.py @@ -0,0 +1,70 @@ +import SoloPy as solo +import time +import argparse + +# 0: Unplug motor from Solo Uno. +# 1: Set the following settings: +# a. Command Mode: Digital +# b. Control Type: Speed +# c. Device Address: 161-166 (i.e. A1 - A6) +# d. Motor Type: BLDC-PMSM +# e. PWM Frequency: 20 kHz +# f. Number of Poles: 14 +# g. Current Limit: 7.5 +# 2: For the following settings, click the lock to edit the values +# a. Current Controller Kp: 0.20 +# b. Current Controller Ki: 0.00165 +# c. Motor Inductance: +# 2: Turn off the Solo Uno, plug in the motor. +# 2: Set can ID in motion terminal (IDs=160-166). +# 3: Run this script to set important values. +# 4: Do "Motor Identification" in Motion Terminal. +# 5: + +parser = argparse.ArgumentParser(description="Set SoloUNO constants") +parser.add_argument('port', type=str, help="USB port") +args = parser.parse_args() + +mySolo = solo.SoloMotorControllerUart(args.port, 0, solo.UART_BAUD_RATE.RATE_937500) +print("Setting Constants") + +while True: + time.sleep(1) + + commandMode, error = mySolo.get_command_mode() + if error == solo.ERROR.NO_ERROR_DETECTED: + print("Configuring") + else: + print("ERROR: " + str(error)) + + if error == solo.ERROR.NO_ERROR_DETECTED: + success_list = [] + + # configurations + success_list.append(mySolo.set_command_mode(solo.COMMAND_MODE.DIGITAL)) + success_list.append(mySolo.set_output_pwm_frequency_khz(20)) + success_list.append(mySolo.set_motor_type(solo.MOTOR_TYPE.BLDC_PMSM)) + success_list.append(mySolo.set_current_limit(7.5)) + success_list.append(mySolo.set_motor_poles_counts(14)) + + # controls + success_list.append(mySolo.set_feedback_control_mode(solo.FEEDBACK_CONTROL_MODE.ENCODERS)) + success_list.append(mySolo.set_control_mode(solo.CONTROL_MODE.SPEED_MODE)) + success_list.append(mySolo.set_incremental_encoder_lines(2048)) + + # motor identification + success_list.append(mySolo.set_current_controller_kp(0.20)) + success_list.append(mySolo.set_current_controller_ki(0.0015)) + success_list.append(mySolo.set_motor_inductance(0.00022)) + success_list.append(mySolo.set_motor_resistance(0.024)) + + if all([e[0] for e in success_list]): + break + else: + print("\n".join([error for bool,error in success_list])) + print("\n\n\n\n\n") + +print("Disconnecting") +mySolo.serial_close() +time.sleep(3) + diff --git a/scripts/SoloConfig/speed_test.py b/scripts/SoloConfig/speed_test.py new file mode 100644 index 0000000..08e7fe1 --- /dev/null +++ b/scripts/SoloConfig/speed_test.py @@ -0,0 +1,45 @@ +import SoloPy as solo +import time +import argparse + + +parser = argparse.ArgumentParser(description="Test SoloUNO BLDC Speed") +parser.add_argument('port', type=str, help="USB port") +args = parser.parse_args() + +# instanciate a SOLO object: +mySolo = solo.SoloMotorControllerUart(args.port, 0, solo.UART_BAUD_RATE.RATE_937500) +print("Start") + +while True: + time.sleep(1) + + commandMode, error = mySolo.get_command_mode() + if error == solo.ERROR.NO_ERROR_DETECTED: + break + else: + print("ERROR: " + str(error)) + +print("Setting kp, ki") +mySolo.set_speed_controller_kp(0.12) +mySolo.set_speed_controller_ki(0.005) + +print("Sending CW speed command") +mySolo.set_motor_direction(solo.DIRECTION.CLOCKWISE) +mySolo.set_speed_reference(1000) + +time.sleep(5) +mySolo.set_speed_reference(0) + +time.sleep(1) +print("Sending CCW speed command") +mySolo.set_motor_direction(solo.DIRECTION.COUNTERCLOCKWISE) +mySolo.set_speed_reference(1000) + +time.sleep(5) +mySolo.set_speed_reference(0) + +print("Done, disconnecting") +#ensure close the serial +mySolo.serial_close() +print("Disconnected") \ No newline at end of file diff --git a/scripts/Teensy4Sketches/Teensy4CANSoloConfig/Teensy4CANSoloConfig.ino b/scripts/Teensy4Sketches/Teensy4CANSoloConfig/Teensy4CANSoloConfig.ino new file mode 100644 index 0000000..95c2b6e --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4CANSoloConfig/Teensy4CANSoloConfig.ino @@ -0,0 +1,9 @@ +void setup() { + // put your setup code here, to run once: + +} + +void loop() { + // put your main code here, to run repeatedly: + +} diff --git a/scripts/Teensy4Sketches/Teensy4CanTest/Teensy4CanTest.ino b/scripts/Teensy4Sketches/Teensy4CanTest/Teensy4CanTest.ino new file mode 100644 index 0000000..baf2026 --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4CanTest/Teensy4CanTest.ino @@ -0,0 +1,42 @@ +#include + +FlexCAN_T4 myCan1; + +elapsedMillis blinkTimer; +const int BLINK_RATE = 500; + +CAN_message_t rmsg; + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(1000 * 1000); +} +void loop() { + + if (myCan1.read(rmsg)) { + Serial.print("CAN1 "); + Serial.print(" ID: 0x"); + Serial.print(rmsg.id, HEX); + + Serial.print(" DATA: "); + char digit[3]; + for (int i = 0; i < rmsg.len; i++) { + sprintf(digit, "%02x", rmsg.buf[i]); + Serial.print(digit); + Serial.print(" "); + } + + + + Serial.print("\n"); + } + + + // blink LED + if (blinkTimer >= BLINK_RATE) { + blinkTimer -= BLINK_RATE; + digitalToggle(LED_BUILTIN); + } + +} diff --git a/scripts/Teensy4Sketches/Teensy4FlexCANSelfTest/Teensy4FlexCANSelfTest.ino b/scripts/Teensy4Sketches/Teensy4FlexCANSelfTest/Teensy4FlexCANSelfTest.ino new file mode 100644 index 0000000..df61de6 --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4FlexCANSelfTest/Teensy4FlexCANSelfTest.ino @@ -0,0 +1,72 @@ +#include + +FlexCAN_T4 myCan1; +FlexCAN_T4 myCan2; + +elapsedMillis blinkTimer; +const int BLINK_RATE = 500; + +elapsedMillis sendTimer; +const int SEND_TIME = 1000; + +CAN_message_t msg; +CAN_message_t rmsg; + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan2.begin(); + myCan1.setBaudRate(250 * 1000); + myCan2.setBaudRate(250 * 1000); +} +void loop() { + + if (sendTimer >= SEND_TIME) { + sendTimer -= SEND_TIME; + //setup message + msg.len = 8; + msg.id = 10; + msg.buf[0] = 1; + msg.buf[1] = 2; + msg.buf[2] = 3; + msg.buf[3] = 4; + msg.buf[4] = 5; + msg.buf[5] = 6; + msg.buf[6] = 7; + msg.buf[7] = 8; + myCan1.write(msg); + } + +// //modify message +// msg.id = 2; +// msg.buf[6] = 4; +// msg.buf[7] = 2; +// +// myCan2.write(msg); + + + + + if (myCan1.read(rmsg)) { + Serial.print("CAN1 "); + Serial.print(" ID: 0x"); + Serial.print(rmsg.id, HEX ); + //Serial.print(int(rmsg.buf)); + Serial.print("\n"); + } + + if (myCan2.read(rmsg)) { + Serial.print("CAN2 "); + Serial.print(" ID: 0x"); + Serial.print(rmsg.id, HEX ); + //Serial.print(int(rmsg.buf)); + Serial.print("\n"); + } + + // blink LED + if (blinkTimer >= BLINK_RATE) { + blinkTimer -= BLINK_RATE; + digitalToggle(LED_BUILTIN); + } + +} diff --git a/scripts/Teensy4Sketches/Teensy4MotorTest/Teensy4MotorTest.ino b/scripts/Teensy4Sketches/Teensy4MotorTest/Teensy4MotorTest.ino new file mode 100644 index 0000000..e06f00a --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4MotorTest/Teensy4MotorTest.ino @@ -0,0 +1,241 @@ +#include + +// constants +const int BLINK_RATE_MS = 500; +const int READ_RATE_MS = 1000; +const int STATE_UPDATE_RATE_MS = 5000; +const int BAUD_RATE = 1000000; + +const int SDO_READ_COMMAND = 0x40; +const int SDO_READ_RESPONSE = 0x42; +const int SDO_WRITE_COMMAND = 0x22; +const int SDO_WRITE_RESPONSE = 0x60; + +const int TEMP_CODE = 0x3039; +const int SPEED_REF_CODE = 0x3005; +const int SPEED_FEEDBACK_CODE = 0x3036; +const int POSITION_FEEDBACK_CODE = 0x3037; +const int MOTOR_DIRECTION_CODE = 0x300C; + +const int MOTOR_SPEED = 3000; +const int NUM_MOTORS = 2; +const int MOTOR_IDS[NUM_MOTORS] = {0xA4, 0xA3}; + +struct CanOpenData { + uint16_t id; + uint8_t type; + uint16_t code; + uint32_t payload; +}; + +enum MotorState { + MOTOR_STATE_FORWARD, + MOTOR_STATE_BACKWARD, + MOTOR_STATE_STOPPED +}; + +// variables +FlexCAN_T4 myCan1; + +elapsedMillis blinkTimer; +elapsedMillis sendTimer; +elapsedMillis stateMachineTimer; +CAN_message_t rmsg; + +enum MotorState curr = MOTOR_STATE_STOPPED; +enum MotorState prev = MOTOR_STATE_STOPPED; + +// functions +void SetSpeedReferenceCommand(int soloID, int speedRef); +void GetSpeedFeedbackCommand(int soloID); +void GetPositionFeedbackCommand(int soloID); +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev); +void printCANMessage(CAN_message_t message); + +CAN_message_t createMessage(struct CanOpenData data); +struct CanOpenData parseMessage(CAN_message_t msg); + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(BAUD_RATE); +} + +void loop() { + + // parse incoming CAN messages + if (myCan1.read(rmsg)) { +// printCANMessage(rmsg); + + struct CanOpenData data = parseMessage(rmsg); + + if (data.type == SDO_READ_RESPONSE) { + if (data.code == SPEED_FEEDBACK_CODE) { + Serial.print("Got speed feedback: "); + Serial.print("[ID="); + Serial.print(data.id, HEX); + Serial.print(", SPEED="); + Serial.print((int)data.payload); + Serial.println("]"); + } else if (data.code == POSITION_FEEDBACK_CODE) { + Serial.print("Got position feedback: "); + int ref = data.payload; + Serial.println(ref); + } + } + } + + // set speed + if (stateMachineTimer >= STATE_UPDATE_RATE_MS) { + stateMachineTimer -= STATE_UPDATE_RATE_MS; + + // determine new state + enum MotorState newState = determineNextState(curr, prev); + + // determine motor speed + int motorSpeed; + if (newState == MOTOR_STATE_FORWARD) { + motorSpeed = MOTOR_SPEED; + } else if (newState == MOTOR_STATE_BACKWARD) { + motorSpeed = -1 * MOTOR_SPEED; + } else { + motorSpeed = 0; + } + + // send command + + + for (int i = 0; i < NUM_MOTORS; i++) { + SetSpeedReferenceCommand(MOTOR_IDS[i], motorSpeed); + } + + + // update state + prev = curr; + curr = newState; + } + + // read speed reference + if (sendTimer >= READ_RATE_MS) { + sendTimer -= READ_RATE_MS; + + for (int i = 0; i < NUM_MOTORS; i++) { + GetSpeedFeedbackCommand(MOTOR_IDS[i]); + } + } + + // blink LED + if (blinkTimer >= BLINK_RATE_MS) { + blinkTimer -= BLINK_RATE_MS; + digitalToggle(LED_BUILTIN); + } + +} + +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev) { + if (curr == MOTOR_STATE_STOPPED) { + if (prev == MOTOR_STATE_FORWARD) { + return MOTOR_STATE_BACKWARD; + } else { + return MOTOR_STATE_FORWARD; + } + } else { + return MOTOR_STATE_STOPPED; + } +} + +void printCANMessage(CAN_message_t message) { + Serial.print("CAN1 "); + Serial.print(" ID: 0x"); + Serial.print(message.id, HEX); + + Serial.print(" DATA: "); + char digit[3]; + for (int i = 0; i < message.len; i++) { + sprintf(digit, "%02x", message.buf[i]); + Serial.print(digit); + Serial.print(" "); + } + + Serial.print("\n"); +} + +void SetSpeedReferenceCommand(int soloID, int speedRef) { + + uint32_t dir = (uint32_t)(speedRef < 0 ? 1 : 0); + uint32_t speedMag = (uint32_t)abs(speedRef); + struct CanOpenData data; + + Serial.print("Speed ref: "); + Serial.println(speedMag); + + // set speed + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = SPEED_REF_CODE, + .payload = speedMag + }; + + myCan1.write(createMessage(data)); + delayMicroseconds(200); + + // set direction + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = MOTOR_DIRECTION_CODE, + .payload = dir + }; + + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +void GetSpeedFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = SPEED_FEEDBACK_CODE, + .payload = 0 + }; + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +void GetPositionFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = POSITION_FEEDBACK_CODE, + .payload = 0 + }; + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +CAN_message_t createMessage(struct CanOpenData data) { + CAN_message_t msg; + msg.len = 8; + msg.id = 0x7FF & data.id; + msg.buf[0] = 0x00FF & data.type; + msg.buf[1] = 0x00FF & data.code; + msg.buf[2] = (0xFF00 & data.code) >> 8; + msg.buf[3] = 0; + msg.buf[4] = 0x000000FF & data.payload; + msg.buf[5] = (0x0000FF00 & data.payload) >> 8; + msg.buf[6] = (0x00FF0000 & data.payload) >> 16; + msg.buf[7] = (0xFF000000 & data.payload) >> 24; + + return msg; +} + +struct CanOpenData parseMessage(CAN_message_t msg) { + struct CanOpenData data; + data.id = msg.id; + data.type = msg.buf[0]; + data.code = (msg.buf[2] << 8) | msg.buf[1]; + data.payload = (msg.buf[7] << 24) | (msg.buf[6] << 16) | (msg.buf[5] << 8) | msg.buf[4]; + + return data; +} diff --git a/scripts/Teensy4Sketches/Teensy4ReadTemp/Teensy4ReadTemp.ino b/scripts/Teensy4Sketches/Teensy4ReadTemp/Teensy4ReadTemp.ino new file mode 100644 index 0000000..c4847a7 --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4ReadTemp/Teensy4ReadTemp.ino @@ -0,0 +1,109 @@ +#include + +FlexCAN_T4 myCan1; + +elapsedMillis blinkTimer; +elapsedMillis sendTimer; + +const int BLINK_RATE = 500; +const int BAUD_RATE = 1000000; +const int SOLO_ID = 0xA4; +const int REQUEST = 0x40; +const int RESPONSE = 0x42; +const int TEMP_CODE = 0x3039; + +CAN_message_t rmsg; + +struct CanOpenData { + uint16_t id; + uint8_t type; + uint16_t code; + uint32_t payload; +}; + +CAN_message_t createMessage(struct CanOpenData data); +struct CanOpenData parseMessage(CAN_message_t msg); + +void sendRequest(); +void printTemp(char *buf, int len); + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(BAUD_RATE); +} + +void loop() { + + if (myCan1.read(rmsg)) { + Serial.print("CAN1 "); + Serial.print(" ID: 0x"); + Serial.print(rmsg.id, HEX); + + Serial.print(" DATA: "); + char digit[3]; + for (int i = 0; i < rmsg.len; i++) { + sprintf(digit, "%02x", rmsg.buf[i]); + Serial.print(digit); + Serial.print(" "); + } + + Serial.print("\n"); + + struct CanOpenData data = parseMessage(rmsg); + + if (data.type == RESPONSE && data.code == TEMP_CODE) { + Serial.print("Got temp value: "); + float temp = data.payload / 131072.0; + Serial.println(temp); + } else { + Serial.println("Got something else!"); + } + + } + + // send CAN message + if (sendTimer >= BLINK_RATE) { + sendTimer -= BLINK_RATE; + struct CanOpenData data = (struct CanOpenData){ + .id = 0x0600 + SOLO_ID, + .type = REQUEST, + .code = TEMP_CODE, + .payload = 0 + }; + myCan1.write(createMessage(data)); + } + + // blink LED + if (blinkTimer >= BLINK_RATE) { + blinkTimer -= BLINK_RATE; + digitalToggle(LED_BUILTIN); + } + +} + +CAN_message_t createMessage(struct CanOpenData data) { + CAN_message_t msg; + msg.len = 8; + msg.id = 0x7FF & data.id; + msg.buf[0] = 0x00FF & data.type; + msg.buf[1] = 0x00FF & data.code; + msg.buf[2] = (0xFF00 & data.code) >> 8; + msg.buf[3] = 0; + msg.buf[4] = 0x000000FF & data.payload; + msg.buf[5] = 0x0000FF00 & data.payload; + msg.buf[6] = 0x00FF0000 & data.payload; + msg.buf[7] = 0xFF000000 & data.payload; + + return msg; +} + +struct CanOpenData parseMessage(CAN_message_t msg) { + struct CanOpenData data; + data.id = msg.id; + data.type = msg.buf[0]; + data.code = (msg.buf[2] << 8) | msg.buf[1]; + data.payload = (msg.buf[7] << 24) | (msg.buf[6] << 16) | (msg.buf[5] << 8) | msg.buf[4]; + + return data; +} diff --git a/scripts/Teensy4Sketches/Teensy4WriteSoloSettings/Teensy4WriteSoloSettings.ino b/scripts/Teensy4Sketches/Teensy4WriteSoloSettings/Teensy4WriteSoloSettings.ino new file mode 100644 index 0000000..48fc041 --- /dev/null +++ b/scripts/Teensy4Sketches/Teensy4WriteSoloSettings/Teensy4WriteSoloSettings.ino @@ -0,0 +1,241 @@ +#include + +// constants +const int BLINK_RATE_MS = 500; +const int READ_RATE_MS = 1000; +const int STATE_UPDATE_RATE_MS = 5000; +const int BAUD_RATE = 1000000; + +const int SDO_READ_COMMAND = 0x40; +const int SDO_READ_RESPONSE = 0x42; +const int SDO_WRITE_COMMAND = 0x22; +const int SDO_WRITE_RESPONSE = 0x60; + +const int TEMP_CODE = 0x3039; +const int SPEED_REF_CODE = 0x3005; +const int SPEED_FEEDBACK_CODE = 0x3036; +const int POSITION_FEEDBACK_CODE = 0x3037; +const int MOTOR_DIRECTION_CODE = 0x300C; + +const int MOTOR_SPEED = 3000; +const int NUM_MOTORS = 3; +const int MOTOR_IDS[NUM_MOTORS] = {0xA4, 0xA3, 0xA6}; + +struct CanOpenData { + uint16_t id; + uint8_t type; + uint16_t code; + uint32_t payload; +}; + +enum MotorState { + MOTOR_STATE_FORWARD, + MOTOR_STATE_BACKWARD, + MOTOR_STATE_STOPPED +}; + +// variables +FlexCAN_T4 myCan1; + +elapsedMillis blinkTimer; +elapsedMillis sendTimer; +elapsedMillis stateMachineTimer; +CAN_message_t rmsg; + +enum MotorState curr = MOTOR_STATE_STOPPED; +enum MotorState prev = MOTOR_STATE_STOPPED; + +// functions +void SetSpeedReferenceCommand(int soloID, int speedRef); +void GetSpeedFeedbackCommand(int soloID); +void GetPositionFeedbackCommand(int soloID); +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev); +void printCANMessage(CAN_message_t message); + +CAN_message_t createMessage(struct CanOpenData data); +struct CanOpenData parseMessage(CAN_message_t msg); + +void setup() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(BAUD_RATE); +} + +void loop() { + + // parse incoming CAN messages + if (myCan1.read(rmsg)) { +// printCANMessage(rmsg); + + struct CanOpenData data = parseMessage(rmsg); + + if (data.type == SDO_READ_RESPONSE) { + if (data.code == SPEED_FEEDBACK_CODE) { + Serial.print("Got speed feedback: "); + Serial.print("[ID="); + Serial.print(data.id, HEX); + Serial.print(", SPEED="); + Serial.print((int)data.payload); + Serial.println("]"); + } else if (data.code == POSITION_FEEDBACK_CODE) { + Serial.print("Got position feedback: "); + int ref = data.payload; + Serial.println(ref); + } + } + } + + // set speed + if (stateMachineTimer >= STATE_UPDATE_RATE_MS) { + stateMachineTimer -= STATE_UPDATE_RATE_MS; + + // determine new state + enum MotorState newState = determineNextState(curr, prev); + + // determine motor speed + int motorSpeed; + if (newState == MOTOR_STATE_FORWARD) { + motorSpeed = MOTOR_SPEED; + } else if (newState == MOTOR_STATE_BACKWARD) { + motorSpeed = -1 * MOTOR_SPEED; + } else { + motorSpeed = 0; + } + + // send command + + + for (int i = 0; i < NUM_MOTORS; i++) { + SetSpeedReferenceCommand(MOTOR_IDS[i], motorSpeed); + } + + + // update state + prev = curr; + curr = newState; + } + + // read speed reference + if (sendTimer >= READ_RATE_MS) { + sendTimer -= READ_RATE_MS; + + for (int i = 0; i < NUM_MOTORS; i++) { + GetSpeedFeedbackCommand(MOTOR_IDS[i]); + } + } + + // blink LED + if (blinkTimer >= BLINK_RATE_MS) { + blinkTimer -= BLINK_RATE_MS; + digitalToggle(LED_BUILTIN); + } + +} + +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev) { + if (curr == MOTOR_STATE_STOPPED) { + if (prev == MOTOR_STATE_FORWARD) { + return MOTOR_STATE_BACKWARD; + } else { + return MOTOR_STATE_FORWARD; + } + } else { + return MOTOR_STATE_STOPPED; + } +} + +void printCANMessage(CAN_message_t message) { + Serial.print("CAN1 "); + Serial.print(" ID: 0x"); + Serial.print(message.id, HEX); + + Serial.print(" DATA: "); + char digit[3]; + for (int i = 0; i < message.len; i++) { + sprintf(digit, "%02x", message.buf[i]); + Serial.print(digit); + Serial.print(" "); + } + + Serial.print("\n"); +} + +void SetSpeedReferenceCommand(int soloID, int speedRef) { + + uint32_t dir = (uint32_t)(speedRef < 0 ? 1 : 0); + uint32_t speedMag = (uint32_t)abs(speedRef); + struct CanOpenData data; + + Serial.print("Speed ref: "); + Serial.println(speedMag); + + // set speed + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = SPEED_REF_CODE, + .payload = speedMag + }; + + myCan1.write(createMessage(data)); + delayMicroseconds(200); + + // set direction + data = (struct CanOpenData) { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_WRITE_COMMAND, + .code = MOTOR_DIRECTION_CODE, + .payload = dir + }; + + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +void GetSpeedFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = SPEED_FEEDBACK_CODE, + .payload = 0 + }; + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +void GetPositionFeedbackCommand(int soloID) { + struct CanOpenData data = { + .id = (uint16_t)(0x0600 + soloID), + .type = SDO_READ_COMMAND, + .code = POSITION_FEEDBACK_CODE, + .payload = 0 + }; + myCan1.write(createMessage(data)); + delayMicroseconds(200); +} + +CAN_message_t createMessage(struct CanOpenData data) { + CAN_message_t msg; + msg.len = 8; + msg.id = 0x7FF & data.id; + msg.buf[0] = 0x00FF & data.type; + msg.buf[1] = 0x00FF & data.code; + msg.buf[2] = (0xFF00 & data.code) >> 8; + msg.buf[3] = 0; + msg.buf[4] = 0x000000FF & data.payload; + msg.buf[5] = (0x0000FF00 & data.payload) >> 8; + msg.buf[6] = (0x00FF0000 & data.payload) >> 16; + msg.buf[7] = (0xFF000000 & data.payload) >> 24; + + return msg; +} + +struct CanOpenData parseMessage(CAN_message_t msg) { + struct CanOpenData data; + data.id = msg.id; + data.type = msg.buf[0]; + data.code = (msg.buf[2] << 8) | msg.buf[1]; + data.payload = (msg.buf[7] << 24) | (msg.buf[6] << 16) | (msg.buf[5] << 8) | msg.buf[4]; + + return data; +} diff --git a/scripts/protobuf/README.md b/scripts/protobuf/README.md new file mode 100644 index 0000000..64f2aa3 --- /dev/null +++ b/scripts/protobuf/README.md @@ -0,0 +1,21 @@ +# Protobuf Example + +### Setup + +1. Install `protoc` protobuf compiler +2. Run the following commands + +```bash +cd urc-firmware/scripts/protobuf +protoc -I ../../protos/ --python_out=. urc.proto +``` + + + +### Running the Script + +1. Connect the Teensy 4.1 and your computer to the same LAN. Configure static IPs for +your computer and the Teensy. +2. Run the `motor_test.py` script. + +Expected behavior: all motors should spin in one direction, stop, spin in the opposite direction, and stop repeatedly. diff --git a/scripts/protobuf/motor_test.py b/scripts/protobuf/motor_test.py new file mode 100644 index 0000000..c7b2129 --- /dev/null +++ b/scripts/protobuf/motor_test.py @@ -0,0 +1,80 @@ +from enum import Enum +import urc_pb2 +import socket +import threading +import time +import signal +import sys + +class MotorState(Enum): + MOTOR_STATE_FORWARD = 1 + MOTOR_STATE_BACKWARD = 2 + MOTOR_STATE_STOPPED = 3 + +# constants +STATE_MACHINE_UPDATE_MS = 5000 +MOTOR_SPEED = 3000 +SERVER_IP = '192.168.1.168' +PORT = 8443 + +# state variables +curr = MotorState.MOTOR_STATE_STOPPED +prev = MotorState.MOTOR_STATE_STOPPED +server_address = (SERVER_IP, PORT) +client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + +def determine_next_state(curr, prev): + if curr == MotorState.MOTOR_STATE_STOPPED: + if prev == MotorState.MOTOR_STATE_FORWARD: + return MotorState.MOTOR_STATE_BACKWARD + else: + return MotorState.MOTOR_STATE_FORWARD + else: + return MotorState.MOTOR_STATE_STOPPED + +def determine_speed(curr): + if curr == MotorState.MOTOR_STATE_FORWARD: + return MOTOR_SPEED + elif curr == MotorState.MOTOR_STATE_BACKWARD: + return -1 * MOTOR_SPEED + else: + return 0 + +# state machine +def state_machine(): + + global curr + global prev + global client_socket + + new_state = determine_next_state(curr, prev) + + prev = curr + curr = new_state + speed = determine_speed(curr) + + print(f"State: [{prev} -> {curr}, speed={speed}]") + + message = urc_pb2.RequestMessage() + message.requestSpeed = True + message.requestDiagnostics = False + message.leftSpeed = speed + message.rightSpeed = speed + message.timestamp = 0 + + payload = message.SerializeToString() + + client_socket.sendto(payload, server_address) + + + +def run_state_machine(): + threading.Timer(STATE_MACHINE_UPDATE_MS / 1000.0, run_state_machine).start() + state_machine() + + +if __name__ == "__main__": + + run_state_machine() + + diff --git a/scripts/protobuf/urc_pb2.py b/scripts/protobuf/urc_pb2.py new file mode 100644 index 0000000..973d727 --- /dev/null +++ b/scripts/protobuf/urc_pb2.py @@ -0,0 +1,234 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: urc.proto + +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='urc.proto', + package='', + syntax='proto2', + serialized_options=None, + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\turc.proto\"\xae\x01\n\x12\x41rmEncodersMessage\x12\x19\n\x11shoulderLiftTicks\x18\x01 \x02(\x05\x12\x19\n\x11shouldSwivelTicks\x18\x02 \x02(\x05\x12\x16\n\x0e\x65lbowLiftTicks\x18\x03 \x02(\x05\x12\x18\n\x10\x65lbowSwivelTicks\x18\x04 \x02(\x05\x12\x16\n\x0ewristLiftTicks\x18\x05 \x02(\x05\x12\x18\n\x10wristSwivelTicks\x18\x06 \x02(\x05\"P\n\x14\x44riveEncodersMessage\x12\x11\n\tleftSpeed\x18\x01 \x01(\x05\x12\x12\n\nrightSpeed\x18\x02 \x01(\x05\x12\x11\n\ttimestamp\x18\x03 \x02(\x05\"\x8e\x01\n\x0eRequestMessage\x12\x14\n\x0crequestSpeed\x18\x01 \x02(\x08\x12\x1a\n\x12requestDiagnostics\x18\x02 \x02(\x08\x12\x11\n\tleftSpeed\x18\x03 \x01(\x05\x12\x12\n\nrightSpeed\x18\x04 \x01(\x05\x12\x10\n\x08\x64uration\x18\x05 \x01(\x05\x12\x11\n\ttimestamp\x18\x06 \x02(\x05' +) + + + + +_ARMENCODERSMESSAGE = _descriptor.Descriptor( + name='ArmEncodersMessage', + full_name='ArmEncodersMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='shoulderLiftTicks', full_name='ArmEncodersMessage.shoulderLiftTicks', index=0, + number=1, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='shouldSwivelTicks', full_name='ArmEncodersMessage.shouldSwivelTicks', index=1, + number=2, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='elbowLiftTicks', full_name='ArmEncodersMessage.elbowLiftTicks', index=2, + number=3, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='elbowSwivelTicks', full_name='ArmEncodersMessage.elbowSwivelTicks', index=3, + number=4, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='wristLiftTicks', full_name='ArmEncodersMessage.wristLiftTicks', index=4, + number=5, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='wristSwivelTicks', full_name='ArmEncodersMessage.wristSwivelTicks', index=5, + number=6, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=14, + serialized_end=188, +) + + +_DRIVEENCODERSMESSAGE = _descriptor.Descriptor( + name='DriveEncodersMessage', + full_name='DriveEncodersMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='leftSpeed', full_name='DriveEncodersMessage.leftSpeed', index=0, + number=1, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='rightSpeed', full_name='DriveEncodersMessage.rightSpeed', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='timestamp', full_name='DriveEncodersMessage.timestamp', index=2, + number=3, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=190, + serialized_end=270, +) + + +_REQUESTMESSAGE = _descriptor.Descriptor( + name='RequestMessage', + full_name='RequestMessage', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='requestSpeed', full_name='RequestMessage.requestSpeed', index=0, + number=1, type=8, cpp_type=7, label=2, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='requestDiagnostics', full_name='RequestMessage.requestDiagnostics', index=1, + number=2, type=8, cpp_type=7, label=2, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='leftSpeed', full_name='RequestMessage.leftSpeed', index=2, + number=3, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='rightSpeed', full_name='RequestMessage.rightSpeed', index=3, + number=4, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='duration', full_name='RequestMessage.duration', index=4, + number=5, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='timestamp', full_name='RequestMessage.timestamp', index=5, + number=6, type=5, cpp_type=1, label=2, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=273, + serialized_end=415, +) + +DESCRIPTOR.message_types_by_name['ArmEncodersMessage'] = _ARMENCODERSMESSAGE +DESCRIPTOR.message_types_by_name['DriveEncodersMessage'] = _DRIVEENCODERSMESSAGE +DESCRIPTOR.message_types_by_name['RequestMessage'] = _REQUESTMESSAGE +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +ArmEncodersMessage = _reflection.GeneratedProtocolMessageType('ArmEncodersMessage', (_message.Message,), { + 'DESCRIPTOR' : _ARMENCODERSMESSAGE, + '__module__' : 'urc_pb2' + # @@protoc_insertion_point(class_scope:ArmEncodersMessage) + }) +_sym_db.RegisterMessage(ArmEncodersMessage) + +DriveEncodersMessage = _reflection.GeneratedProtocolMessageType('DriveEncodersMessage', (_message.Message,), { + 'DESCRIPTOR' : _DRIVEENCODERSMESSAGE, + '__module__' : 'urc_pb2' + # @@protoc_insertion_point(class_scope:DriveEncodersMessage) + }) +_sym_db.RegisterMessage(DriveEncodersMessage) + +RequestMessage = _reflection.GeneratedProtocolMessageType('RequestMessage', (_message.Message,), { + 'DESCRIPTOR' : _REQUESTMESSAGE, + '__module__' : 'urc_pb2' + # @@protoc_insertion_point(class_scope:RequestMessage) + }) +_sym_db.RegisterMessage(RequestMessage) + + +# @@protoc_insertion_point(module_scope) diff --git a/src/MotorTest/main.cpp b/src/MotorTest/main.cpp new file mode 100644 index 0000000..575712c --- /dev/null +++ b/src/MotorTest/main.cpp @@ -0,0 +1,118 @@ +#include +#include "SoloCAN.hpp" + +// constants +const int BLINK_RATE_MS = 500; +const int READ_RATE_MS = 1000; +const int STATE_UPDATE_RATE_MS = 5000; +const int BAUD_RATE = 1000000; + +const int MOTOR_SPEED = 3000; +const int NUM_MOTORS = 3; +const int MOTOR_IDS[NUM_MOTORS] = {0xA4, 0xA3, 0xA6}; + +enum MotorState { + MOTOR_STATE_FORWARD, + MOTOR_STATE_BACKWARD, + MOTOR_STATE_STOPPED +}; + +// variables +FlexCAN_T4 myCan1; + +elapsedMillis blinkTimer; +elapsedMillis sendTimer; +elapsedMillis stateMachineTimer; +CAN_message_t rmsg; + +enum MotorState curr = MOTOR_STATE_STOPPED; +enum MotorState prev = MOTOR_STATE_STOPPED; + +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev); + +int main() { + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(BAUD_RATE); + + solo_can::SoloCan solo = solo_can::SoloCan(myCan1); + + while (true) { + + // parse incoming CAN messages + if (myCan1.read(rmsg)) { + struct solo_can::CanOpenData data = solo_can::parseMessage(rmsg); + + if (data.type == solo_can::SDO_READ_RESPONSE) { + if (data.code == solo_can::SPEED_FEEDBACK_CODE) { + Serial.print("Got speed feedback: "); + Serial.print("[ID="); + Serial.print(data.id, HEX); + Serial.print(", SPEED="); + Serial.print((int)data.payload); + Serial.println("]"); + } else if (data.code == solo_can::POSITION_FEEDBACK_CODE) { + Serial.print("Got position feedback: "); + int ref = data.payload; + Serial.println(ref); + } + } + } + + // set speed + if (stateMachineTimer >= STATE_UPDATE_RATE_MS) { + stateMachineTimer -= STATE_UPDATE_RATE_MS; + + // determine new state + enum MotorState newState = determineNextState(curr, prev); + + // determine motor speed + int motorSpeed; + if (newState == MOTOR_STATE_FORWARD) { + motorSpeed = MOTOR_SPEED; + } else if (newState == MOTOR_STATE_BACKWARD) { + motorSpeed = -1 * MOTOR_SPEED; + } else { + motorSpeed = 0; + } + + // send command + for (int i = 0; i < NUM_MOTORS; i++) { + solo.SetSpeedReferenceCommand(MOTOR_IDS[i], motorSpeed); + } + + + // update state + prev = curr; + curr = newState; + } + + // read speed reference + if (sendTimer >= READ_RATE_MS) { + sendTimer -= READ_RATE_MS; + + for (int i = 0; i < NUM_MOTORS; i++) { + solo.GetSpeedFeedbackCommand(MOTOR_IDS[i]); + } + } + + // blink LED + if (blinkTimer >= BLINK_RATE_MS) { + blinkTimer -= BLINK_RATE_MS; + digitalToggle(LED_BUILTIN); + } + } + +} + +enum MotorState determineNextState(enum MotorState curr, enum MotorState prev) { + if (curr == MOTOR_STATE_STOPPED) { + if (prev == MOTOR_STATE_FORWARD) { + return MOTOR_STATE_BACKWARD; + } else { + return MOTOR_STATE_FORWARD; + } + } else { + return MOTOR_STATE_STOPPED; + } +} diff --git a/src/ReadTemp/main.cpp b/src/ReadTemp/main.cpp new file mode 100644 index 0000000..e5e2932 --- /dev/null +++ b/src/ReadTemp/main.cpp @@ -0,0 +1,54 @@ +#include +#include "SoloCAN.hpp" + +const int BLINK_RATE = 500; +const int BAUD_RATE = 1000000; +const int NUM_MOTORS = 3; +const int MOTOR_IDS[NUM_MOTORS] = {0xA4, 0xA3, 0xA6}; + +FlexCAN_T4 myCan1; +elapsedMillis blinkTimer; +elapsedMillis sendTimer; +CAN_message_t rmsg; + +int main() { + + pinMode(LED_BUILTIN, OUTPUT); + myCan1.begin(); + myCan1.setBaudRate(BAUD_RATE); + + solo_can::SoloCan solo = solo_can::SoloCan(myCan1); + + while (true) { + // parse incoming CAN messages + if (myCan1.read(rmsg)) { + struct solo_can::CanOpenData data = solo_can::parseMessage(rmsg); + + if (data.type == solo_can::SDO_READ_RESPONSE) { + if (data.code == solo_can::TEMP_CODE) { + Serial.print("Got board temperature: "); + Serial.print("[ID="); + Serial.print(data.id, HEX); + Serial.print(", TEMP="); + Serial.print(solo_can::toFloat(data.payload), 7); + Serial.println("]"); + } + } + } + + // send CAN message + if (sendTimer >= BLINK_RATE) { + sendTimer -= BLINK_RATE; + for (int i = 0; i < NUM_MOTORS; i++) { + solo.GetBoardTemperatureCommand(MOTOR_IDS[i]); + } + } + + // blink LED + if (blinkTimer >= BLINK_RATE) { + blinkTimer -= BLINK_RATE; + digitalToggle(LED_BUILTIN); + } + } + +} \ No newline at end of file diff --git a/src/SoloComputerControl/main.cpp b/src/SoloComputerControl/main.cpp new file mode 100644 index 0000000..005d652 --- /dev/null +++ b/src/SoloComputerControl/main.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include "urc.pb.h" +#include "Messages.hpp" +#include "SoloCAN.hpp" + +// constants +const int BLINK_RATE_MS = 500; +const int CAN_READ_RATE_MS = 200; +const int UDP_WRITE_RATE_MS = 500; +const int BAUD_RATE = 1000000; +const int NUM_MOTORS = 6; +const int MOTOR_IDS[NUM_MOTORS] = {0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6}; +const int PORT = 8443; +const uint8_t CLIENT_IP[] = { 192, 168, 1, 151 }; + +// variables +FlexCAN_T4 can; +qindesign::network::EthernetUDP udp; +std::map encoderData; +RequestMessage requestMessage; +DriveEncodersMessage responseMessage; + +// timer variables +elapsedMillis currentTime; +elapsedMillis blinkTimer; +elapsedMillis canReadTimer; +elapsedMillis udpWriteTimer; + +int main() { + // LED setup + pinMode(LED_BUILTIN, OUTPUT); + + // Ethernet setup + qindesign::network::Ethernet.begin(); + udp.begin(PORT); + + // CAN setup + can.begin(); + can.setBaudRate(BAUD_RATE); + solo_can::SoloCan solo = solo_can::SoloCan(can); + + // initialize data + requestMessage = RequestMessage_init_zero; + responseMessage = DriveEncodersMessage_init_zero; + CAN_message_t canMsg; + solo_can::CanOpenData canResponseMessage; + size_t requestLength; + uint8_t requestBuffer[256]; + uint8_t responseBuffer[256]; + + while (true) { + + // read incoming UDP messages + requestLength = udp.parsePacket(); + if (udp.available()) { + + Serial.println("Packet received"); + + memset(requestBuffer, 0, 256); + udp.readBytes(requestBuffer, requestLength); + protobuf::Messages::decodeRequest(requestBuffer, requestLength, requestMessage); + + // write CAN + for (int i = 0; i < 3; i++) { + solo.SetSpeedReferenceCommand(MOTOR_IDS[i], requestMessage.leftSpeed); + } + + for (int i = 3; i < 6; i++) { + solo.SetSpeedReferenceCommand(MOTOR_IDS[i], requestMessage.rightSpeed); + } + } + + // check for incoming CAN messages + if (can.read(canMsg)) { + canResponseMessage = solo_can::parseMessage(canMsg); + + // record speed reference command + if (canResponseMessage.type == solo_can::SDO_READ_RESPONSE && canResponseMessage.code == solo_can::SPEED_FEEDBACK_CODE) { + encoderData[canResponseMessage.id] = canResponseMessage.payload; + } + } + + // write UDP message at regular interval + if (udpWriteTimer >= UDP_WRITE_RATE_MS) { + udpWriteTimer -= UDP_WRITE_RATE_MS; + + // populate responseMessage + responseMessage.timestamp = currentTime; + + int avgSpeedLeft = 0; + for (int i = 0; i < 3; i++) { + avgSpeedLeft += encoderData[MOTOR_IDS[i]]; + } + avgSpeedLeft /= 3; + responseMessage.leftSpeed = avgSpeedLeft; + + int avgSpeedRight = 0; + for (int i = 3; i < 6; i++) { + avgSpeedRight += encoderData[MOTOR_IDS[i]]; + } + avgSpeedRight /= 3; + responseMessage.leftSpeed = avgSpeedRight; + + size_t responseLength = protobuf::Messages::encodeResponse(responseBuffer, sizeof(responseBuffer), responseMessage); + udp.beginPacket(CLIENT_IP, PORT); + udp.write(responseBuffer, responseLength); + udp.endPacket(); + + // Serial.println("Packet sent"); + } + + // send CAN read speed reference command + if (canReadTimer >= CAN_READ_RATE_MS) { + canReadTimer -= CAN_READ_RATE_MS; + + for (int i = 0; i < NUM_MOTORS; i++) { + solo.GetSpeedFeedbackCommand(MOTOR_IDS[i]); + } + } + + // blink LED + if (blinkTimer >= BLINK_RATE_MS) { + blinkTimer -= BLINK_RATE_MS; + digitalToggle(LED_BUILTIN); + } + } +} \ No newline at end of file diff --git a/src/Stepper/examples/BidirectionalCommunication/CoolStep/CoolStep.ino b/src/Stepper/examples/BidirectionalCommunication/CoolStep/CoolStep.ino new file mode 100644 index 0000000..94cda77 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/CoolStep/CoolStep.ino @@ -0,0 +1,117 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 500; +const int32_t VELOCITY = 20000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; +const uint8_t LOOPS_BEFORE_TOGGLING = 3; +const TMC2209::CurrentIncrement CURRENT_INCREMENT = TMC2209::CURRENT_INCREMENT_8; +const TMC2209::MeasurementCount MEASUREMENT_COUNT = TMC2209::MEASUREMENT_COUNT_1; +const uint32_t COOL_STEP_DURATION_THRESHOLD = 2000; +const uint8_t COOL_STEP_LOWER_THRESHOLD = 1; +const uint8_t COOL_STEP_UPPER_THRESHOLD = 0; + +uint8_t loop_count = 0; +bool cool_step_enabled = false; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.setCoolStepCurrentIncrement(CURRENT_INCREMENT); + stepper_driver.setCoolStepMeasurementCount(MEASUREMENT_COUNT); + stepper_driver.setCoolStepDurationThreshold(COOL_STEP_DURATION_THRESHOLD); + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + if (cool_step_enabled) + { + Serial.println("cool step enabled"); + } + else + { + Serial.println("cool step disabled"); + } + + uint32_t interstep_duration = stepper_driver.getInterstepDuration(); + Serial.print("interstep_duration = "); + Serial.println(interstep_duration); + delay(DELAY); + + uint16_t stall_guard_result = stepper_driver.getStallGuardResult(); + Serial.print("stall_guard_result = "); + Serial.println(stall_guard_result); + delay(DELAY); + + uint8_t pwm_scale_sum = stepper_driver.getPwmScaleSum(); + Serial.print("pwm_scale_sum = "); + Serial.println(pwm_scale_sum); + delay(DELAY); + + int16_t pwm_scale_auto = stepper_driver.getPwmScaleAuto(); + Serial.print("pwm_scale_auto = "); + Serial.println(pwm_scale_auto); + delay(DELAY); + + uint8_t pwm_offset_auto = stepper_driver.getPwmOffsetAuto(); + Serial.print("pwm_offset_auto = "); + Serial.println(pwm_offset_auto); + delay(DELAY); + + uint8_t pwm_gradient_auto = stepper_driver.getPwmGradientAuto(); + Serial.print("pwm_gradient_auto = "); + Serial.println(pwm_gradient_auto); + delay(DELAY); + + TMC2209::Status status = stepper_driver.getStatus(); + Serial.print("status.current_scaling = "); + Serial.println(status.current_scaling); + + TMC2209::Settings settings = stepper_driver.getSettings(); + Serial.print("settings.irun_register_value = "); + Serial.println(settings.irun_register_value); + + Serial.println(); + + if (++loop_count == LOOPS_BEFORE_TOGGLING) + { + loop_count = 0; + cool_step_enabled = not cool_step_enabled; + if (cool_step_enabled) + { + stepper_driver.enableCoolStep(COOL_STEP_LOWER_THRESHOLD, + COOL_STEP_UPPER_THRESHOLD); + } + else + { + stepper_driver.disableCoolStep(); + } + } +} diff --git a/src/Stepper/examples/BidirectionalCommunication/HoldCurrent/HoldCurrent.ino b/src/Stepper/examples/BidirectionalCommunication/HoldCurrent/HoldCurrent.ino new file mode 100644 index 0000000..aa78463 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/HoldCurrent/HoldCurrent.ino @@ -0,0 +1,75 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 2000; +const int32_t VELOCITY = 10000; +const int32_t VELOCITY_STOPPED = 0; +const uint8_t PERCENT_MIN = 0; +const uint8_t PERCENT_MAX = 100; +const uint8_t PERCENT_INC = 10; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + +uint8_t hold_current_percent = PERCENT_INC; + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.enable(); + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + Serial.println("enable and run"); + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); + + Serial.print("setHoldCurrent("); + Serial.print(hold_current_percent); + Serial.println(")"); + stepper_driver.setHoldCurrent(hold_current_percent); + delay(DELAY); + + Serial.println("stop"); + stepper_driver.moveAtVelocity(VELOCITY_STOPPED); + delay(DELAY); + + uint8_t pwm_scale_sum = stepper_driver.getPwmScaleSum(); + Serial.print("pwm_scale_sum = "); + Serial.println(pwm_scale_sum); + delay(DELAY); + + stepper_driver.disable(); + Serial.println("disable"); + Serial.println(); + delay(DELAY); + + if (hold_current_percent == PERCENT_MAX) + { + hold_current_percent = PERCENT_MIN; + } + hold_current_percent += PERCENT_INC; +} diff --git a/src/Stepper/examples/BidirectionalCommunication/MicrostepsPerStep/MicrostepsPerStep.ino b/src/Stepper/examples/BidirectionalCommunication/MicrostepsPerStep/MicrostepsPerStep.ino new file mode 100644 index 0000000..10bd764 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/MicrostepsPerStep/MicrostepsPerStep.ino @@ -0,0 +1,66 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 4000; +const int32_t VELOCITY = 200; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; +const uint8_t MICROSTEPS_PER_STEP_EXPONENT_MIN = 0; +const uint8_t MICROSTEPS_PER_STEP_EXPONENT_MAX = 8; +const uint8_t MICROSTEPS_PER_STEP_EXPONENT_INC = 1; + +uint8_t microsteps_per_step_exponent = MICROSTEPS_PER_STEP_EXPONENT_MIN; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.setMicrostepsPerStepPowerOfTwo(microsteps_per_step_exponent); + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + Serial.print("setMicrostepsPerStepPowerOfTwo("); + Serial.print(microsteps_per_step_exponent); + Serial.println(")"); + stepper_driver.setMicrostepsPerStepPowerOfTwo(microsteps_per_step_exponent); + Serial.print("getMicrostepsPerStep() = "); + Serial.println(stepper_driver.getMicrostepsPerStep()); + delay(DELAY); + + uint32_t interstep_duration = stepper_driver.getInterstepDuration(); + Serial.print("interstep_duration = "); + Serial.println(interstep_duration); + Serial.println(); + + microsteps_per_step_exponent += MICROSTEPS_PER_STEP_EXPONENT_INC; + if (microsteps_per_step_exponent > MICROSTEPS_PER_STEP_EXPONENT_MAX) + { + microsteps_per_step_exponent = MICROSTEPS_PER_STEP_EXPONENT_MIN; + } +} diff --git a/src/Stepper/examples/BidirectionalCommunication/MoveAtVelocity/MoveAtVelocity.ino b/src/Stepper/examples/BidirectionalCommunication/MoveAtVelocity/MoveAtVelocity.ino new file mode 100644 index 0000000..f1c40b6 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/MoveAtVelocity/MoveAtVelocity.ino @@ -0,0 +1,72 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 2000; +const int32_t VELOCITY = 20000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + delay(DELAY); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableCoolStep(); + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + bool hardware_disabled = stepper_driver.hardwareDisabled(); + TMC2209::Settings settings = stepper_driver.getSettings(); + TMC2209::Status status = stepper_driver.getStatus(); + + if (hardware_disabled) + { + Serial.println("Stepper driver is hardware disabled!"); + } + else if (not settings.software_enabled) + { + Serial.println("Stepper driver is software disabled!"); + } + else if ((not status.standstill)) + { + Serial.print("Moving at velocity "); + Serial.println(VELOCITY); + + uint32_t interstep_duration = stepper_driver.getInterstepDuration(); + Serial.print("which is equal to an interstep_duration of "); + Serial.println(interstep_duration); + } + else + { + Serial.println("Not moving, something is wrong!"); + } + + Serial.println(); + delay(DELAY); +} diff --git a/src/Stepper/examples/BidirectionalCommunication/ReplyDelay/ReplyDelay.ino b/src/Stepper/examples/BidirectionalCommunication/ReplyDelay/ReplyDelay.ino new file mode 100644 index 0000000..f17c1d9 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/ReplyDelay/ReplyDelay.ino @@ -0,0 +1,62 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 3000; +const uint8_t REPEAT_COUNT_MAX = 2; + +// Instantiate TMC2209 +TMC2209 stepper_driver; + +uint8_t reply_delay; +uint8_t repeat_count; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + reply_delay = 0; + stepper_driver.setReplyDelay(reply_delay); + + repeat_count = 0; +} + +void loop() +{ + if (stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver is setup and communicating!"); + } + else + { + Serial.println("Stepper driver is not communicating!"); + } + Serial.print("repeat count "); + Serial.print(repeat_count); + Serial.print(" out of "); + Serial.println(REPEAT_COUNT_MAX); + Serial.print("reply delay = "); + Serial.println(reply_delay); + Serial.println(); + delay(DELAY); + + if (++repeat_count >= REPEAT_COUNT_MAX) + { + repeat_count = 0; + if (++reply_delay >= stepper_driver.REPLY_DELAY_MAX) + { + reply_delay = 0; + } + stepper_driver.setReplyDelay(reply_delay); + } +} diff --git a/src/Stepper/examples/BidirectionalCommunication/RunCurrent/RunCurrent.ino b/src/Stepper/examples/BidirectionalCommunication/RunCurrent/RunCurrent.ino new file mode 100644 index 0000000..753de8b --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/RunCurrent/RunCurrent.ino @@ -0,0 +1,63 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +// Identify which microcontroller serial port is connected to the TMC2209 +// e.g. Serial1, Serial2... +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 2000; +const int32_t VELOCITY = 10000; +const uint8_t PERCENT_MIN = 0; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t PERCENT_MAX = 100; +const uint8_t PERCENT_INC = 10; + +uint8_t run_current_percent = PERCENT_INC; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + Serial.print("setRunCurrent("); + Serial.print(run_current_percent); + Serial.println(")"); + stepper_driver.setRunCurrent(run_current_percent); + delay(DELAY); + + TMC2209::Status status = stepper_driver.getStatus(); + Serial.print("status.current_scaling = "); + Serial.println(status.current_scaling); + Serial.println(); + + if (run_current_percent == PERCENT_MAX) + { + run_current_percent = PERCENT_MIN; + } + run_current_percent += PERCENT_INC; +} diff --git a/src/Stepper/examples/BidirectionalCommunication/SettingsAndStatus/SettingsAndStatus.ino b/src/Stepper/examples/BidirectionalCommunication/SettingsAndStatus/SettingsAndStatus.ino new file mode 100644 index 0000000..c539609 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/SettingsAndStatus/SettingsAndStatus.ino @@ -0,0 +1,133 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 2000; + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); +} + +void loop() +{ + Serial.println("*************************"); + Serial.println("getSettings()"); + TMC2209::Settings settings = stepper_driver.getSettings(); + Serial.print("settings.is_communicating = "); + Serial.println(settings.is_communicating); + Serial.print("settings.is_setup = "); + Serial.println(settings.is_setup); + Serial.print("settings.software_enabled = "); + Serial.println(settings.software_enabled); + Serial.print("settings.microsteps_per_step = "); + Serial.println(settings.microsteps_per_step); + Serial.print("settings.inverse_motor_direction_enabled = "); + Serial.println(settings.inverse_motor_direction_enabled); + Serial.print("settings.stealth_chop_enabled = "); + Serial.println(settings.stealth_chop_enabled); + Serial.print("settings.standstill_mode = "); + switch (settings.standstill_mode) + { + case TMC2209::NORMAL: + Serial.println("normal"); + break; + case TMC2209::FREEWHEELING: + Serial.println("freewheeling"); + break; + case TMC2209::STRONG_BRAKING: + Serial.println("strong_braking"); + break; + case TMC2209::BRAKING: + Serial.println("braking"); + break; + } + Serial.print("settings.irun_percent = "); + Serial.println(settings.irun_percent); + Serial.print("settings.irun_register_value = "); + Serial.println(settings.irun_register_value); + Serial.print("settings.ihold_percent = "); + Serial.println(settings.ihold_percent); + Serial.print("settings.ihold_register_value = "); + Serial.println(settings.ihold_register_value); + Serial.print("settings.iholddelay_percent = "); + Serial.println(settings.iholddelay_percent); + Serial.print("settings.iholddelay_register_value = "); + Serial.println(settings.iholddelay_register_value); + Serial.print("settings.automatic_current_scaling_enabled = "); + Serial.println(settings.automatic_current_scaling_enabled); + Serial.print("settings.automatic_gradient_adaptation_enabled = "); + Serial.println(settings.automatic_gradient_adaptation_enabled); + Serial.print("settings.pwm_offset = "); + Serial.println(settings.pwm_offset); + Serial.print("settings.pwm_gradient = "); + Serial.println(settings.pwm_gradient); + Serial.print("settings.cool_step_enabled = "); + Serial.println(settings.cool_step_enabled); + Serial.print("settings.analog_current_scaling_enabled = "); + Serial.println(settings.analog_current_scaling_enabled); + Serial.print("settings.internal_sense_resistors_enabled = "); + Serial.println(settings.internal_sense_resistors_enabled); + Serial.println("*************************"); + Serial.println(); + + Serial.println("*************************"); + Serial.println("hardwareDisabled()"); + bool hardware_disabled = stepper_driver.hardwareDisabled(); + Serial.print("hardware_disabled = "); + Serial.println(hardware_disabled); + Serial.println("*************************"); + Serial.println(); + + Serial.println("*************************"); + Serial.println("getStatus()"); + TMC2209::Status status = stepper_driver.getStatus(); + Serial.print("status.over_temperature_warning = "); + Serial.println(status.over_temperature_warning); + Serial.print("status.over_temperature_shutdown = "); + Serial.println(status.over_temperature_shutdown); + Serial.print("status.short_to_ground_a = "); + Serial.println(status.short_to_ground_a); + Serial.print("status.short_to_ground_b = "); + Serial.println(status.short_to_ground_b); + Serial.print("status.low_side_short_a = "); + Serial.println(status.low_side_short_a); + Serial.print("status.low_side_short_b = "); + Serial.println(status.low_side_short_b); + Serial.print("status.open_load_a = "); + Serial.println(status.open_load_a); + Serial.print("status.open_load_b = "); + Serial.println(status.open_load_b); + Serial.print("status.over_temperature_120c = "); + Serial.println(status.over_temperature_120c); + Serial.print("status.over_temperature_143c = "); + Serial.println(status.over_temperature_143c); + Serial.print("status.over_temperature_150c = "); + Serial.println(status.over_temperature_150c); + Serial.print("status.over_temperature_157c = "); + Serial.println(status.over_temperature_157c); + Serial.print("status.current_scaling = "); + Serial.println(status.current_scaling); + Serial.print("status.stealth_chop_mode = "); + Serial.println(status.stealth_chop_mode); + Serial.print("status.standstill = "); + Serial.println(status.standstill); + Serial.println("*************************"); + Serial.println(); + + Serial.println(); + delay(DELAY); +} diff --git a/src/Stepper/examples/BidirectionalCommunication/StallGuard/StallGuard.ino b/src/Stepper/examples/BidirectionalCommunication/StallGuard/StallGuard.ino new file mode 100644 index 0000000..ed6793e --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/StallGuard/StallGuard.ino @@ -0,0 +1,57 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 200; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; +const int32_t VELOCITY = 20000; +const uint8_t STALL_GUARD_THRESHOLD = 50; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.setStallGuardThreshold(STALL_GUARD_THRESHOLD); + stepper_driver.enable(); + stepper_driver.moveAtVelocity(VELOCITY); +} + +void loop() +{ + if (not stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver not setup and communicating!"); + return; + } + + Serial.print("run_current_percent = "); + Serial.println(RUN_CURRENT_PERCENT); + + Serial.print("stall_guard_threshold = "); + Serial.println(STALL_GUARD_THRESHOLD); + + uint16_t stall_guard_result = stepper_driver.getStallGuardResult(); + Serial.print("stall_guard_result = "); + Serial.println(stall_guard_result); + + Serial.println(); + delay(DELAY); + +} diff --git a/src/Stepper/examples/BidirectionalCommunication/TestBaudRate/TestBaudRate.ino b/src/Stepper/examples/BidirectionalCommunication/TestBaudRate/TestBaudRate.ino new file mode 100644 index 0000000..5c82f84 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/TestBaudRate/TestBaudRate.ino @@ -0,0 +1,98 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const long SERIAL1_BAUD_RATE_COUNT = 10; +const long SERIAL1_BAUD_RATES[SERIAL1_BAUD_RATE_COUNT] = +{ + 500000, + 250000, + 115200, + 57600, + 38400, + 31250, + 28800, + 19200, + 14400, + 9600 +}; +const uint8_t SUCCESSIVE_OPERATION_COUNT = 3; +const int DELAY = 2000; + +// Instantiate TMC2209 +TMC2209 stepper_driver; +uint8_t serial1_baud_rate_index = 0; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); +} + +void loop() +{ + long serial1_baud_rate = SERIAL1_BAUD_RATES[serial1_baud_rate_index++]; + stepper_driver.setup(serial_stream,serial1_baud_rate); + if (serial1_baud_rate_index == SERIAL1_BAUD_RATE_COUNT) + { + serial1_baud_rate_index = 0; + } + + bool test_further = false; + + Serial.println("*************************"); + Serial.print("serial1_baud_rate = "); + Serial.println(serial1_baud_rate); + + if (stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver setup and communicating!"); + test_further = true; + } + else + { + Serial.println("Stepper driver not setup and communicating!"); + } + + if (test_further) + { + uint32_t microstep_sum = 0; + for (uint8_t i=0; i 0) + { + Serial.println("Successive read test passed!"); + } + else + { + Serial.println("Successive read test failed!"); + } + uint8_t itc_begin = stepper_driver.getInterfaceTransmissionCounter(); + for (uint8_t i=0; i + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 3000; + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); +} + +void loop() +{ + if (stepper_driver.isSetupAndCommunicating()) + { + Serial.println("Stepper driver is setup and communicating!"); + Serial.println("Try turning driver power off to see what happens."); + } + else if (stepper_driver.isCommunicatingButNotSetup()) + { + Serial.println("Stepper driver is communicating but not setup!"); + Serial.println("Running setup again..."); + stepper_driver.setup(serial_stream); + } + else + { + Serial.println("Stepper driver is not communicating!"); + Serial.println("Try turning driver power on to see what happens."); + } + Serial.println(); + delay(DELAY); +} diff --git a/src/Stepper/examples/BidirectionalCommunication/TestDisconnection/TestDisconnection.ino b/src/Stepper/examples/BidirectionalCommunication/TestDisconnection/TestDisconnection.ino new file mode 100644 index 0000000..f0c8ce4 --- /dev/null +++ b/src/Stepper/examples/BidirectionalCommunication/TestDisconnection/TestDisconnection.ino @@ -0,0 +1,90 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 6000; + +const int LOOP_COUNT = 100; + +unsigned long time_begin; +unsigned long time_end; +TMC2209::Status status; + +typedef void (*DriverFunction)(void); + +// Instantiate TMC2209 +TMC2209 stepper_driver; + +void driverWriteFunction() +{ + stepper_driver.enable(); +} + +void driverReadFunction() +{ + status = stepper_driver.getStatus(); +} + +void runTestLoop(DriverFunction driver_function) +{ + time_begin = millis(); + for (uint16_t i=0; i + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +HardwareSerial & serial_stream = Serial3; +const uint8_t HARDWARE_ENABLE_PIN = 4; + +const int32_t RUN_VELOCITY = 20000; +const int32_t STOP_VELOCITY = 0; +const int RUN_DURATION = 2000; +const int STOP_DURATION = 1000; +const int RUN_COUNT = 4; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; +bool invert_direction = false; + +void setup() +{ + stepper_driver.setup(serial_stream); + + stepper_driver.setHardwareEnablePin(HARDWARE_ENABLE_PIN); + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableCoolStep(); + stepper_driver.enable(); +} + +void loop() +{ + stepper_driver.moveAtVelocity(STOP_VELOCITY); + delay(STOP_DURATION); + if (invert_direction) + { + stepper_driver.enableInverseMotorDirection(); + } + else + { + stepper_driver.disableInverseMotorDirection(); + } + invert_direction = not invert_direction; + + stepper_driver.moveAtVelocity(RUN_VELOCITY); + + for (uint8_t run=0; run + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +HardwareSerial & serial_stream = Serial3; + +const int32_t RUN_VELOCITY = 20000; +const int32_t STOP_VELOCITY = 0; +const int RUN_DURATION = 2000; +const int STOP_DURATION = 1000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; +bool invert_direction = false; + +void setup() +{ + stepper_driver.setup(serial_stream); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableCoolStep(); + stepper_driver.enable(); +} + +void loop() +{ + stepper_driver.moveAtVelocity(STOP_VELOCITY); + delay(STOP_DURATION); + if (invert_direction) + { + stepper_driver.enableInverseMotorDirection(); + } + else + { + stepper_driver.disableInverseMotorDirection(); + } + invert_direction = not invert_direction; + + stepper_driver.moveAtVelocity(RUN_VELOCITY); + + delay(RUN_DURATION); +} diff --git a/src/Stepper/examples/UnidirectionalCommunication/SoftwareSerial/SoftwareSerial.ino b/src/Stepper/examples/UnidirectionalCommunication/SoftwareSerial/SoftwareSerial.ino new file mode 100644 index 0000000..ba93f1a --- /dev/null +++ b/src/Stepper/examples/UnidirectionalCommunication/SoftwareSerial/SoftwareSerial.ino @@ -0,0 +1,55 @@ +#include + +// SoftwareSerial can be used on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ + +// Software serial ports should only be used for unidirectional communication +// The RX pin does not need to be connected, but it must be specified when +// creating an instance of a SoftwareSerial object +const uint8_t RX_PIN = 15; +const uint8_t TX_PIN = 14; +SoftwareSerial soft_serial(RX_PIN, TX_PIN); + +const int32_t RUN_VELOCITY = 20000; +const int32_t STOP_VELOCITY = 0; +const int RUN_DURATION = 2000; +const int STOP_DURATION = 1000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; +bool invert_direction = false; + +void setup() +{ + stepper_driver.setup(soft_serial); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableCoolStep(); + stepper_driver.enable(); +} + +void loop() +{ + stepper_driver.moveAtVelocity(STOP_VELOCITY); + delay(STOP_DURATION); + if (invert_direction) + { + stepper_driver.enableInverseMotorDirection(); + } + else + { + stepper_driver.disableInverseMotorDirection(); + } + invert_direction = not invert_direction; + + stepper_driver.moveAtVelocity(RUN_VELOCITY); + + delay(RUN_DURATION); +} diff --git a/src/Stepper/examples/UnidirectionalCommunication/Standstill/Standstill.ino b/src/Stepper/examples/UnidirectionalCommunication/Standstill/Standstill.ino new file mode 100644 index 0000000..83ce480 --- /dev/null +++ b/src/Stepper/examples/UnidirectionalCommunication/Standstill/Standstill.ino @@ -0,0 +1,55 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +HardwareSerial & serial_stream = Serial3; + +const long SERIAL_BAUD_RATE = 115200; +const int DELAY = 4000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; +const uint8_t HOLD_CURRENT_STANDSTILL = 0; + +// Instantiate TMC2209 +TMC2209 stepper_driver; + + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.setHoldCurrent(HOLD_CURRENT_STANDSTILL); + stepper_driver.enable(); +} + +void loop() +{ + Serial.println("standstill mode = NORMAL"); + stepper_driver.setStandstillMode(stepper_driver.NORMAL); + delay(DELAY); + + Serial.println("standstill mode = FREEWHEELING"); + stepper_driver.setStandstillMode(stepper_driver.FREEWHEELING); + delay(DELAY); + + Serial.println("standstill mode = STRONG_BRAKING"); + stepper_driver.setStandstillMode(stepper_driver.STRONG_BRAKING); + delay(DELAY); + + Serial.println("standstill mode = BRAKING"); + stepper_driver.setStandstillMode(stepper_driver.BRAKING); + delay(DELAY); + + Serial.println(); +} diff --git a/src/Stepper/examples/UnidirectionalCommunication/StepAndDirection/StepAndDirection.ino b/src/Stepper/examples/UnidirectionalCommunication/StepAndDirection/StepAndDirection.ino new file mode 100644 index 0000000..c7736c8 --- /dev/null +++ b/src/Stepper/examples/UnidirectionalCommunication/StepAndDirection/StepAndDirection.ino @@ -0,0 +1,49 @@ +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +HardwareSerial & serial_stream = Serial3; + +const uint8_t STEP_PIN = 2; +const uint8_t DIRECTION_PIN = 3; +const uint32_t STEP_COUNT = 51200; +const uint16_t HALF_STEP_DURATION_MICROSECONDS = 10; +const uint16_t STOP_DURATION = 1000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + + +// Instantiate TMC2209 +TMC2209 stepper_driver; + +void setup() +{ + stepper_driver.setup(serial_stream); + + pinMode(STEP_PIN, OUTPUT); + pinMode(DIRECTION_PIN, OUTPUT); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableCoolStep(); + stepper_driver.enable(); +} + +void loop() +{ + // One step takes two iterations through the for loop + for (uint32_t i=0; i +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +const long SERIAL_BAUD_RATE = 115200; +const int BETWEEN_MOVE_DELAY = 2000; +const int CHECK_AT_POSITION_DELAY = 500; + +// Stepper driver settings +HardwareSerial & serial_stream = Serial3; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const int RUN_CURRENT_PERCENT = 100; +const int MICROSTEPS_PER_STEP = 256; + +// Instantiate stepper driver +TMC2209 stepper_driver; + +// Stepper controller settings +const int CHIP_SELECT_PIN = 10; +const int CLOCK_FREQUENCY_MHZ = 32; +const int MOTOR_INDEX = 0; +const int STEPS_PER_REV = 200; +const int MICROSTEPS_PER_REV = STEPS_PER_REV*MICROSTEPS_PER_STEP; +const long REVS_PER_MOVE = 10; +const long TARGET_POSITION = REVS_PER_MOVE * MICROSTEPS_PER_REV; +const int ACCELERATION_MAX = 2 * MICROSTEPS_PER_REV; +const long ZERO_POSITION = 0; +const long VELOCITY_MAX = 2 * MICROSTEPS_PER_REV; +const long VELOCITY_MIN = 500; + +// Instantiate stepper controller +TMC429 stepper_controller; + +long target_position, actual_position; + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableAutomaticCurrentScaling(); + stepper_driver.enableAutomaticGradientAdaptation(); + stepper_driver.enableCoolStep(); + + stepper_controller.setup(CHIP_SELECT_PIN, CLOCK_FREQUENCY_MHZ); + stepper_controller.disableLeftSwitchStop(MOTOR_INDEX); + stepper_controller.disableRightSwitches(); + stepper_controller.disableRightSwitchStop(MOTOR_INDEX); + stepper_controller.setLimitsInHz(MOTOR_INDEX, VELOCITY_MIN, VELOCITY_MAX, ACCELERATION_MAX); + stepper_controller.setRampMode(MOTOR_INDEX); + + stepper_controller.setActualPosition(MOTOR_INDEX, ZERO_POSITION); + stepper_controller.setTargetPosition(MOTOR_INDEX, ZERO_POSITION); + + stepper_driver.enable(); +} + +void loop() +{ + actual_position = stepper_controller.getActualPosition(MOTOR_INDEX); + Serial.print("actual position: "); + Serial.println(actual_position); + Serial.println(); + delay(BETWEEN_MOVE_DELAY); + + Serial.print("set target position: "); + Serial.println(TARGET_POSITION); + stepper_controller.setTargetPosition(MOTOR_INDEX, TARGET_POSITION); + while (!stepper_controller.atTargetPosition(MOTOR_INDEX)) + { + Serial.print("target position: "); + Serial.println(stepper_controller.getTargetPosition(MOTOR_INDEX)); + Serial.print("actual position: "); + Serial.println(stepper_controller.getActualPosition(MOTOR_INDEX)); + delay(CHECK_AT_POSITION_DELAY); + } + Serial.println("at target position!"); + Serial.println(); + delay(BETWEEN_MOVE_DELAY); + + Serial.print("set target position: "); + Serial.println(ZERO_POSITION); + stepper_controller.setTargetPosition(MOTOR_INDEX, ZERO_POSITION); + while (!stepper_controller.atTargetPosition(MOTOR_INDEX)) + { + Serial.print("target position: "); + Serial.println(stepper_controller.getTargetPosition(MOTOR_INDEX)); + Serial.print("actual position: "); + Serial.println(stepper_controller.getActualPosition(MOTOR_INDEX)); + delay(CHECK_AT_POSITION_DELAY); + } + Serial.println(); + delay(BETWEEN_MOVE_DELAY); +} diff --git a/src/Stepper/examples/UnidirectionalCommunication/TMC429ThreeSteppers/TMC429ThreeSteppers.ino b/src/Stepper/examples/UnidirectionalCommunication/TMC429ThreeSteppers/TMC429ThreeSteppers.ino new file mode 100644 index 0000000..7295931 --- /dev/null +++ b/src/Stepper/examples/UnidirectionalCommunication/TMC429ThreeSteppers/TMC429ThreeSteppers.ino @@ -0,0 +1,126 @@ +#include +#include + +// This example will not work on Arduino boards without at least three +// HardwareSerial ports. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +const long SERIAL_BAUD_RATE = 115200; +const int BETWEEN_MOVE_DELAY = 2000; +const int CHECK_AT_POSITION_DELAY = 500; + +// Stepper drivers settings +enum{MOTOR_COUNT=3}; +HardwareSerial * serial_stream_ptrs[MOTOR_COUNT] = +{ + &Serial1, + &Serial2, + &Serial3, +}; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const int RUN_CURRENT_PERCENT = 100; +const int MICROSTEPS_PER_STEP = 256; + +// Instantiate stepper drivers +TMC2209 stepper_drivers[MOTOR_COUNT]; + +// Stepper controller settings +const int CHIP_SELECT_PIN = 10; +const int CLOCK_FREQUENCY_MHZ = 32; +const int MOTOR_INDEX = 0; +const int STEPS_PER_REV = 200; +const int MICROSTEPS_PER_REV = STEPS_PER_REV*MICROSTEPS_PER_STEP; +const long REVS_PER_MOVE = 10; +const long TARGET_POSITION = REVS_PER_MOVE * MICROSTEPS_PER_REV; +const int ACCELERATION_MAX = 2 * MICROSTEPS_PER_REV; +const long ZERO_POSITION = 0; +const long VELOCITY_MAX = 2 * MICROSTEPS_PER_REV; +const long VELOCITY_MIN = 500; + +// Instantiate stepper controller +TMC429 stepper_controller; + +long target_position, actual_position; + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + for (size_t motor_index=0; motor_index +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +const long SERIAL_BAUD_RATE = 115200; +const int LOOP_DELAY = 1000; + +// Stepper driver settings +HardwareSerial & serial_stream = Serial3; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const int RUN_CURRENT_PERCENT = 100; +const int MICROSTEPS_PER_STEP = 256; + +// Instantiate stepper driver +TMC2209 stepper_driver; + +// Stepper controller settings +const int CHIP_SELECT_PIN = 10; +const int CLOCK_FREQUENCY_MHZ = 32; +const int MOTOR_INDEX = 0; +const int STEPS_PER_REV = 200; +const int REVS_PER_SEC_MAX = 2; +const int MICROSTEPS_PER_REV = STEPS_PER_REV*MICROSTEPS_PER_STEP; +const int ACCELERATION_MAX = MICROSTEPS_PER_REV / 2; +const long VELOCITY_MAX = REVS_PER_SEC_MAX * MICROSTEPS_PER_REV; +const long VELOCITY_MIN = 50; + +// Instantiate stepper controller +TMC429 stepper_controller; + +long target_velocity, actual_velocity; + +void setup() +{ + Serial.begin(SERIAL_BAUD_RATE); + + stepper_driver.setup(serial_stream); + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + stepper_driver.enableAutomaticCurrentScaling(); + stepper_driver.enableAutomaticGradientAdaptation(); + stepper_driver.enableCoolStep(); + + stepper_controller.setup(CHIP_SELECT_PIN, CLOCK_FREQUENCY_MHZ); + stepper_controller.disableLeftSwitchStop(MOTOR_INDEX); + stepper_controller.disableRightSwitches(); + stepper_controller.setVelocityMode(MOTOR_INDEX); + stepper_controller.setLimitsInHz(MOTOR_INDEX, VELOCITY_MIN, VELOCITY_MAX, ACCELERATION_MAX); + + stepper_driver.enable(); +} + +void loop() +{ + delay(LOOP_DELAY); + + stepper_controller.setTargetVelocityInHz(MOTOR_INDEX, VELOCITY_MAX); + Serial.print("set target_velocity: "); + Serial.println(target_velocity); + Serial.print("at target_velocity: "); + Serial.println(stepper_controller.atTargetVelocity(MOTOR_INDEX)); + + target_velocity = stepper_controller.getTargetVelocityInHz(MOTOR_INDEX); + + actual_velocity = stepper_controller.getActualVelocityInHz(MOTOR_INDEX); + Serial.print("actual_velocity: "); + Serial.println(actual_velocity); +} diff --git a/src/Stepper/main.cpp b/src/Stepper/main.cpp new file mode 100644 index 0000000..1be4679 --- /dev/null +++ b/src/Stepper/main.cpp @@ -0,0 +1,57 @@ +#include +#include + +// This example will not work on Arduino boards without HardwareSerial ports, +// such as the Uno, Nano, and Mini. +// +// See this reference for more details: +// https://www.arduino.cc/reference/en/language/functions/communication/serial/ +// +// To make this library work with those boards, refer to this library example: +// examples/UnidirectionalCommunication/SoftwareSerial + +HardwareSerial & serial_stream = Serial2; +// const uint8_t RX_PIN = 15; +// const uint8_t TX_PIN = 14; +// SoftwareSerial soft_serial(RX_PIN, TX_PIN); + +const int32_t RUN_VELOCITY = 10000; +const int32_t STOP_VELOCITY = 0; +const int RUN_DURATION = 2000; +const int STOP_DURATION = 1000; +// current values may need to be reduced to prevent overheating depending on +// specific motor and power supply voltage +const uint8_t RUN_CURRENT_PERCENT = 100; + +TMC2209 stepper_driver; +bool invert_direction = false; + +void setup() +{ + stepper_driver.setup(serial_stream); + + stepper_driver.setRunCurrent(RUN_CURRENT_PERCENT); + // stepper_driver.enableCoolStep(); + stepper_driver.enable(); +} + +void loop() +{ + Serial.printf("%d\n", stepper_driver.getStatus()); + + stepper_driver.moveAtVelocity(STOP_VELOCITY); + delay(STOP_DURATION); + if (invert_direction) + { + stepper_driver.enableInverseMotorDirection(); + } + else + { + stepper_driver.disableInverseMotorDirection(); + } + invert_direction = !invert_direction; + + stepper_driver.moveAtVelocity(RUN_VELOCITY); + + delay(RUN_DURATION); +} \ No newline at end of file diff --git a/src/Drivers/EthernetDriver.cpp b/src/main/Drivers/EthernetDriver.cpp similarity index 100% rename from src/Drivers/EthernetDriver.cpp rename to src/main/Drivers/EthernetDriver.cpp diff --git a/src/Drivers/RoboClawController.cpp b/src/main/Drivers/RoboClawController.cpp similarity index 100% rename from src/Drivers/RoboClawController.cpp rename to src/main/Drivers/RoboClawController.cpp diff --git a/src/Managers/EthernetManager.cpp b/src/main/Managers/EthernetManager.cpp similarity index 100% rename from src/Managers/EthernetManager.cpp rename to src/main/Managers/EthernetManager.cpp diff --git a/src/Managers/MotorManager.cpp b/src/main/Managers/MotorManager.cpp similarity index 100% rename from src/Managers/MotorManager.cpp rename to src/main/Managers/MotorManager.cpp diff --git a/src/main.cpp b/src/main/main.cpp similarity index 100% rename from src/main.cpp rename to src/main/main.cpp