Steffen Brucker
2 years ago
commit
4bc7e99780
10 changed files with 921 additions and 0 deletions
-
5.gitignore
-
10.vscode/extensions.json
-
38include/M034.h
-
66include/Pin_Definitions.h
-
39include/README
-
46lib/README
-
16platformio.ini
-
58src/M034.cpp
-
632src/main.cpp
-
11test/README
@ -0,0 +1,5 @@ |
|||||
|
.pio |
||||
|
.vscode/.browse.c_cpp.db* |
||||
|
.vscode/c_cpp_properties.json |
||||
|
.vscode/launch.json |
||||
|
.vscode/ipch |
@ -0,0 +1,10 @@ |
|||||
|
{ |
||||
|
// See http://go.microsoft.com/fwlink/?LinkId=827846 |
||||
|
// for the documentation about the extensions.json format |
||||
|
"recommendations": [ |
||||
|
"platformio.platformio-ide" |
||||
|
], |
||||
|
"unwantedRecommendations": [ |
||||
|
"ms-vscode.cpptools-extension-pack" |
||||
|
] |
||||
|
} |
@ -0,0 +1,38 @@ |
|||||
|
/* |
||||
|
* M034.h - Library for M034 |
||||
|
* Created by brucker 22.03.2023 |
||||
|
*/ |
||||
|
|
||||
|
#ifndef M034_H |
||||
|
#define M034_H |
||||
|
|
||||
|
#include <Arduino.h> // main arduino library |
||||
|
#include <SPI.h> // Arduino SPI library |
||||
|
/* |
||||
|
#include "Pin_Definitions.h" // pin definitions |
||||
|
#include <Ponoor_PowerSTEP01Library.h> // stepper motor shield library |
||||
|
#include <SPI.h> // Arduino SPI library |
||||
|
#include <EEPROM.h> // Arduino EEPROM library |
||||
|
*/ |
||||
|
|
||||
|
/********************************************************************** |
||||
|
* classes * |
||||
|
**********************************************************************/ |
||||
|
// encoder position class |
||||
|
class NanotecAbsolutEncoder{ |
||||
|
public: |
||||
|
NanotecAbsolutEncoder(SPIClass *SPIPort); |
||||
|
void readPosition(void); |
||||
|
uint16_t getMultiturn(void); |
||||
|
uint32_t getSingleturn(void); |
||||
|
uint8_t getStatus(void); |
||||
|
private: |
||||
|
uint16_t _startbits = 0; |
||||
|
uint16_t _multiturn = 0; |
||||
|
uint32_t _singleturn = 0; |
||||
|
uint8_t _status = 0; |
||||
|
uint8_t _readBytes[7]; |
||||
|
SPIClass *_SPI; |
||||
|
}; |
||||
|
|
||||
|
#endif |
@ -0,0 +1,66 @@ |
|||||
|
/* |
||||
|
* Pin Definitins for ST Nucleo-F429ZI 144 Pin |
||||
|
* Created by brucker 21.03.2023 |
||||
|
*/ |
||||
|
|
||||
|
#ifndef M034_PIN_DEFINITIONS_H |
||||
|
#define M034_PIN_DEFINITIONS_H |
||||
|
// Pin Numbers: Nucleo Pin Name // Nucleo Connector Pin, STM32 Pin, Function |
||||
|
// Pin definitions |
||||
|
|
||||
|
// I2C |
||||
|
int const PIN_I2C_SDA = D20; // CN7:4, PB9 |
||||
|
int const PIN_I2C_SCL = D21; // CN7:2, PB8 |
||||
|
|
||||
|
// SPI1 (Arduino compatible) |
||||
|
int const PIN_SPI_1_MOSI = D11; // CN7:14, PA7 |
||||
|
int const PIN_SPI_1_MISO = D12; // CN7:12, PA6 |
||||
|
int const PIN_SPI_1_CLK = D13; // CN7:10, PA5 |
||||
|
int const PIN_SPI_1_SS = D10; // CN7:16, PD14 |
||||
|
// SPI3 |
||||
|
int const PIN_SPI_3_MOSI = PB5_ALT1;// CN7:13, PB5, D22 |
||||
|
int const PIN_SPI_3_MISO = PB4_ALT1;// CN7:19, PB4, D25 |
||||
|
int const PIN_SPI_3_CLK = PB3_ALT1; // CN7:15, PB3, D23 |
||||
|
int const PIN_SPI_3_SS = D24; // CN7:17, PA4 |
||||
|
// SPI4 |
||||
|
int const PIN_SPI_4_MOSI = D59; // CN9:20, PE6 |
||||
|
int const PIN_SPI_4_MISO = D58; // CN9:18, PE5 |
||||
|
int const PIN_SPI_4_CLK = D39; // CN10:26, PE12 |
||||
|
int const PIN_SPI_4_SS = D57; // CN9:16, PE4 |
||||
|
// SPI5 |
||||
|
int const PIN_SPI_5_MOSI = D63; // CN9:28, PF9 |
||||
|
int const PIN_SPI_5_MISO = D61; // CN9:24, PF8 |
||||
|
int const PIN_SPI_5_CLK = D62; // CN9:26, PF7 |
||||
|
|
||||
|
// Pin definitions for the X-NUCLEO-IHM03A1 |
||||
|
int const PIN_STEPPER_FLAG = D2; // CN10:12, PF15 |
||||
|
int const PIN_STEPPER_BUSY = D4; // CN10:8, PF14 |
||||
|
int const PIN_STEPPER_RESET = D8; // CN7:20, PF12 |
||||
|
int const PIN_STEPPER_STEP_CLK = D9;// CN7:18, PD15, SPI1 |
||||
|
int const PIN_STEPPER_SPI_SS = D10; // CN7:16, PD14, SPI1 |
||||
|
// naming scheme of stepper motor library |
||||
|
int const nBUSY_PIN = D4; |
||||
|
int const nSTBY_nRESET_PIN = D8; |
||||
|
int const STCK_PIN = D9; |
||||
|
int const nCS_PIN = D10; |
||||
|
|
||||
|
// RGB-LED |
||||
|
int const PIN_RGB_LED_R = D6; // CN10:4, PE9 |
||||
|
int const PIN_RGB_LED_G = D5; // CN10:6, PE11 |
||||
|
int const PIN_RGB_LED_B = D3; // CN10:10, PE13 |
||||
|
|
||||
|
// Optical UART |
||||
|
int const PIN_OPTICAL_RX = D0; // CN10:16, PG9, USART6 |
||||
|
int const PIN_OPTICAL_TX = D1; // CN10:14, PG14, USART6 |
||||
|
|
||||
|
// TTL input on Lemo for position controll |
||||
|
int const PIN_POS1 = A3; // CN9:7, PF3 |
||||
|
int const PIN_POS2 = A4; // CN9:9, PF5 |
||||
|
int const PIN_POS3 = A5; // CN9:11, PF10 |
||||
|
|
||||
|
// ADC inputs |
||||
|
int const PIN_ADC_5V = A1; // CN9:3, PC0 |
||||
|
int const PIN_ADV_VIN = A2; // CN9:5, PC3 |
||||
|
int const PIN_ADC_I_M = A0; // CN9:1, PA3 |
||||
|
|
||||
|
#endif //M034_PIN_DEFINITIONS_H |
@ -0,0 +1,39 @@ |
|||||
|
|
||||
|
This directory is intended for project header files. |
||||
|
|
||||
|
A header file is a file containing C declarations and macro definitions |
||||
|
to be shared between several project source files. You request the use of a |
||||
|
header file in your project source file (C, C++, etc) located in `src` folder |
||||
|
by including it, with the C preprocessing directive `#include'. |
||||
|
|
||||
|
```src/main.c |
||||
|
|
||||
|
#include "header.h" |
||||
|
|
||||
|
int main (void) |
||||
|
{ |
||||
|
... |
||||
|
} |
||||
|
``` |
||||
|
|
||||
|
Including a header file produces the same results as copying the header file |
||||
|
into each source file that needs it. Such copying would be time-consuming |
||||
|
and error-prone. With a header file, the related declarations appear |
||||
|
in only one place. If they need to be changed, they can be changed in one |
||||
|
place, and programs that include the header file will automatically use the |
||||
|
new version when next recompiled. The header file eliminates the labor of |
||||
|
finding and changing all the copies as well as the risk that a failure to |
||||
|
find one copy will result in inconsistencies within a program. |
||||
|
|
||||
|
In C, the usual convention is to give header files names that end with `.h'. |
||||
|
It is most portable to use only letters, digits, dashes, and underscores in |
||||
|
header file names, and at most one dot. |
||||
|
|
||||
|
Read more about using header files in official GCC documentation: |
||||
|
|
||||
|
* Include Syntax |
||||
|
* Include Operation |
||||
|
* Once-Only Headers |
||||
|
* Computed Includes |
||||
|
|
||||
|
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html |
@ -0,0 +1,46 @@ |
|||||
|
|
||||
|
This directory is intended for project specific (private) libraries. |
||||
|
PlatformIO will compile them to static libraries and link into executable file. |
||||
|
|
||||
|
The source code of each library should be placed in a an own separate directory |
||||
|
("lib/your_library_name/[here are source files]"). |
||||
|
|
||||
|
For example, see a structure of the following two libraries `Foo` and `Bar`: |
||||
|
|
||||
|
|--lib |
||||
|
| | |
||||
|
| |--Bar |
||||
|
| | |--docs |
||||
|
| | |--examples |
||||
|
| | |--src |
||||
|
| | |- Bar.c |
||||
|
| | |- Bar.h |
||||
|
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html |
||||
|
| | |
||||
|
| |--Foo |
||||
|
| | |- Foo.c |
||||
|
| | |- Foo.h |
||||
|
| | |
||||
|
| |- README --> THIS FILE |
||||
|
| |
||||
|
|- platformio.ini |
||||
|
|--src |
||||
|
|- main.c |
||||
|
|
||||
|
and a contents of `src/main.c`: |
||||
|
``` |
||||
|
#include <Foo.h> |
||||
|
#include <Bar.h> |
||||
|
|
||||
|
int main (void) |
||||
|
{ |
||||
|
... |
||||
|
} |
||||
|
|
||||
|
``` |
||||
|
|
||||
|
PlatformIO Library Dependency Finder will find automatically dependent |
||||
|
libraries scanning project source files. |
||||
|
|
||||
|
More information about PlatformIO Library Dependency Finder |
||||
|
- https://docs.platformio.org/page/librarymanager/ldf.html |
@ -0,0 +1,16 @@ |
|||||
|
; PlatformIO Project Configuration File |
||||
|
; |
||||
|
; Build options: build flags, source filter |
||||
|
; Upload options: custom upload port, speed and extra flags |
||||
|
; Library options: dependencies, extra library storages |
||||
|
; Advanced options: extra scripting |
||||
|
; |
||||
|
; Please visit documentation for the other options and examples |
||||
|
; https://docs.platformio.org/page/projectconf.html |
||||
|
|
||||
|
[env:nucleo_f429zi] |
||||
|
platform = ststm32 |
||||
|
board = nucleo_f429zi |
||||
|
framework = arduino |
||||
|
lib_deps = ponoor/Ponoor PowerSTEP01 Library@^1.0.2 |
||||
|
monitor_speed = 921600 |
@ -0,0 +1,58 @@ |
|||||
|
/*
|
||||
|
* M034.h - Library for M034 |
||||
|
* Created by brucker 22.03.2023 |
||||
|
*/ |
||||
|
|
||||
|
#include "M034.h" // classes and functions for M034
|
||||
|
|
||||
|
/*
|
||||
|
* Nanotec absolut position encoder |
||||
|
* SSI format length: 51 bit |
||||
|
* 16 startbits, all always zero |
||||
|
* 16 multiturn bits, MSB first |
||||
|
* 17 singleturn bits, MSB first |
||||
|
* 1 bit "D1", always zero |
||||
|
* 1 bit "D2", Error if 0 |
||||
|
* SPI read 7 bytes = 56 bit |
||||
|
* convert read data: |
||||
|
* shift relevant bits to the right position, mask not relevant bits and link bytes |
||||
|
*/ |
||||
|
|
||||
|
NanotecAbsolutEncoder::NanotecAbsolutEncoder(SPIClass *SPIPort){ // constructor
|
||||
|
_SPI = SPIPort; |
||||
|
} |
||||
|
|
||||
|
void NanotecAbsolutEncoder::readPosition(){ // read position from encoder
|
||||
|
|
||||
|
noInterrupts(); |
||||
|
for(uint8_t i=0; i<7; i++){ // read 7 bytes
|
||||
|
_readBytes[i] = _SPI->transfer (0); // store in array
|
||||
|
} |
||||
|
interrupts(); |
||||
|
delayMicroseconds(10); // time out of encoder is typ. 7µs
|
||||
|
} |
||||
|
|
||||
|
uint16_t NanotecAbsolutEncoder::getMultiturn(){ |
||||
|
_multiturn = (uint16_t) _readBytes[2] << 9; // convert and return multiturn
|
||||
|
_multiturn |= (uint16_t) _readBytes[3] << 1; |
||||
|
_multiturn |= (uint16_t) _readBytes[4] >> 7; |
||||
|
return _multiturn; |
||||
|
} |
||||
|
|
||||
|
uint32_t NanotecAbsolutEncoder::getSingleturn(){ // convert and return singelturn
|
||||
|
_singleturn = (uint32_t) (_readBytes[4] & 0x7F) << 10; |
||||
|
_singleturn |= (uint32_t) _readBytes[5] << 2; |
||||
|
_singleturn |= (uint32_t) _readBytes[6] >> 6; |
||||
|
return _singleturn; |
||||
|
} |
||||
|
|
||||
|
uint8_t NanotecAbsolutEncoder::getStatus(){ // check if encoder or transmission error exists
|
||||
|
_startbits = (uint16_t) _readBytes[0] << 9; // convert startbits
|
||||
|
_startbits |= (uint16_t) _readBytes[1] << 1; |
||||
|
_startbits |= (uint16_t) _readBytes[2] >> 7; |
||||
|
_status = (_readBytes[6] >> 4) & 0x03; // convert status
|
||||
|
if(_status == 1 & _startbits == 0){ |
||||
|
return 0; // return without error
|
||||
|
} |
||||
|
else return 1; // return with error
|
||||
|
} |
@ -0,0 +1,632 @@ |
|||||
|
/*
|
||||
|
* short explanation |
||||
|
*/ |
||||
|
|
||||
|
#include <Arduino.h> // main arduino library
|
||||
|
#include <Ponoor_PowerSTEP01Library.h> // stepper motor shield library
|
||||
|
#include <SPI.h> // Arduino SPI library
|
||||
|
#include <EEPROM.h> // Arduino EEPROM library
|
||||
|
#include "Pin_Definitions.h" // pin definitions
|
||||
|
#include "M034.h" // include header file for project
|
||||
|
|
||||
|
// string with manual for printing over UART
|
||||
|
const String manual = "\nM034 stepper motor controll\n" |
||||
|
"protocol structure: command parameter e.g. movl 200\n" |
||||
|
"commands (in small letters):\n" |
||||
|
"help: print instruction manual\n" |
||||
|
"movl (move left): spin x steps counterclockwise\n" |
||||
|
"movr (move right): spin x steps clockwise\n" |
||||
|
"movp (move to position): move motor to position uint32_t x\n" |
||||
|
"gtmp (go to motor position): go to saved motor position x {0-2}\n" |
||||
|
"gtep (go to encoder position): go to saved encoder position x {0-2}\n" |
||||
|
"smpo (set motor position): save current motor position (moved steps) as position x {0-2}\n" |
||||
|
"sepo (set encoder position): save current encoder position as position x {0-2}\n" |
||||
|
"getp (get position): get current position\n" |
||||
|
"setn (set null): set current position as null (home)\n" |
||||
|
"setm (set mode): set mode to: 0 - voltage mode; 1 - current mode\n" |
||||
|
"sets (set microstep size):\n" |
||||
|
"\tOptions: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128\n" |
||||
|
"\tSetS parameter = 1 for 1; 2 for 1/2; ...\n" |
||||
|
"\tIn current mode the maximum microstepping resolution is 1/16\n" |
||||
|
"\tIf changed, Null and saved positions get lost\n" |
||||
|
"gete (get encoder position): get current encoder singleturn position (average over x)\n" |
||||
|
"seep (set EEPROM encoder position): save current encoder singleturn home position to EEPROM\n" |
||||
|
"reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM\n" |
||||
|
"init (initialize): move to encoder EEPROM home position and set it to position null\n" |
||||
|
"\n" |
||||
|
; |
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* constant definitions * |
||||
|
**********************************************************************/ |
||||
|
const uint32_t SERIAL_BAUD_RATE = 921600; // Baud Rate of serial output
|
||||
|
// if changed, must also be changed in platformio.ini: monitor_speed
|
||||
|
const SPISettings SPI_ENC_Settings(2600000, MSBFIRST, SPI_MODE3); // SPI settings for stepper motor encoder
|
||||
|
const uint8_t POSITION_COUNT = 3; // legth of position array
|
||||
|
const uint32_t ENCODER_AV_COUNT = 1024; // Number of encoder positions over which to average
|
||||
|
const uint32_t MOTORSTEPS = 200; // number of full steps wich the motor has
|
||||
|
|
||||
|
// EEPROM (test)
|
||||
|
// const
|
||||
|
|
||||
|
// motor power settings. Complete explanation in setup. Tested with 16 microsteps
|
||||
|
const byte CM_RUN_TVAL = 20; // motor power settings for current mode
|
||||
|
const byte CM_ACC_TVAL = 32; |
||||
|
const byte CM_DEC_TVAL = 32; |
||||
|
const byte CM_HOLD_TVAL = 16; |
||||
|
const float CM_MAX_SPEED = 1000.0; |
||||
|
const byte VM_RUN_TVAL = 16; // motor power settings for voltage mode
|
||||
|
const byte VM_ACC_TVAL = 24; |
||||
|
const byte VM_DEC_TVAL = 24; |
||||
|
const byte VM_HOLD_TVAL = 28; |
||||
|
const float VM_MAX_SPEED = 1000.0; |
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* global variables * |
||||
|
**********************************************************************/ |
||||
|
|
||||
|
long positionMotor[POSITION_COUNT] = {(long)0, (long)1062, (long)-1067}; // array to save motor positions
|
||||
|
int32_t positionEncoder[POSITION_COUNT] = {}; // array to save motor positions
|
||||
|
uint16_t eeAddress = 0; // Location of next free EEPROM address
|
||||
|
uint32_t motorMicrosteps = 16; // number of microsteps wich currently used
|
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* function declaration * |
||||
|
**********************************************************************/ |
||||
|
void controlUart(uint8_t port); // check if uart communication is incomming on one of the UART's. If this is the case, read and execute incomming command
|
||||
|
int32_t averageEncPosition(uint32_t count); // read and build avverage of current encoder position
|
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* instance generation * |
||||
|
**********************************************************************/ |
||||
|
HardwareSerial Serial6 (D0, D1); // create Serial instance for optical links on USART6
|
||||
|
SPIClass SPI_Encoder(PIN_SPI_5_MOSI, PIN_SPI_5_MISO, PIN_SPI_5_CLK); // create SPI instance for encoder on SPI5
|
||||
|
//SPIClass * SPI_Encoder_ptr = &SPI_Encoder; // pointer to SPI_Encoder
|
||||
|
//NanotecAbsolutEncoder encoder(SPI_Encoder_ptr); // create encoder instance
|
||||
|
NanotecAbsolutEncoder encoder(&SPI_Encoder); // create encoder instance
|
||||
|
powerSTEP driver(0, nCS_PIN, nSTBY_nRESET_PIN); // powerSTEP library instance, parameters are distance from the end of a daisy-chain of drivers, !CS pin, !STBY/!Reset pin
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* setup * |
||||
|
**********************************************************************/ |
||||
|
void setup() { |
||||
|
// Prepare pins
|
||||
|
pinMode(nSTBY_nRESET_PIN, OUTPUT);// for stepper motor shield
|
||||
|
pinMode(nCS_PIN, OUTPUT); // for stepper motor shield
|
||||
|
pinMode(PIN_SPI_1_MOSI, OUTPUT); // SPI1 MOSI for stepper motor shield
|
||||
|
pinMode(PIN_SPI_1_MISO, INPUT); // SPI1 MISO for stepper motor shield
|
||||
|
pinMode(PIN_SPI_1_CLK, OUTPUT); // SPI1 CLK for stepper motor shield
|
||||
|
|
||||
|
pinMode(PIN_SPI_5_MOSI, OUTPUT); // SPI5 MOSI: not nessesary for our encoder
|
||||
|
pinMode(PIN_SPI_5_MISO, INPUT); // SPI5 MISO from encoder
|
||||
|
pinMode(PIN_SPI_5_CLK, OUTPUT); // SPI5 CLK for encoder
|
||||
|
|
||||
|
pinMode(LED_GREEN, OUTPUT); // Green LED on Nucleo-144
|
||||
|
pinMode(LED_BLUE, OUTPUT); // Blue LED on Nucleo-144
|
||||
|
pinMode(LED_RED, OUTPUT); // Red LED on Nucleo-144
|
||||
|
|
||||
|
pinMode(PIN_RGB_LED_R, OUTPUT); // Red part of the RGB LED
|
||||
|
pinMode(PIN_RGB_LED_G, OUTPUT); // Green part of the RGB LED
|
||||
|
pinMode(PIN_RGB_LED_B, OUTPUT); // Blue part of the RGB LED
|
||||
|
|
||||
|
pinMode(PIN_POS1, INPUT_PULLDOWN);// TTL Input for position controll
|
||||
|
pinMode(PIN_POS2, INPUT_PULLDOWN);// TTL Input for position controll
|
||||
|
pinMode(PIN_POS3, INPUT_PULLDOWN);// TTL Input for position controll
|
||||
|
|
||||
|
Serial.begin(SERIAL_BAUD_RATE); // Start serial UART on USB
|
||||
|
Serial6.begin(SERIAL_BAUD_RATE); // Start serial UART on optical links
|
||||
|
|
||||
|
SPI_Encoder.begin(); // Start SPI for stepper motor encoder
|
||||
|
SPI_Encoder.beginTransaction(SPI_ENC_Settings); // set correct settings
|
||||
|
|
||||
|
|
||||
|
|
||||
|
// initialise stepper motor shield
|
||||
|
// Reset powerSTEP and set CS
|
||||
|
digitalWrite(nSTBY_nRESET_PIN, HIGH); |
||||
|
digitalWrite(nSTBY_nRESET_PIN, LOW); |
||||
|
digitalWrite(nSTBY_nRESET_PIN, HIGH); |
||||
|
digitalWrite(nCS_PIN, HIGH); |
||||
|
|
||||
|
SPI.begin(); // Start SPI1 for stepper shield
|
||||
|
SPI.setDataMode(SPI_MODE3); // set correct SPI mode
|
||||
|
|
||||
|
// Configure powerSTEP
|
||||
|
driver.SPIPortConnect(&SPI); // give library the SPI port (only the one on an Uno)
|
||||
|
|
||||
|
driver.configSyncPin(BUSY_PIN, 0); // use SYNC/nBUSY pin as nBUSY,
|
||||
|
// thus syncSteps (2nd paramater) does nothing
|
||||
|
|
||||
|
driver.configStepMode(STEP_FS_16); // 1/16 microstepping, full steps = STEP_FS,
|
||||
|
//driver.configStepMode(STEP_FS); // 1/16 microstepping, full steps = STEP_FS,
|
||||
|
// options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128
|
||||
|
// In current mode the maximum microstepping resolution is 1/16
|
||||
|
|
||||
|
driver.setMaxSpeed(1000.0); // max speed in units of full steps/s (15.25 to 15610)
|
||||
|
driver.setFullSpeed(2000.0); // full steps/s threshold for disabling microstepping
|
||||
|
//driver.setAcc(1800); // full steps/s^2 acceleration
|
||||
|
//driver.setDec(1800); // full steps/s^2 deceleration
|
||||
|
driver.setAcc(900); // full steps/s^2 acceleration
|
||||
|
driver.setDec(900); // full steps/s^2 deceleration
|
||||
|
|
||||
|
driver.setSlewRate(SR_520V_us); // faster may give more torque (but also EM noise),
|
||||
|
// options are: 114, 220, 400, 520, 790, 980(V/us)
|
||||
|
|
||||
|
driver.setOCThreshold(16); // over-current threshold for the 2.8A NEMA23 motor
|
||||
|
// used in testing. If your motor stops working for
|
||||
|
// no apparent reason, it's probably this. Start low
|
||||
|
// and increase until it doesn't trip, then maybe
|
||||
|
// add one to avoid misfires. Can prevent catastrophic
|
||||
|
// failures caused by shorts
|
||||
|
driver.setOCShutdown(OC_SD_ENABLE); // shutdown motor bridge on over-current event
|
||||
|
// to protect against permanant damage
|
||||
|
|
||||
|
driver.setPWMFreq(PWM_DIV_1, PWM_MUL_0_75); // 16MHz*0.75/(512*1) = 23.4375kHz
|
||||
|
// power is supplied to stepper phases as a sin wave,
|
||||
|
// frequency is set by two PWM modulators,
|
||||
|
// Fpwm = Fosc*m/(512*N), N and m are set by DIV and MUL,
|
||||
|
// options: DIV: 1, 2, 3, 4, 5, 6, 7,
|
||||
|
// MUL: 0.625, 0.75, 0.875, 1, 1.25, 1.5, 1.75, 2
|
||||
|
|
||||
|
driver.setVoltageComp(VS_COMP_DISABLE); // no compensation for variation in Vs as
|
||||
|
// ADC voltage divider is not populated
|
||||
|
|
||||
|
driver.setSwitchMode(SW_USER); // switch doesn't trigger stop, status can be read.
|
||||
|
// SW_HARD_STOP: TP1 causes hard stop on connection
|
||||
|
// to GND, you get stuck on switch after homing
|
||||
|
|
||||
|
driver.setOscMode(INT_16MHZ); // 16MHz internal oscillator as clock source
|
||||
|
|
||||
|
// KVAL registers set the power to the motor by adjusting the PWM duty cycle,
|
||||
|
// use a value between 0-255 where 0 = no power, 255 = full power.
|
||||
|
// Start low and monitor the motor temperature until you find a safe balance
|
||||
|
// between power and temperature. Only use what you need
|
||||
|
|
||||
|
driver.setRunTVAL(CM_RUN_TVAL); |
||||
|
driver.setAccTVAL(CM_ACC_TVAL); |
||||
|
driver.setDecTVAL(CM_DEC_TVAL); |
||||
|
driver.setHoldTVAL(CM_HOLD_TVAL); |
||||
|
driver.setCurrentMode(); // set current mode
|
||||
|
/*
|
||||
|
driver.setMaxSpeed(1000.); |
||||
|
driver.setRunKVAL(VM_RUN_TVAL); |
||||
|
driver.setAccKVAL(VM_ACC_TVAL); |
||||
|
driver.setDecKVAL(VM_DEC_TVAL); |
||||
|
driver.setHoldKVAL(VM_HOLD_TVAL); |
||||
|
*/ |
||||
|
|
||||
|
driver.setParam(ALARM_EN, 0x8F); // disable ADC UVLO (divider not populated),
|
||||
|
// disable stall detection (not configured),
|
||||
|
// disable switch (not using as hard stop)
|
||||
|
|
||||
|
driver.getStatus(); // clears error flags
|
||||
|
// end of stepper motor shield initialisation
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
// led greeting
|
||||
|
digitalWrite(PIN_RGB_LED_R, LOW); // set all off
|
||||
|
digitalWrite(PIN_RGB_LED_G, LOW); |
||||
|
digitalWrite(PIN_RGB_LED_B, LOW); |
||||
|
digitalWrite(PIN_RGB_LED_R, HIGH);// red on
|
||||
|
delay(300); |
||||
|
digitalWrite(PIN_RGB_LED_R, LOW); // red off
|
||||
|
digitalWrite(PIN_RGB_LED_G, HIGH);// blue on
|
||||
|
delay(300); |
||||
|
digitalWrite(PIN_RGB_LED_G, LOW); // blue off
|
||||
|
digitalWrite(PIN_RGB_LED_B, HIGH);// green on
|
||||
|
delay(300); |
||||
|
digitalWrite(PIN_RGB_LED_B, LOW); // green off
|
||||
|
|
||||
|
Serial.print(manual); // print instruction manual on USB UART
|
||||
|
Serial6.print(manual); // print instruction manual on optical UART
|
||||
|
} // end of setup()
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* loop * |
||||
|
**********************************************************************/ |
||||
|
void loop() { |
||||
|
if(Serial.available() != 0){ // check if uart communication is incomming on USB UART
|
||||
|
controlUart(3); // If this is the case, read and execute incomming command
|
||||
|
} |
||||
|
if(Serial6.available() != 0){ // check if uart communication is incomming on optical UART
|
||||
|
controlUart(6); // If this is the case, execute command
|
||||
|
} |
||||
|
|
||||
|
if(digitalRead(PIN_POS1)==HIGH){ // if TTL position 1 input is HIGH
|
||||
|
driver.goTo(positionMotor[0]); // drive motor to saved position 1
|
||||
|
} |
||||
|
if(digitalRead(PIN_POS2)==HIGH){ // same for position 2
|
||||
|
driver.goTo(positionMotor[1]); |
||||
|
} |
||||
|
if(digitalRead(PIN_POS3)==HIGH){ // and position 3
|
||||
|
driver.goTo(positionMotor[2]); |
||||
|
} |
||||
|
|
||||
|
} // end of loop()
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
|
||||
|
/**********************************************************************
|
||||
|
* functions * |
||||
|
**********************************************************************/ |
||||
|
// read and execute incomming UART command
|
||||
|
void controlUart(uint8_t port){ |
||||
|
String input; // String for buffering read data
|
||||
|
String parameterStr; // STring for extracted parameter
|
||||
|
uint32_t parameter = 0; // variable for storing command parameter
|
||||
|
String message; // String for buffering vallidation message
|
||||
|
String errorMessage; // String for buffering error message
|
||||
|
|
||||
|
switch(port){ // Read from the port where the communication is coming from
|
||||
|
case 3: |
||||
|
input = Serial.readStringUntil('\n'); // read till line feed
|
||||
|
break; |
||||
|
case 6: |
||||
|
input = Serial6.readStringUntil('\n'); // read till line feed
|
||||
|
break; |
||||
|
} |
||||
|
input.trim(); // remove leading and trailing whitespace
|
||||
|
parameterStr = input.substring(5); // extract parameter: read from sign 5
|
||||
|
parameter = atoi(parameterStr.c_str()); // convert String into int
|
||||
|
|
||||
|
// search for controll string. If found execute command.
|
||||
|
// movl (move left): spin x steps counterclockwise
|
||||
|
if(input.startsWith("movl")){ |
||||
|
driver.move(REV, parameter); |
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
message = "Motor is moved counterclockwise. Steps: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
|
||||
|
// movr (move right): spin x steps clockwise
|
||||
|
else if(input.startsWith("movr")){ |
||||
|
driver.move(FWD, parameter); |
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
message = "Motor is moved clockwise. Steps: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
|
||||
|
// movp (move to position): move motor to position uint32_t x
|
||||
|
else if(input.startsWith("movp")){ |
||||
|
driver.goTo(parameter); |
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
message = "Motor is moved to position: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
|
||||
|
// gtmp (go to motor position): go to saved motor position x {0-2}
|
||||
|
else if(input.startsWith("gtmp")){ |
||||
|
if(parameter<POSITION_COUNT){ |
||||
|
driver.goTo(positionMotor[parameter]); |
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
message = "Motor is moved to saved position: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
else{ |
||||
|
errorMessage = "Parameter out of range"; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// gtep (go to encoder position): go to saved encoder position x {0-2}
|
||||
|
else if(input.startsWith("gtep")){ |
||||
|
if(parameter<POSITION_COUNT){ |
||||
|
int32_t currentPosition; |
||||
|
int32_t steps2Drive = 1; |
||||
|
uint8_t i = 0; // loop counter
|
||||
|
message = "Motor is moved to saved position: "; |
||||
|
message += (String) parameter; |
||||
|
message += ". Moved steps: "; |
||||
|
while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time
|
||||
|
currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
|
||||
|
steps2Drive = int32_t ((double)(currentPosition - positionEncoder[parameter]) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
|
||||
|
if(steps2Drive<0){ |
||||
|
driver.move(REV, ((unsigned long) (steps2Drive * -1))); |
||||
|
} |
||||
|
else{ |
||||
|
driver.move(FWD, (unsigned long) steps2Drive); |
||||
|
} |
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
delay(10); |
||||
|
message += (String) steps2Drive; |
||||
|
message += ("; "); |
||||
|
i++; |
||||
|
if(i==10){ |
||||
|
errorMessage = ("positioning not possible. Check neutron switch. Maybe motor parameter must be updated"); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// smpo (set motor position): save current motor position (moved steps) as position x {0-2}
|
||||
|
else if(input.startsWith("smpo")){ |
||||
|
if(parameter<POSITION_COUNT){ |
||||
|
positionMotor[parameter] = driver.getPos(); |
||||
|
message = "Current position is saved as motor position: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
else{ |
||||
|
errorMessage = "Parameter out of range"; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// sepo (set encoder position): save current encoder position as position x {0-2}
|
||||
|
else if(input.startsWith("sepo")){ |
||||
|
if(parameter<POSITION_COUNT){ |
||||
|
positionEncoder[parameter] = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
|
||||
|
message = "Current position is saved as encoder position: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
else{ |
||||
|
errorMessage = "Parameter out of range"; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// getp (get position): get current position
|
||||
|
else if(input.startsWith("getp")){ |
||||
|
long position = driver.getPos(); |
||||
|
message = "Current position is: "; |
||||
|
message = (String) position; |
||||
|
} |
||||
|
|
||||
|
// setn (set null): set current position as null (home)
|
||||
|
else if(input.startsWith("setn")){ |
||||
|
driver.resetPos(); |
||||
|
message = "Current position is set to 0"; |
||||
|
} |
||||
|
|
||||
|
// setm (set mode): set mode to: 0 - voltage mode; 1 - current mode
|
||||
|
else if(input.startsWith("setm")){ |
||||
|
if(parameter==0){ |
||||
|
// Settings at 24V
|
||||
|
driver.softHiZ(); |
||||
|
driver.setMaxSpeed(VM_MAX_SPEED); |
||||
|
driver.setRunTVAL(VM_RUN_TVAL); |
||||
|
driver.setAccTVAL(VM_ACC_TVAL); |
||||
|
driver.setDecTVAL(VM_DEC_TVAL); |
||||
|
driver.setHoldTVAL(VM_HOLD_TVAL); |
||||
|
driver.setVoltageMode(); |
||||
|
message = ("Voltage mode is set"); |
||||
|
} |
||||
|
else if(parameter==1){ |
||||
|
driver.softHiZ(); |
||||
|
driver.setMaxSpeed(CM_MAX_SPEED); |
||||
|
driver.setRunKVAL(CM_RUN_TVAL); |
||||
|
driver.setAccKVAL(CM_ACC_TVAL); |
||||
|
driver.setDecKVAL(CM_DEC_TVAL); |
||||
|
driver.setHoldKVAL(CM_HOLD_TVAL); |
||||
|
driver.setCurrentMode(); |
||||
|
message = ("Current mode is set"); |
||||
|
} |
||||
|
else errorMessage = "Parameter out of range"; |
||||
|
} |
||||
|
|
||||
|
// sets (set microstep size):
|
||||
|
// Options: 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128
|
||||
|
// SetS parameter = 1 for 1; 2 for 1/2; ...
|
||||
|
// In current mode the maximum microstepping resolution is 1/16
|
||||
|
// If changed, Null and saved positions get lost
|
||||
|
else if(input.startsWith("sets")){ |
||||
|
if(parameter==1){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS); // full steps
|
||||
|
message = "Full step is set"; |
||||
|
} |
||||
|
else if(parameter==2){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_2); // 2 microsteps
|
||||
|
message = "microstep size 1/2 is set"; |
||||
|
} |
||||
|
else if(parameter==4){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_4); // 4 microsteps
|
||||
|
message = "microstep size 1/4 is set"; |
||||
|
} |
||||
|
else if(parameter==8){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_8); // 8 microsteps
|
||||
|
message = "microstep size 1/8 is set"; |
||||
|
} |
||||
|
else if(parameter==16){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_16); // 16 microsteps
|
||||
|
message = "microstep size 1/16 is set"; |
||||
|
} |
||||
|
else if(parameter==32){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_32); // 32 microsteps
|
||||
|
message = "microstep size 1/32 is set"; |
||||
|
} |
||||
|
else if(parameter==64){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_64); // 64 microsteps
|
||||
|
message = "microstep size 1/64 is set"; |
||||
|
} |
||||
|
else if(parameter==128){ |
||||
|
driver.softHiZ(); |
||||
|
driver.configStepMode(STEP_FS_128); // 128 microsteps
|
||||
|
message = "microstep size 1/128 is set"; |
||||
|
} |
||||
|
else{ |
||||
|
errorMessage = "wrong parameter for sets"; |
||||
|
} |
||||
|
message += "\nposition lost, new initialisation needed"; |
||||
|
} |
||||
|
|
||||
|
// gete (get encoder position): get current encoder singleturn position (average over x)
|
||||
|
else if(input.startsWith("gete")){ |
||||
|
if(parameter>0){ |
||||
|
message = (String) averageEncPosition(parameter); // read and build avverage of current encoder position
|
||||
|
} |
||||
|
else{ |
||||
|
errorMessage = "Parameter out of range"; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// seep (set EEPROM encoder position): save current encoder singleturn home position to EEPROM
|
||||
|
else if(input.startsWith("seep")){ |
||||
|
int32_t currentPosition; |
||||
|
currentPosition = averageEncPosition(ENCODER_AV_COUNT); |
||||
|
EEPROM.put(eeAddress, currentPosition); |
||||
|
message = "Saved as encoder home position in EEPROM: "; |
||||
|
message += (String) currentPosition; |
||||
|
} |
||||
|
|
||||
|
// reep (reed EEPROM encoder position): reed current encoder singleturn home position from EEPROM
|
||||
|
else if(input.startsWith("reep")){ |
||||
|
EEPROM.get(eeAddress, parameter); // reed address from EEPROM and save in parameter
|
||||
|
message = "Encoder home position in EEPROM is: "; |
||||
|
message += (String) parameter; |
||||
|
} |
||||
|
|
||||
|
// init (initialize): move to encoder EEPROM home position and set it to position null
|
||||
|
else if(input.startsWith("init")){ |
||||
|
int32_t homePosition; |
||||
|
int32_t currentPosition; |
||||
|
int32_t steps2Drive = 1; |
||||
|
uint8_t i = 0; // loop counter
|
||||
|
message = "Moved steps to EEPROM encoder home position: "; |
||||
|
driver.softStop(); // hold motor
|
||||
|
driver.move(FWD, 10); // move a little bit so that the later move begins at a good position
|
||||
|
while(driver.busyCheck()); |
||||
|
driver.move(REV, 10); // move back so that a second init returns 0 moved steps
|
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); // hold motor
|
||||
|
while(driver.busyCheck()); |
||||
|
delay(100); // wait till motor stands still
|
||||
|
EEPROM.get(eeAddress, homePosition); // read home position from EEPROM
|
||||
|
|
||||
|
|
||||
|
while(steps2Drive != 0){ // move motor to position null till the position is reached, because null is not reached exactly every time
|
||||
|
currentPosition = averageEncPosition(ENCODER_AV_COUNT); // read singleturn position
|
||||
|
steps2Drive = int32_t ((double)(currentPosition - homePosition) / (131072.0 / (MOTORSTEPS*motorMicrosteps))); // calculate steps to move
|
||||
|
driver.resetPos(); // set motor driver position 0
|
||||
|
driver.goTo(steps2Drive); // move calculated steps
|
||||
|
while(driver.busyCheck()); |
||||
|
driver.softStop(); |
||||
|
while(driver.busyCheck()); |
||||
|
delay(10); |
||||
|
message += (String) steps2Drive; |
||||
|
message += ("; "); |
||||
|
i++; |
||||
|
if(i==10){ |
||||
|
errorMessage = ("init not possible. Check neutron switch. Maybe motor parameter must be updated"); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
driver.resetPos(); // set motor driver position 0
|
||||
|
while(driver.busyCheck()); |
||||
|
} |
||||
|
|
||||
|
// help: print instruction manual
|
||||
|
else if(input.startsWith("help")){ |
||||
|
message = manual; |
||||
|
} |
||||
|
|
||||
|
// hold: test how much hold power is needed
|
||||
|
else if(input.startsWith("hold")){ |
||||
|
delay(1000); |
||||
|
driver.setHoldKVAL(24); |
||||
|
delay(2000); |
||||
|
driver.setHoldKVAL(16); |
||||
|
delay(2000); |
||||
|
message = ("hold test done"); |
||||
|
} |
||||
|
|
||||
|
// setb: test set backup register
|
||||
|
else if(input.startsWith("setb")){ |
||||
|
uint32_t index = 0; |
||||
|
uint32_t value = 1000; |
||||
|
int32_t currentPosition; |
||||
|
currentPosition = averageEncPosition(ENCODER_AV_COUNT); |
||||
|
//setBackupRegister(index, value);
|
||||
|
EEPROM.put(8, currentPosition); |
||||
|
|
||||
|
message = ("Backup register written"); |
||||
|
} |
||||
|
|
||||
|
// getb: test set backup register
|
||||
|
else if(input.startsWith("getb")){ |
||||
|
Serial6.println(getBackupRegister(0)); |
||||
|
uint8_t test = 0; |
||||
|
message = ("Backup register:"); |
||||
|
for(int8_t i=10; i<20; i++){ |
||||
|
message += (" "); |
||||
|
message += EEPROM.get(i, test); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// command unknown
|
||||
|
else{ |
||||
|
errorMessage = "Command unknown or parameter is missing. Send 'help' to get the manual again"; |
||||
|
} |
||||
|
|
||||
|
if(message.length() != 0){ // if message available, print on the port on which command was receiverd
|
||||
|
switch(port){ |
||||
|
case 3: |
||||
|
Serial.println(message); |
||||
|
break; |
||||
|
case 6: |
||||
|
Serial6.println(message); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
if(errorMessage.length() != 0){ // if error message available, print on the port on which command was receiverd
|
||||
|
switch(port){ |
||||
|
case 3: |
||||
|
Serial.println(errorMessage); |
||||
|
break; |
||||
|
case 6: |
||||
|
Serial6.println(errorMessage); |
||||
|
break; |
||||
|
} |
||||
|
} |
||||
|
return; |
||||
|
} // end of controlUart()
|
||||
|
|
||||
|
|
||||
|
|
||||
|
// read and build avverage of current encoder position
|
||||
|
int32_t averageEncPosition(uint32_t count){ |
||||
|
int32_t encoderPositionAv = 0; |
||||
|
uint32_t i = 0; |
||||
|
for(i; i<count; i++){ |
||||
|
encoder.readPosition(); // read position from encoder
|
||||
|
if(encoder.getStatus()==0){ // if status is good,
|
||||
|
encoderPositionAv += encoder.getSingleturn(); //sum up positions
|
||||
|
} |
||||
|
else{ // if status is not good,
|
||||
|
i--; // count down i so that the count is correct
|
||||
|
} |
||||
|
} |
||||
|
encoderPositionAv = encoderPositionAv / i; // build average
|
||||
|
return encoderPositionAv; |
||||
|
} // end of averageEncPosition()
|
@ -0,0 +1,11 @@ |
|||||
|
|
||||
|
This directory is intended for PlatformIO Test Runner and project tests. |
||||
|
|
||||
|
Unit Testing is a software testing method by which individual units of |
||||
|
source code, sets of one or more MCU program modules together with associated |
||||
|
control data, usage procedures, and operating procedures, are tested to |
||||
|
determine whether they are fit for use. Unit testing finds problems early |
||||
|
in the development cycle. |
||||
|
|
||||
|
More information about PlatformIO Unit Testing: |
||||
|
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html |
Write
Preview
Loading…
Cancel
Save
Reference in new issue