//#include "AVRStepperPins.h"
#ifndef ARDUINO_ARCH_ESP32
#error "Select an ESP32 board"
#endif
// These define's must be placed at the beginning before #include "_TIMERINTERRUPT_LOGLEVEL_.h"
// _TIMERINTERRUPT_LOGLEVEL_ from 0 to 4
#define _TIMERINTERRUPT_LOGLEVEL_ 3
//----------------------------------------------------------------------------------------
// Include files
//----------------------------------------------------------------------------------------
//#include <ACAN_ESP32.h>
#include <esp_chip_info.h>
#include <esp_flash.h>
#include <core_version.h> // For ARDUINO_ESP32_RELEASE
//#include "ESP32TimerInterrupt.h"
#include <esp_task_wdt.h>
#include "FastAccelStepper.h"
#include <DebugLog.h>
#define VERSION "post-4f2e1e4". //version of FastAccelStepper
//----------------------------------------------------------------------------------------
// Data structures
//----------------------------------------------------------------------------------------
//constants
#define EN_PIN 3 //Define the enable signal port, filling with dummy for index 0 as motors have index starting at 1
#define STP_PIN 2 //Define the pulse signal port
#define DIR_PIN 0 //Define the direction signal port
#define ENDSTOP_PIN 6 //Define the endstop input port (incl. pullup)
#define LED_PIN 8 //Define internal LED pin port
#define slaveID 0x11 // CAN ID of node
#define maxparRx 4 // max number of receive parameters
#define maxparTx 4 // max number of send parameters
const uint8_t led_pin = 8; //Define internal LED pin port ESP32 C3
#define MAX_STEPPER 1 // this is a one-stepper board
#define CW 1 // CW means pos increases
#define CCW 0 // CCW means position decreases
#define MAXMESSAGE 20 // max lenth of error message
//variables
struct structcmdDB {
uint8_t cmd; // command ID
int lenRx; // length of payload (excl CRC) = 1 to 7
int nparamRx; // total number of prameters to receive
int nstartRx[maxparRx]; // start of MSB (note: numbering 7 6 5 ...1 0 15 14 ...9 8 23 ..)
int nbitRx[maxparRx]; // total of bits for parameter
bool bsignRx[maxparRx]; // is param signed (true) or unsigned (flase)
int nscaleRx[maxparRx]; // scaler for floating numbers
uint8_t actiononRx; // action index
int lenTx; // length of payload (excl CRC) = 1 to 7
int nparamTx; // total number of prameters to transmit
int nstartTx[maxparTx]; // start of MSB (see note numberin)
int nbitTx[maxparTx]; // total of bits for parameter
// bool bsignTx[maxparTx]; // is param signed (true) or unsigned (flase) -> not used
int nscaleTx[maxparTx]; // scaler for floating numbers
} ;
structcmdDB cmdDB[4] = {
{0x01, 1, 0, {}, {}, {}, {}, 1, 2, 1, {0} ,{8} ,{1} }, // Ping, confirm 1 = ok, 0 = false
{0x02, 1, 0, {}, {}, {}, {}, 1, 3, 2, {0,8} ,{8,8} ,{1,1} }, // test, confirm 1 = ok, 0 = false
{0x03, 1, 0, {}, {}, {}, {}, 1, 3, 3, {0,4,8},{4,4,8},{1,1,1}}, // test, confirm 1 = ok, 0 = false
{0x00}
};
struct structcmdaction {
uint8_t actiontxcmd; // action id related to command
uint8_t onreceipt; // what to do on receipt of command, 1: return 1 (OK),return 2 (noted)
uint8_t onfail; // what if failed
uint8_t onOK; // what if command executed
bool returnvalue; // true if main controller expects value(s) back
};
structcmdaction cmdaction[2] = {
{0x01,0x01, 0x00, 0xFF, false}, // ping confirm receipt
{0x00}
};
union Uint64on8 { // union to map 64bit to 8x 8bit
uint64_t data;
struct {
uint8_t bdata[8];
};
};
//----------------------------------------------------------------------------------------
// Stepper
//----------------------------------------------------------------------------------------
struct stepper_config_s {
uint8_t step;
uint8_t enable_low_active;
uint8_t enable_high_active;
uint8_t direction;
uint16_t dir_change_delay;
bool direction_high_count_up;
bool auto_enable;
uint32_t on_delay_us;
uint16_t off_delay_ms;
};
const struct stepper_config_s stepper_config[MAX_STEPPER] = {
{
step : 2,
enable_low_active : PIN_UNDEFINED,
enable_high_active : 6,
direction : 0,
dir_change_delay : 0,
direction_high_count_up : true,
auto_enable : false,
on_delay_us : 50,
off_delay_ms : 1000
}
};
//----------------------------------------------------------------------------------------
//. Robot node
// parameters
//----------------------------------------------------------------------------------------
// homing
int16_t cwstop = 0; // position vs zero if homing CW
int16_t ccwstop = 0; // position vs zero if homing CCW
uint8_t stopbackoff = 10; // backoff when hitting home
uint16_t stopgap = 100; // gap between left approach and right approach home switch
uint8_t homedir = CW; // direction to move to home from sleep position
int16_t posmax = 0; // max position vs zero
int16_t posmin = 0; // min pos vs zero
//speed
enum { booth, config, home, idle, move, go2sleep, shutdown, error } mode = booth;
enum { none, warning, warningandinform, stopandinform, stopfatal} errorlevel = none;
//----------------------------------------------------------------------------------------
//. Stepper
// creators for stepper
//----------------------------------------------------------------------------------------
FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper[MAX_STEPPER];
//----------------------------------------------------------------------------------------
//. CAN functions
// ESP32 Desired Bit Rate
//----------------------------------------------------------------------------------------
static const uint32_t DESIRED_BIT_RATE = 1000UL * 250UL ; // 250 kb/s
//----------------------------------------------------------------------------------------
// CAN Support functions send and receive
//----------------------------------------------------------------------------------------
//CAN communication
uint rxCnt = 0; //Receive data count
uint txCnt = 0; //Receive data count
unsigned long msgTime = 0;
/*-----------------------------------------------------------------------------------------
Function: Calculate the checksum of a set of data
Input: buffer data to be verified
size The number of data to be verified
output: checksum
*/
uint8_t getCheckSum(uint8_t id, uint8_t *buffer,uint8_t size){
uint8_t i;
uint16_t sum=id;
for(i=0;i<size;i++)
{
sum += buffer[i]; //Calculate accumulated value
}
return(sum&0xFF); //return checksum
} //getCheckSum
/*-----------------------------------------------------------------------------------------
returnmsg generic function makes message to send via CAN
parameters:
slaveAddr: CAN ID of the slave (0 for broadcast to all slaves)
cmd: command message
param[]: parameters for cmd
*/
bool returnmsg(uint8_t slaveAddr, uint8_t cmd, int param[maxparTx]) {
uint64_t dataTx = 0;
uint64_t tmpdata;
uint64_t mask;
union Uint64on8 bufdata;
int cmdidx;
int offset;
CANMessage frame ;
bool retVal = false; //return value
int i = 0;
while ((cmdDB[i].cmd != cmd) && (cmdDB[i].cmd != 0x00)) {
i++;
}
if (cmdDB[i].cmd == 0x00) {
PRINTLN("Cmd ",cmd, " not found. Ignore");
return(retVal);
}
cmdidx = i;
PRINT("returnmsg: slave =",slaveAddr, ", cmd=", cmd,", param= ");
for (int i = 0;i<cmdDB[cmdidx].nparamTx;i++) {
PRINT(param[i], ", ");
}
PRINTLN("<-");
dataTx = 0;
for (int i = 0; i < cmdDB[cmdidx].nparamTx;i++) {
mask = pow(2,cmdDB[cmdidx].nbitTx[i])-1;
tmpdata = (int64_t) param[i]& mask;
offset = cmdDB[cmdidx].nstartTx[i]+ cmdDB[cmdidx].nbitTx[i]-1;
dataTx = dataTx | tmpdata<<(8+ int(1+offset/8)*8 - offset%8-1);
}
bufdata.data = dataTx;
bufdata.bdata[0] = cmd;
frame.id =slaveAddr;
frame.ext = false ;
frame.rtr = false ;
frame.len = cmdDB[cmdidx].lenTx +1;
bufdata.bdata[cmdDB[cmdidx].lenTx] = getCheckSum(frame.id, bufdata.bdata,cmdDB[cmdidx].lenTx);
for(int i = 0;i<=frame.len;i++){
frame.data[i] = bufdata.bdata[i] ;
}
msgTime = micros();
const bool ok = ACAN_ESP32::can.tryToSend (frame) ;
if (ok) {
txCnt += 1 ;
retVal = true;
}
else {
PRINT("Failed to reply command: ",cmd);
}
return(retVal);
}
/*-----------------------------------------------------------------------------------------
Function: wait for the slave to answer, set the timeout to 3000ms
Input: len Length of the response, i.e number of data fields
output:
run successfully true
failed to run false
timeout no response false
*/
int CheckMsg( ) {
int retVal = 0; //return value
unsigned long sTime = millis(); //timing start time
unsigned long time; //current moment
uint8_t msgaction;
uint8_t cmd;
int param[4] = {0,0,0,0};
CANMessage framerx ;
// sTime = millis(); //get the current moment
// resetRxBuffer();
// PRINT(".");
while (ACAN_ESP32::can.receive (framerx)) {
PRINT("Return Msg: ", micros()-msgTime, ", ", framerx.id, ", ");
for (int i = 0;i<=framerx.len;i++) {
PRINT(framerx.data[i], ", ");
}
PRINTLN();
if (framerx.id == slaveID) {
cmd = framerx.data[0];
int i = 0;
while ((cmdDB[i].cmd != cmd) && (cmdDB[i].cmd != 0x00)) {
i++;
}
if (cmdDB[i].cmd == 0x00) {
PRINTLN("Cmd ",cmd, " not found. Ignore");
return(retVal);
}
msgaction = cmdDB[i].actiononRx;
int j = 0;
while ((cmdaction[j].actiontxcmd != msgaction) && (cmdaction[j].actiontxcmd != 0x00)) {
j++;
}
if (cmdaction[j].actiontxcmd == 0x00) {
PRINTLN("Received msg action not defined ",msgaction, " not found. Ignore");
return(retVal);
}
retVal = i;
if (cmdaction[j].onreceipt == 1) {
param[0] = 1;
returnmsg(slaveID, cmd, param);
}
}
}
return(retVal);
}
//----------------------------------------------------------------------------------------
// Setup
//----------------------------------------------------------------------------------------
void setup() {
int param[4] = {0,0,0,0};
//--- Switch on builtin led
pinMode (LED_BUILTIN, OUTPUT) ;
digitalWrite (LED_BUILTIN, HIGH) ;
//--- Start serial
Serial.begin(115200);
while (!Serial) {
; // wait for USB serial port to connect.
}
delay (1000) ;
//--- Display ESP32 Chip Info
uint32_t size_flash_chip ;
esp_chip_info_t chip_info ;
esp_chip_info (&chip_info) ;
esp_flash_get_size (NULL, &size_flash_chip) ;
PRINTLN("ESP32 Arduino Release: ", ARDUINO_ESP32_RELEASE) ;
PRINTLN("ESP32 Chip Revision: ", chip_info.revision) ;
PRINTLN("ESP32 SDK: ", ESP.getSdkVersion ()) ;
PRINTLN("ESP32 Flash: ", size_flash_chip / (1024 * 1024)) ;
PRINTLN(" MB ",((chip_info.features & CHIP_FEATURE_EMB_FLASH) != 0) ? "(embeded)" : "(external)") ;
PRINTLN("APB CLOCK: ", APB_CLK_FREQ," Hz") ;
//--- Configure stepper
pinMode(EN_PIN, OUTPUT); //Set the enable signal port to output mode
pinMode(STP_PIN, OUTPUT); //Set the pulse signal port to output mode
pinMode(DIR_PIN, OUTPUT); //Set the direction signal port to output mode
pinMode(ENDSTOP_PIN, INPUT_PULLUP); //Set the direction signal port to output mode
digitalWrite(EN_PIN, LOW); //enable motor
digitalWrite(STP_PIN, LOW); //enable motor
digitalWrite(DIR_PIN, HIGH); //output direction signal
//--- Configure ESP32 CAN
PRINTLN("Configure ESP32 CAN") ;
ACAN_ESP32_Settings settings (DESIRED_BIT_RATE) ;
// settings.mRequestedCANMode = ACAN_ESP32_Settings::LoopBackMode ;
// settings.mRxPin = GPIO_NUM_4 ; // Optional, default Tx pin is GPIO_NUM_4
// settings.mTxPin = GPIO_NUM_5 ; // Optional, default Rx pin is GPIO_NUM_5
const uint32_t errorCode = ACAN_ESP32::can.begin (settings) ;
if (errorCode == 0) {
PRINTLN("Bit Rate prescaler: ",settings.mBitRatePrescaler) ;
PRINTLN("Time Segment 1: ",settings.mTimeSegment1) ;
PRINTLN("Time Segment 2: ",settings.mTimeSegment2) ;
PRINTLN("RJW: ",settings.mRJW) ;
PRINTLN("Triple Sampling: ",settings.mTripleSampling ? "yes" : "no") ;
PRINTLN("Actual bit rate: ",settings.actualBitRate ()," bit/s") ;
PRINTLN("Exact bit rate ? ",settings.exactBitRate () ? "yes" : "no") ;
PRINTLN("Distance ",settings.ppmFromDesiredBitRate ()," ppm") ;
PRINTLN("Sample point xx: ",settings.samplePointFromBitStart (),"%") ;
PRINTLN("Configuration OK!");
}else {
PRINTLN("Configuration error 0x", errorCode, HEX) ;
}
// PRINTLN("Stepper Version: ",MSG_STEPPER_VERSION);
PRINTLN(" F_CPU=",F_CPU);
PRINTLN(" TICKS_PER_S=",TICKS_PER_S);
// If you are not sure, that the stepper hardware is working,
// then try first direct port manipulation and uncomment the next line.
// Alternatively use e.g. M1 T by serial command
// test_direct_drive(&stepper_config[0]);
engine.init();
if (led_pin != PIN_UNDEFINED) {
engine.setDebugLed(led_pin);
}
for (uint8_t i = 0; i < MAX_STEPPER; i++) {
FastAccelStepper *s = NULL;
const struct stepper_config_s *config = &stepper_config[i];
if (config->step != PIN_UNDEFINED) {
s = engine.stepperConnectToPin(config->step);
if (s) {
s->setDirectionPin(config->direction, config->direction_high_count_up,
config->dir_change_delay);
s->setEnablePin(config->enable_low_active, true);
s->setEnablePin(config->enable_high_active, false);
s->setAutoEnable(config->auto_enable);
s->setDelayToEnable(config->on_delay_us);
s->setDelayToDisable(config->off_delay_ms);
}
}
stepper[i] = s;
}
}
void onError(char message[MAXMESSAGE], uint16_t error ) {
}
//----------------------------------------------------------------------------------------
// Setup
//----------------------------------------------------------------------------------------
void loop() {
if (mode == booth) {
}
else if (mode == config) {
}
else if (mode == home) {
}
else if (mode == idle) {
}
else if (mode == move) {
}
else if (mode == go2sleep) {
}
else if (mode == shutdown) {
}
else if (mode == error) {
}
else {
PRINTLN("unknown mode ", mode);
}
}