├── Firmware ├── README ├── drivers.zip └── stepper_nano_zero │ ├── stepper_nano_zero.ino │ ├── steppin.h │ ├── ftoa.h │ ├── commands.h │ ├── utils.h │ ├── sine.h │ ├── utils.cpp │ ├── eeprom.h │ ├── nzs.h │ ├── gfxfont.h │ ├── as5047d.h │ ├── A1333.h │ ├── A5995.h │ ├── Flash.h │ ├── nzs_lcd.h │ ├── planner.h │ ├── A4954.h │ ├── calibration.h │ ├── angle.h │ ├── nonvolatile.h │ ├── fet_driver.h │ ├── ftoa.cpp │ ├── command.h │ ├── nonvolatile.cpp │ ├── Flash.cpp │ ├── steppin.cpp │ ├── planner.cpp │ ├── Adafruit_GFX.h │ ├── syslog.cpp │ ├── Adafruit_SSD1306.h │ ├── stepper_controller.h │ ├── A5995.cpp │ ├── eeprom.cpp │ ├── as5047d.cpp │ ├── syslog.h │ ├── glcdfont.c │ ├── command.cpp │ ├── sine.cpp │ ├── nzs_lcd.cpp │ ├── A4954.cpp │ ├── calibration.cpp │ └── A1333.cpp ├── hardware ├── README ├── Image │ ├── 3_ADD_URL.png │ ├── 6_CALIBRATE.png │ ├── 5_BUILD_UPLOAD.png │ ├── 7_CALIBRATE_OK.png │ ├── MKS_SERVO42A.png │ ├── 4_INSTALL_BOARD.png │ ├── MKS_SERVO42A_V1_SIZE.png │ ├── 1_MKS_SERVO42A_USB_DRIVE.png │ ├── MKS_SERVO42A_V1_WIRING.png │ └── 2_MKS_SERVO42A_INSTALL_SUCCESS.png └── MKS SERVO42A_V1.1_001 │ ├── MKS SERVO42A_V1.1_001 BOM.pdf │ ├── MKS SERVO42A_V1.1_001 SCH.pdf │ ├── MKS SERVO42A_V1.1_001 SIZE.pdf │ ├── MKS SERVO42A_V1.1_001 PCB_TOP.pdf │ └── MKS SERVO42A_V1.1_001 PCB_BOTTOM.pdf └── README.md /Firmware/README: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /hardware/README: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Firmware/drivers.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/Firmware/drivers.zip -------------------------------------------------------------------------------- /hardware/Image/3_ADD_URL.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/3_ADD_URL.png -------------------------------------------------------------------------------- /hardware/Image/6_CALIBRATE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/6_CALIBRATE.png -------------------------------------------------------------------------------- /hardware/Image/5_BUILD_UPLOAD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/5_BUILD_UPLOAD.png -------------------------------------------------------------------------------- /hardware/Image/7_CALIBRATE_OK.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/7_CALIBRATE_OK.png -------------------------------------------------------------------------------- /hardware/Image/MKS_SERVO42A.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/MKS_SERVO42A.png -------------------------------------------------------------------------------- /hardware/Image/4_INSTALL_BOARD.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/4_INSTALL_BOARD.png -------------------------------------------------------------------------------- /hardware/Image/MKS_SERVO42A_V1_SIZE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/MKS_SERVO42A_V1_SIZE.png -------------------------------------------------------------------------------- /hardware/Image/1_MKS_SERVO42A_USB_DRIVE.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/1_MKS_SERVO42A_USB_DRIVE.png -------------------------------------------------------------------------------- /hardware/Image/MKS_SERVO42A_V1_WIRING.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/MKS_SERVO42A_V1_WIRING.png -------------------------------------------------------------------------------- /hardware/Image/2_MKS_SERVO42A_INSTALL_SUCCESS.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/Image/2_MKS_SERVO42A_INSTALL_SUCCESS.png -------------------------------------------------------------------------------- /hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 BOM.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 BOM.pdf -------------------------------------------------------------------------------- /hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 SCH.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 SCH.pdf -------------------------------------------------------------------------------- /hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 SIZE.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 SIZE.pdf -------------------------------------------------------------------------------- /hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 PCB_TOP.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 PCB_TOP.pdf -------------------------------------------------------------------------------- /hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 PCB_BOTTOM.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/makerbase-mks/MKS-SERVO42A/HEAD/hardware/MKS SERVO42A_V1.1_001/MKS SERVO42A_V1.1_001 PCB_BOTTOM.pdf -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/stepper_nano_zero.ino: -------------------------------------------------------------------------------- 1 | #include "nzs.h" 2 | 3 | NZS nzs; 4 | 5 | 6 | void setup() { 7 | nzs.begin(); 8 | } 9 | 10 | 11 | void loop() { 12 | nzs.loop(); 13 | } 14 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/steppin.h: -------------------------------------------------------------------------------- 1 | #ifndef __STEPPIN_H___ 2 | #define __STEPPIN_H___ 3 | #include "board.h" 4 | 5 | void stepPinSetup(void); //setup step pin 6 | 7 | int32_t getSteps(void); //returns the number of steps changed since last call 8 | 9 | 10 | #endif // __STEPPIN_H___ 11 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/ftoa.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ftoa.h 3 | * 4 | * Created on: Jan 6, 2017 5 | * Author: tstern 6 | */ 7 | 8 | #ifndef FTOA_H_ 9 | #define FTOA_H_ 10 | 11 | #define MAX_MANTISA (1000) 12 | 13 | int ftoa (float x, char *str, char prec, char format); 14 | 15 | 16 | 17 | #endif /* FTOA_H_ */ 18 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/commands.h: -------------------------------------------------------------------------------- 1 | #ifndef __COMMANDS_H__ 2 | #define __COMMANDS_H__ 3 | #include 4 | #include "stepper_controller.h" 5 | #include "nzs.h" 6 | 7 | extern StepperCtrl stepperCtrl; 8 | extern eepromData_t PowerupEEPROM; 9 | 10 | void commandsInit(void); 11 | int commandsProcess(void); 12 | 13 | #endif //__COMMANDS_H__ 14 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | 13 | /* this file contains generic utilities and functions */ 14 | 15 | #ifndef UTILS_H_ 16 | #define UTILS_H_ 17 | 18 | 19 | double CubicInterpolate( 20 | double y0,double y1, 21 | double y2,double y3, 22 | double mu); 23 | 24 | 25 | 26 | 27 | #endif /* UTILS_H_ */ 28 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/sine.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * sine.h 3 | * 4 | * Created on: Dec 24, 2016 5 | * Author: tstern 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | 17 | #ifndef SINE_H_ 18 | #define SINE_H_ 19 | 20 | #include "board.h" 21 | 22 | #define SINE_STEPS (1024L) 23 | 24 | #define SINE_MAX ((int32_t)(32768L)) 25 | 26 | 27 | int16_t sine(uint16_t angle); 28 | int16_t cosine(uint16_t angle); 29 | 30 | 31 | #endif /* SINE_H_ */ 32 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/utils.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | 13 | #include "utils.h" 14 | #include "syslog.h" 15 | 16 | double CubicInterpolate( 17 | double y0,double y1, 18 | double y2,double y3, 19 | double mu) 20 | { 21 | double a0,a1,a2,a3,mu2; 22 | 23 | mu2 = mu*mu; 24 | a0 = y3 - y2 - y0 + y1; 25 | a1 = y0 - y1 - a0; 26 | a2 = y2 - y0; 27 | a3 = y1; 28 | 29 | return(a0*mu*mu2+a1*mu2+a2*mu+a3); 30 | } 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/eeprom.h: -------------------------------------------------------------------------------- 1 | /* 2 | * eeprom.h 3 | * 4 | * Created on: May 30, 2017 5 | * Author: tstern 6 | */ 7 | 8 | #ifndef EEPROM_H_ 9 | #define EEPROM_H_ 10 | #include "Flash.h" 11 | #include "calibration.h" 12 | #include "board.h" 13 | 14 | /* 15 | * This EEPROM implementation provides 60bytes of "eeprom space" (we reserve 4 bytes for overhead) 16 | * The EEPROM uses two rows of flash (256 bytes per row), which 17 | * for the SAMD21G18A this allows a minimual 200k writes, but typically 1200k 18 | */ 19 | 20 | typedef enum { 21 | EEPROM_OK =0, 22 | EEPROM_FAILED=1, 23 | EEPROM_CORRUPT=2, 24 | } eepromError_t; 25 | 26 | 27 | eepromError_t eepromInit(void); 28 | int eepromWriteCache(uint8_t *ptrData, uint32_t size); //returns number bytes written to cache 29 | eepromError_t eepromFlush(void); //flush the cache to flash memory 30 | int eepromRead(uint8_t *ptrData, uint32_t size); //returns number of bytes actually read, whcih could be less than size requested 31 | 32 | #endif /* EEPROM_H_ */ 33 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/nzs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * nzs.h 3 | * 4 | * Created on: Dec 8, 2016 5 | * Author: trampas 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | #ifndef NZS_H_ 17 | #define NZS_H_ 18 | 19 | #include "board.h" 20 | #include "nzs_lcd.h" 21 | #include "stepper_controller.h" 22 | #include "planner.h" 23 | 24 | typedef struct 25 | { 26 | int64_t angle; 27 | uint16_t encoderAngle; 28 | uint8_t valid; 29 | }eepromData_t; 30 | 31 | class NZS //nano Zero Stepper 32 | { 33 | 34 | public: 35 | void begin(void); 36 | void loop(void); 37 | 38 | }; 39 | 40 | 41 | #endif /* NZS_H_ */ 42 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/gfxfont.h: -------------------------------------------------------------------------------- 1 | // Font structures for newer Adafruit_GFX (1.1 and later). 2 | // Example fonts are included in 'Fonts' directory. 3 | // To use a font in your Arduino sketch, #include the corresponding .h 4 | // file and pass address of GFXfont struct to setFont(). Pass NULL to 5 | // revert to 'classic' fixed-space bitmap font. 6 | 7 | #ifndef _GFXFONT_H_ 8 | #define _GFXFONT_H_ 9 | 10 | typedef struct { // Data stored PER GLYPH 11 | uint16_t bitmapOffset; // Pointer into GFXfont->bitmap 12 | uint8_t width, height; // Bitmap dimensions in pixels 13 | uint8_t xAdvance; // Distance to advance cursor (x axis) 14 | int8_t xOffset, yOffset; // Dist from cursor pos to UL corner 15 | } GFXglyph; 16 | 17 | typedef struct { // Data stored for FONT AS A WHOLE: 18 | uint8_t *bitmap; // Glyph bitmaps, concatenated 19 | GFXglyph *glyph; // Glyph array 20 | uint8_t first, last; // ASCII extents 21 | uint8_t yAdvance; // Newline distance (y axis) 22 | } GFXfont; 23 | 24 | #endif // _GFXFONT_H_ 25 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/as5047d.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __AS5047D_H__ 13 | #define __AS5047D_H__ 14 | 15 | #include 16 | #define AS5047D_DEGREES_PER_BIT (360.0/(float)(0x3FFF)) 17 | 18 | class AS5047D { 19 | private: 20 | int chipSelectPin; 21 | int16_t readAddress(uint16_t addr); 22 | bool error=false; 23 | bool as5047d=true; 24 | public: 25 | boolean begin(int csPin); 26 | int16_t readEncoderAngle(void); 27 | void diagnostics(char *ptrStr); 28 | int16_t readEncoderAnglePipeLineRead(void); 29 | bool getError(void) {return error;}; 30 | }; 31 | 32 | #endif //__AS5047D_H__ 33 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A1333.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | 13 | #ifndef __A1333_H__ 14 | #define __A1333_H__ 15 | 16 | #include 17 | #define A1333_DEGREES_PER_BIT (360.0/(float)(0x7FFF)) 18 | 19 | 20 | #define PIN_A1333_CS (16)//analogInputToDigitalPin(PIN_A2)) 21 | 22 | 23 | class A1333 { 24 | private: 25 | int chipSelectPin; 26 | int16_t readAddress(uint16_t addr); 27 | bool error=false; 28 | bool a1333=true; 29 | public: 30 | boolean begin(int csPin); 31 | int16_t readEncoderAngle(void); 32 | void diagnostics(char *ptrStr); 33 | int16_t readEncoderAnglePipeLineRead(void); 34 | bool getError(void) {return error;}; 35 | }; 36 | 37 | #endif //__A1333_H__ 38 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A5995.h: -------------------------------------------------------------------------------- 1 | /* 2 | * A5995.h 3 | * 4 | * Created on: Feb 2, 2017 5 | * Author: tstern 6 | */ 7 | 8 | #ifndef A5995_H_ 9 | #define A5995_H_ 10 | 11 | #include 12 | #include "board.h" 13 | #include "angle.h" 14 | #include "sine.h" 15 | 16 | #define A5995_NUM_MICROSTEPS (256) 17 | 18 | 19 | //prevent someone for making a mistake with the code 20 | #if ((A5995_NUM_MICROSTEPS*4) != SINE_STEPS) 21 | #error "SINE_STEPS must be 4x of Micro steps for the move function" 22 | #endif 23 | 24 | 25 | 26 | /* 27 | * When it comes to the stepper driver if we use angles 28 | * we will always have a rounding error. For example 29 | * a 0-65536(360) angle for 1.8 degree step is 327.68 so 30 | * if you increment 200 of these as 327 you have a 13.6 error 31 | * after one rotation. 32 | * If you use floating point the effect is the same but takes longer. 33 | * 34 | * The only error-less accumulation system is to use native units, ie full 35 | * steps and microsteps. 36 | * 37 | */ 38 | 39 | class A5995 40 | { 41 | private: 42 | uint32_t lastStepMicros; // time in microseconds that last step happened 43 | bool forwardRotation=true; 44 | volatile bool enabled=true; 45 | 46 | public: 47 | void begin(void); 48 | 49 | //moves motor where the modulo of A4954_NUM_MICROSTEPS is a full step. 50 | int32_t move(int32_t stepAngle, uint32_t mA); 51 | 52 | uint32_t microsSinceStep(void) {return micros()-lastStepMicros;}; 53 | void setRotationDirection(bool forward) {forwardRotation=forward;}; 54 | 55 | void enable(bool enable); 56 | void limitCurrent(uint8_t percent) {return;}; //Not used 57 | }; 58 | 59 | 60 | 61 | #endif /* A5995_H_ */ 62 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/Flash.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __FLASH__H__ 13 | #define __FLASH__H__ 14 | 15 | #include 16 | #include "syslog.h" 17 | 18 | 19 | #define FLASH_PAGE_SIZE_NZS (64) //bytes 20 | #define FLASH_ROW_SIZE (FLASH_PAGE_SIZE_NZS*4) //defined in the datasheet as 4x page size 21 | #define FLASH_ERASE_VALUE (0xFF) //value of flash after an erase 22 | 23 | #define FLASH_ALLOCATE(name, size) \ 24 | __attribute__((__aligned__(FLASH_ROW_SIZE))) \ 25 | const uint8_t name[(size+(FLASH_ROW_SIZE-1))/FLASH_ROW_SIZE*FLASH_ROW_SIZE] = { }; 26 | 27 | bool flashInit(void); //this checks that our assumptions are true 28 | 29 | bool flashErase(const volatile void *flash_ptr, uint32_t size); 30 | void flashWrite(const volatile void *flash_ptr,const void *data,uint32_t size); 31 | void flashWritePage(const volatile void *flash_ptr, const void *data, uint32_t size); 32 | 33 | //you can read by dereferencing pointer but we will add a read 34 | static inline int32_t flashRead(const volatile void *flash_ptr, void *data, uint32_t size) 35 | { 36 | memcpy(data, (const void *)flash_ptr, size); 37 | } 38 | 39 | 40 | 41 | 42 | #endif //__FLASH__H__ 43 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/nzs_lcd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * nzs_lcd.h 3 | * 4 | * Created on: Dec 8, 2016 5 | * Author: trampas 6 | * 7 | * 8 | * Misfit Tech invests time and resources providing this open source code, 9 | * please support Misfit Tech and open-source hardware by purchasing 10 | * products from Misfit Tech, www.misifittech.net! 11 | * 12 | * Written by Trampas Stern for Misfit Tech. 13 | * BSD license, check license.txt for more information 14 | * All text above, must be included in any redistribution 15 | *********************************************************************/ 16 | 17 | #ifndef NZS_LCD_H_ 18 | #define NZS_LCD_H_ 19 | 20 | #include "Arduino.h" 21 | #include "syslog.h" 22 | #include "board.h" 23 | #include "stepper_controller.h" 24 | 25 | #include "Adafruit_GFX.h" 26 | #include "Adafruit_SSD1306.h" 27 | #include "gfxfont.h" 28 | 29 | 30 | typedef struct { 31 | char str[15]; 32 | } options_t; 33 | 34 | typedef struct { 35 | char str[15]; 36 | 37 | //only one of the following should be not null 38 | int (*func)(int argc, char *argv[]); 39 | options_t *ptrOptions; 40 | 41 | } menuItem_t; 42 | 43 | 44 | 45 | 46 | 47 | class NZS_LCD 48 | { 49 | private: 50 | bool displayEnabled; 51 | Adafruit_SSD1306 display; 52 | StepperCtrl *ptrStepperCtrl; 53 | menuItem_t *ptrMenu; 54 | int32_t menuIndex; 55 | bool menuActive; 56 | 57 | options_t *ptrOptions; 58 | int32_t optionIndex; 59 | 60 | int32_t buttonState; 61 | 62 | void updateLCD(void); 63 | void showMenu(void); 64 | void updateMenu(void); 65 | void showOptions(void); 66 | public: 67 | void forceMenuActive(void); 68 | void setMenu(menuItem_t *pMenu); 69 | void begin(StepperCtrl *ptrStepperCtrl); //sets up the LCD 70 | void process(void); //processes the LCD and updates as needed 71 | void showSplash(void); 72 | void lcdShow(const char *line1, const char *line2,const char *line3); 73 | 74 | 75 | }; 76 | 77 | 78 | #endif /* NZS_LCD_H_ */ 79 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/planner.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | 13 | /* 14 | * This file implements a trajectory planner for use with serial 15 | * interface. It allows the smart stepper to move at constant velocity. 16 | * Additionally you can move to some location at constant velocity or 17 | * with a trajectory curve 18 | */ 19 | 20 | #ifndef PLANNER_H_ 21 | #define PLANNER_H_ 22 | #include "board.h" 23 | #include "stepper_controller.h" 24 | 25 | #define PLANNER_UPDATE_RATE_HZ (3000UL) //how often planner updates PID 26 | 27 | typedef enum { 28 | PLANNER_NONE =0, 29 | PLANNER_CV =1, //constant velocity 30 | //PLANNER_CA =2, //constant accleration 31 | //PLANNER_S_CURVE =3, //s-curve move 32 | } PlannerMode; 33 | class Planner 34 | { 35 | private: 36 | StepperCtrl *ptrStepperCtrl; 37 | volatile PlannerMode currentMode=PLANNER_NONE; 38 | //todo we should not use floating point, rather use "Angle" 39 | volatile float endAngle; 40 | volatile float startAngle; 41 | volatile float currentSetAngle; 42 | volatile float tickIncrement; 43 | 44 | public: 45 | void begin(StepperCtrl *ptrStepper); 46 | bool moveConstantVelocity(float finalAngle, float rpm); //moves to the final location at a constant RPM 47 | void tick(void); //this is called on regulat tick interval 48 | void stop(void); 49 | bool done(void) {return currentMode==PLANNER_NONE;} 50 | }; 51 | 52 | 53 | extern Planner SmartPlanner; 54 | 55 | 56 | #endif /* PLANNER_H_ */ 57 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A4954.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __A4954__H__ 13 | #define __A4954__H__ 14 | #include 15 | #include "board.h" 16 | #include "angle.h" 17 | #include "sine.h" 18 | 19 | #define A4954_NUM_MICROSTEPS (256) 20 | #define A4954_MIN_TIME_BETWEEN_STEPS_MICROS (1000) 21 | 22 | //prevent someone for making a mistake with the code 23 | #if ((A4954_NUM_MICROSTEPS*4) != SINE_STEPS) 24 | #error "SINE_STEPS must be 4x of Micro steps for the move function" 25 | #endif 26 | 27 | 28 | 29 | /* 30 | * When it comes to the stepper driver if we use angles 31 | * we will always have a rounding error. For example 32 | * a 0-65536(360) angle for 1.8 degree step is 327.68 so 33 | * if you increment 200 of these as 327 you have a 13.6 error 34 | * after one rotation. 35 | * If you use floating point the effect is the same but takes longer. 36 | * 37 | * The only error-less accumulation system is to use native units, ie full 38 | * steps and microsteps. 39 | * 40 | */ 41 | 42 | class A4954 43 | { 44 | private: 45 | uint32_t lastStepMicros; // time in microseconds that last step happened 46 | bool forwardRotation=true; 47 | volatile bool enabled=true; 48 | 49 | public: 50 | void begin(void); 51 | 52 | //moves motor where the modulo of A4954_NUM_MICROSTEPS is a full step. 53 | int32_t move(int32_t stepAngle, uint32_t mA); 54 | 55 | uint32_t microsSinceStep(void) {return micros()-lastStepMicros;}; 56 | void setRotationDirection(bool forward) {forwardRotation=forward;}; 57 | 58 | void enable(bool enable); 59 | void limitCurrent(uint8_t percent); //higher more current 60 | }; 61 | 62 | 63 | 64 | #endif //__A4954__H__ 65 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MKS-SERVO42A 2 | # Features 3 | - Based on the open project of nano_stepper by Misfittech [click here](https://github.com/Misfittech/nano_stepper) 4 | - CPU is Atmel ATSAMD21G18A-U, 48MHZ 5 | - The magnetic encoder to Allegro's A1333LLETR-T Contactless 0° to 360° angle sensor IC ,12bit 6 | - The Drive is Allegro's A4954LPT, support 2A current 7 | - Support setting parameter by I2C interface oled0.96 and USB instruction 8 | - Compatible with all 3D printing motherboards by use MKS SV_EXT V1.1 9 | - Support build and upload firmware to board by Arduino paltform 10 | 11 | ## Related tutorials and Notice 12 | - User Manual. [click here](https://github.com/makerbase-mks/MKS-SERVO42A/wiki/MKS-SERVO42A-User-Manual) 13 | - Support Arduino build and upload. [click here](https://github.com/makerbase-mks/MKS-SERVO42A/wiki/MKS-SERVO42A--firmware-build-and-upload-tutorial) 14 | - MKS Closed-loop Stepper Motor Installation Tutorial. [click here](https://www.youtube.com/watch?v=mQyXR3hITy0) 15 | - Thank you for ruiraptor's about MKS Servo42 Close Loop Motor Tests & Results. [click here](https://www.youtube.com/watch?v=R1TghZmE6Gs) 16 | - How buy the MKS SERVO42A. [click here](https://www.aliexpress.com/item/32917408111.html?spm=2114.12010612.8148356.3.ea781b96kMAQsQ) 17 | - Welcome to follow us on Facebook to learn about the company's latest developments. [click here](https://www.facebook.com/Makerbase.mks/) 18 | 19 | ## License 20 | The hardware is under the Creative Commons Attribution Share-Alike 4.0 License as much of the work is based on Mechaduino project by J. Church. 21 | [https://github.com/jcchurch13/Mechaduino-Firmware](https://github.com/jcchurch13/Mechaduino-Firmware "https://github.com/jcchurch13/Mechaduino-Firmware"). 22 | 23 | The firmware is based on nano_stepper project by Misfittech：[https://github.com/Misfittech/nano_stepper](http:://github.com/Misfittech/nano_stepper "https://github.com/Misfittech/nano_stepper"), which is licensed as GPL V3 for non-commercial use. If you want to release a closed source version of this product, please contact MisfitTech.net for licensing details. 24 | 25 | ## Note 26 | - Thank you for using MKS products. If you have any questions during use, please contact us in time and we will work with you to solve it. 27 | - For more product dynamic information and tutorial materials, you can always follow MKS's Facebook/Twitter/Discord/Reddit/Youtube and Github. Thank you! 28 | - MKS Github: https://github.com/makerbase-mks 29 | - MKS Facebook: https://www.facebook.com/Makerbase.mks/ 30 | - MKS Twitter: https://twitter.com/home?lang=en 31 | - MKS Discord: https://discord.gg/4uar57NEyU 32 | - MKS Reddit: https://www.reddit.com/user/MAKERBASE-TEAM/ 33 | ![mks_link](https://user-images.githubusercontent.com/12979070/149612770-b5b383b3-90b1-4122-aa9e-c9f3dba648fe.png) 34 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/calibration.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __CALIBRAITON_H__ 13 | #define __CALIBRAITON_H__ 14 | 15 | #include 16 | #include "syslog.h" 17 | #include "angle.h" 18 | 19 | 20 | //this file implements a table that is linearly interpolated circular calibration table 21 | // it is assumed the data wraps around, ie you interpolated 65536==0 22 | //we want this to be "whole" steps, for 1.8 degree motors this should be 200. 23 | // 200 will work for 0.9 degree too, but could be 400. However 400 is not good for 1.8 degree motors 24 | #define CALIBRATION_TABLE_SIZE (200) 25 | 26 | #define CALIBRATION_STEPS ((uint32_t)ANGLE_STEPS) // this is one rotation ie 0-65535 aka 65536 steps is 0-360 degrees 27 | 28 | #define CALIBRATION_ERROR_NOT_SET (-1) //indicated that the calibration value is not set. 29 | 30 | #define CALIBRATION_UPDATE_RATE (32) //number of samples to keep 1 pole running average 31 | #define CALIBRATION_MIN_ERROR (4) //the minimal expected error on our calibration 4 ~=+/0.2 degrees 32 | 33 | 34 | typedef struct { 35 | uint16_t table[CALIBRATION_TABLE_SIZE]; 36 | bool status; 37 | } FlashCalData_t; 38 | 39 | 40 | 41 | 42 | 43 | typedef struct { 44 | Angle value; //cal value 45 | int16_t error; //error assuming it is constantly updated 46 | } CalData_t; 47 | 48 | class CalibrationTable 49 | { 50 | private: 51 | CalData_t table[CALIBRATION_TABLE_SIZE]; 52 | 53 | bool fastCalVaild=false; 54 | void loadFromFlash(void); 55 | bool flashGood(void); //returns true if the flash copy of calibration is valid 56 | 57 | void updateFastCal(void); 58 | void createFastCal(void); 59 | 60 | public: 61 | void init(void); 62 | void saveToFlash(void); //saves the calibration to flash 63 | bool updateTableValue(int32_t index, int32_t value); 64 | void updateTable(Angle actualAngle, Angle encoderValue); 65 | int getValue(Angle actualAngle, CalData_t *ptrData); 66 | Angle getCal(Angle actualAngle); 67 | bool calValid(void); 68 | Angle reverseLookup(Angle encoderAngle); //this turns encoder angle into real angle 69 | void printCalTable(void); 70 | void smoothTable(void); 71 | 72 | Angle fastReverseLookup(Angle encoderAngle); 73 | }; 74 | 75 | 76 | 77 | 78 | #endif //__CALIBRAITON_H__ 79 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/angle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * angle.h 3 | * 4 | * Created on: Sep 1, 2016 5 | * Author: TStern 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | #ifndef ANGLE_H_ 17 | #define ANGLE_H_ 18 | #include 19 | #include 20 | #include 21 | 22 | #define ANGLE_STEPS (0x010000UL) //65536 23 | #define ANGLE_MAX ((uint16_t)0x0FFFF) 24 | 25 | #define ANGLE_FROM_DEGREES(x) ((int32_t) ( ((float)ANGLE_STEPS*(float)(x)+180.0)/360.0 ) ) 26 | #define ANGLE_T0_DEGREES(x) ( (float) ((float(x)*360.0)/((float)ANGLE_STEPS) )) 27 | class Angle 28 | { 29 | private: 30 | uint16_t angle; 31 | public: 32 | Angle(void) {angle=0;} 33 | Angle(int32_t x) {angle=(uint16_t)x;} 34 | Angle(const Angle &x) {angle=x.angle;} 35 | 36 | int16_t operator-( const Angle &a2) 37 | { 38 | int32_t x,y,dx; 39 | x=(int32_t)angle; 40 | y=(int32_t)a2.angle; 41 | dx=x-y; 42 | if (abs(x-y)>ANGLE_STEPS/2) 43 | { 44 | //we have a wrap condition 45 | if (x>y) 46 | { 47 | dx=x-(y+ANGLE_STEPS); 48 | }else if (xANGLE_MAX) 61 | // { 62 | // y=y-ANGLE_STEPS; 63 | // } 64 | // while(y<-ANGLE_MAX) 65 | // { 66 | // y=y+ANGLE_STEPS; 67 | // } 68 | // 69 | // dx=x-y; 70 | // if (abs(x-y)>ANGLE_STEPS/2) 71 | // { 72 | // //we have a wrap condition 73 | // if (x>y) 74 | // { 75 | // dx=x-(y+ANGLE_STEPS); 76 | // }else if (x=ANGLE_STEPS) 95 | { 96 | a=a-ANGLE_STEPS; 97 | } 98 | while (a<0) 99 | { 100 | a=a+ANGLE_STEPS; 101 | } 102 | return Angle((uint16_t)a); 103 | } 104 | Angle operator+(const unsigned long int x) 105 | { 106 | uint32_t a; 107 | a=(uint32_t)angle+ x; 108 | while (a>=ANGLE_STEPS) 109 | { 110 | a=a-ANGLE_STEPS; 111 | } 112 | return Angle((uint16_t)a); 113 | } 114 | 115 | operator uint16_t() const {return angle;} 116 | operator uint32_t() const {return (uint32_t)angle;} 117 | operator int32_t() const {return (int32_t)angle;} 118 | 119 | 120 | 121 | }; 122 | 123 | 124 | #endif /* ANGLE_H_ */ 125 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/nonvolatile.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __NONVOLATILE__H__ 13 | #define __NONVOLATILE__H__ 14 | 15 | #include "calibration.h" 16 | #include "board.h" 17 | 18 | 19 | typedef struct { 20 | float Kp; 21 | float Ki; 22 | float Kd; 23 | bool parametersVaild; 24 | } PIDparams_t; 25 | 26 | typedef struct { 27 | int32_t currentMa; //maximum current for the motor 28 | int32_t currentHoldMa; //hold current for the motor 29 | int32_t homeMa; //maximum current when error homing 30 | int32_t homeHoldMa; //hold current when error homing 31 | bool motorWiring; //forward wiring of motor or reverse 32 | int32_t fullStepsPerRotation; //how many full steps per rotation is the motor 33 | bool parametersVaild; 34 | } MotorParams_t; 35 | 36 | typedef struct { 37 | int32_t microsteps; //number of microsteps on the dir/step pin interface from host 38 | RotationDir_t dirPinRotation; //is the direction pin high for clockwise or counterClockWise 39 | int32_t errorLimit; //error limit before error pin asserts 65536==360degrees 40 | ErrorPinMode_t errorPinMode; //is error pin used for enable, error, or bidirectional 41 | feedbackCtrl_t controllerMode; //feedback mode for the controller 42 | int32_t homePin; //if greater than zero this is the pin we use trigger home current settings 43 | bool errorLogic; //if high and error will be high on output pin 44 | int32_t homeAngleDelay; //the angle to delay before switching to lower homing current 45 | bool parametersVaild; 46 | } SystemParams_t; 47 | 48 | #ifdef NZS_FAST_CAL 49 | typedef struct { 50 | uint16_t angle[16384]; 51 | uint16_t checkSum; 52 | }FastCal_t; 53 | #endif 54 | 55 | typedef struct { 56 | FlashCalData_t CalibrationTable; 57 | __attribute__((__aligned__(8))) PIDparams_t sPID; //simple PID parameters 58 | __attribute__((__aligned__(8))) PIDparams_t pPID; //position PID parameters 59 | __attribute__((__aligned__(8))) PIDparams_t vPID; //velocity PID parameters 60 | __attribute__((__aligned__(8))) SystemParams_t SystemParams; 61 | __attribute__((__aligned__(8))) MotorParams_t motorParams; 62 | #ifdef NZS_FAST_CAL 63 | __attribute__((__aligned__(8))) FastCal_t FastCal; 64 | #endif 65 | } nvm_t; 66 | 67 | #ifdef NZS_FAST_CAL 68 | extern const uint16_t NVM_flash[16767]; 69 | #else 70 | extern const uint16_t NVM_flash[256]; 71 | #endif 72 | #define NVM ((const nvm_t *)NVM_flash) 73 | 74 | bool nvmWriteCalTable(void *ptrData, uint32_t size); 75 | bool nvmWrite_sPID(float Kp, float Ki, float Kd); 76 | bool nvmWrite_pPID(float Kp, float Ki, float Kd); 77 | bool nvmWrite_vPID(float Kp, float Ki, float Kd); 78 | bool nvmWriteSystemParms(SystemParams_t &systemParams); 79 | bool nvmWriteMotorParms(MotorParams_t &motorParams); 80 | bool nvmErase(void); 81 | 82 | #endif // __NONVOLATILE__H__ 83 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/fet_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * fet_driver.h 3 | * 4 | * Created on: Dec 24, 2016 5 | * Author: tstern 6 | * 7 | * This file supports using discrete FETs as drivers for stepper motor 8 | * 9 | * Misfit Tech invests time and resources providing this open source code, 10 | * please support Misfit Tech and open-source hardware by purchasing 11 | * products from Misfit Tech, www.misifittech.net! 12 | * 13 | * Written by Trampas Stern for Misfit Tech. 14 | * BSD license, check license.txt for more information 15 | * All text above, must be included in any redistribution 16 | *********************************************************************/ 17 | 18 | #ifndef FET_DRIVER_H_ 19 | #define FET_DRIVER_H_ 20 | 21 | 22 | 23 | #include 24 | #include "board.h" 25 | #include "angle.h" 26 | #include "sine.h" 27 | 28 | #ifdef NEMA_23_10A_HW 29 | #define FET_DRIVER_NUM_MICROSTEPS (SINE_STEPS/4) //number of steps to use for microstepping, default is 256 30 | #define FET_DRIVER_NUM_ZERO_AVG (100) 31 | 32 | 33 | #define FET_ADC_TO_MA(x) (((x)*2537)/1000) 34 | #define FET_MA_TO_ADC(x) (((x)*1000)/2537) 35 | //prvent someone for making a mistake with the code 36 | #if ((FET_DRIVER_NUM_MICROSTEPS*4) != SINE_STEPS) 37 | #error "SINE_STEPS must be 4x of Micro steps for the move function" 38 | #endif 39 | 40 | /* 41 | * When it comes to the stepper driver if we use angles 42 | * we will always have a rounding error. For example 43 | * a 0-65536(360) angle for 1.8 degree step is 327.68 so 44 | * if you increment 200 of these as 327 you have a 13.6 error 45 | * after one rotation. 46 | * If you use floating point the effect is the same but takes longer. 47 | * 48 | * The only error-less accumulation system is to use native units, ie full 49 | * steps and microsteps. 50 | * 51 | */ 52 | 53 | class FetDriver 54 | { 55 | static FetDriver *ptrInstance; 56 | private: 57 | uint32_t lastStepMicros; // time in microseconds that last step happened 58 | 59 | int32_t PWM_Table_B[512]; 60 | int32_t PWM_Table_A[512]; 61 | 62 | bool forwardRotation=true; 63 | volatile bool enabled=true; 64 | 65 | volatile int32_t adc; 66 | 67 | 68 | volatile int32_t coilB_value=0; 69 | volatile int32_t coilB_Zero=-1; 70 | volatile int32_t coilB_SetPoint=100; 71 | volatile int32_t coilB_error=0; 72 | 73 | volatile int32_t coilA_value=0; 74 | volatile int32_t coilA_Zero=-1; 75 | volatile int32_t coilA_SetPoint=200; 76 | volatile int32_t coilA_error=0; 77 | void ctrl_update(uint16_t channel, uint16_t value); 78 | void measureCoilB_zero(void); 79 | void measureCoilA_zero(void); 80 | void CalTableB(int32_t maxMA); 81 | void CalTableA(int32_t maxMA); 82 | int coilA_PWM(int32_t value); 83 | void coilB_PWM(int32_t value); 84 | int32_t getCoilB_mA(void); 85 | int32_t getCoilA_mA(void); 86 | public: 87 | 88 | static void ADC_Callback(uint16_t channel, uint16_t value); 89 | void begin(void); 90 | 91 | //moves motor where the modulo of A4954_NUM_MICROSTEPS is a full step. 92 | int32_t move(int32_t stepAngle, uint32_t mA); 93 | 94 | uint32_t microsSinceStep(void) {return micros()-lastStepMicros;}; 95 | void setRotationDirection(bool forward) {forwardRotation=forward;}; 96 | 97 | void enable(bool enable) {enabled=enable;}; 98 | void limitCurrent(uint8_t x) {return;}; 99 | }; 100 | 101 | 102 | #endif //#ifdef NEMA_23_10A_HW 103 | #endif /* FET_DRIVER_H_ */ 104 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/ftoa.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "board.h" 3 | #include "ftoa.h" 4 | /******************************************************************* 5 | * FUNCTION: ftoa 6 | * AUTHOR = TRAMPAS STERN 7 | * FILE = strio.c 8 | * DATE = 2/6/2003 4:27:14 PM 9 | * 10 | * PARAMETERS: long,*str, int count 11 | * 12 | * DESCRIPTION: Convets an float to string 13 | * format 'f', 'E', or 'e' 14 | * 15 | * 16 | * RETURNS: 17 | * 18 | * NOTE this code was found on the web and modified to actually work 19 | *******************************************************************/ 20 | int ftoa (float x, char *str, char prec, char format) 21 | { 22 | 23 | int ie, i, k, ndig, fstyle; 24 | double y; 25 | char *start; 26 | 27 | start=str; 28 | 29 | //based on percission set number digits 30 | ndig=prec+1; 31 | if (prec<0) 32 | ndig=7; 33 | if (prec>22) 34 | ndig=23; 35 | 36 | fstyle = 0; //exponent 'e' 37 | if (format == 'f' || format == 'F') 38 | fstyle = 1; //normal 'f' 39 | if (format=='g' || format=='G') 40 | fstyle=2; 41 | 42 | ie = 0; 43 | /* if x negative, write minus and reverse */ 44 | if ( x < 0) 45 | { 46 | *str++ = '-'; 47 | x = -x; 48 | } 49 | 50 | //if (x<0.0) then increment by 10 till betwen 1.0 and 10.0 51 | if (x!=0.0) 52 | { 53 | while (x < 1.0) 54 | { 55 | x =x* 10.0; 56 | ie--; 57 | } 58 | } 59 | 60 | //if x>10 then let's shift it down 61 | while (x >= 10.0) 62 | { 63 | x = x*(1.0/10.0); 64 | ie++; 65 | } 66 | 67 | if (ABS(ie)>MAX_MANTISA) 68 | { 69 | if (fstyle==1) 70 | { 71 | fstyle=0; 72 | format='e'; 73 | //ie=2; 74 | } 75 | } 76 | 77 | 78 | /* in f format, number of digits is related to size */ 79 | if (fstyle) 80 | ndig =ndig + ie; 81 | 82 | if(prec==0 && ie>ndig && fstyle) 83 | { 84 | ndig=ie; 85 | } 86 | 87 | /* round. x is between 1 and 10 and ndig will be printed to 88 | right of decimal point so rounding is ... */ 89 | y=1; 90 | for (i = 1; i < ndig; i++) //find lest significant digit 91 | y = y *(1.0/10.0); //multiply by 1/10 is faster than divides 92 | 93 | x = x+ y *(1.0/2.0); //add rounding 94 | 95 | /* repair rounding disasters */ 96 | if (x >= 10.0) 97 | { 98 | x = 1.0; 99 | ie++; 100 | ndig++; 101 | } 102 | 103 | //check and see if the number is less than 1.0 104 | if (fstyle && ie<0) 105 | { 106 | *str++ = '0'; 107 | if (prec!=0) 108 | *str++ = '.'; 109 | if (ndig < 0) 110 | ie = ie-ndig; /* limit zeros if underflow */ 111 | for (i = -1; i > ie; i--) 112 | *str++ = '0'; 113 | } 114 | 115 | //for each digit 116 | for (i=0; i < ndig; i++) 117 | { 118 | float b; 119 | k = x; //k = most significant digit 120 | *str++ = k + '0'; //output the char representation 121 | if (((!fstyle && i==0) || (fstyle && i==ie)) && prec!=0) 122 | *str++ = '.'; //output a decimal point 123 | b=(float)k; 124 | //multiply by 10 before subtraction to remove 125 | //errors from limited number of bits in float. 126 | b=b*10.0; 127 | x=x*10.0; 128 | x =x - b; //subtract k from x 129 | //b=x+b; 130 | //x =x* 10.0; //get next digit 131 | } 132 | 133 | /* now, in estyle, put out exponent if not zero */ 134 | if (!fstyle && ie != 0) 135 | { 136 | *str++ = format; 137 | if (ie < 0) //if number has negative exponent 138 | { 139 | ie = -ie; 140 | *str++ = '-'; 141 | } 142 | 143 | //now we need to convert the exponent to string 144 | for (k=1000; k>ie; k=k/10); //find the decade of exponent 145 | 146 | for (; k > 0; k=k/10) 147 | { 148 | char t; 149 | t=DIV(ie,k); 150 | *str++ = t + '0'; 151 | ie = ie -(t*k); 152 | } 153 | 154 | } 155 | *str++ = '\0'; 156 | return (str-start); //return string length 157 | } 158 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/command.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __COMMAND_H 3 | #define __COMMAND_H 4 | 5 | #include 6 | #include 7 | #include "syslog.h" 8 | /* 9 | * Usage: 10 | * 11 | #include 12 | #include "uart_e0.h" 13 | 14 | sCmdUart KeyfobCmdUart; // UART used for the keyfob command line interface 15 | 16 | CMD_STR(help,"Displays this message"); 17 | 18 | //List of supported commands 19 | sCommand KeyfobCmds[] = 20 | { 21 | COMMAND(help), 22 | {"",0,""}, //End of list signal 23 | }; 24 | 25 | // print out the help strings for the commands 26 | static int help_cmd(sCmdUart *ptrUart,int argc, char * argv[]) 27 | { 28 | sCommand cmd_list; 29 | int i; 30 | 31 | //now let's parse the command 32 | i=0; 33 | memcpy(&cmd_list, &KeyfobCmds[i], sizeof(sCommand)); 34 | while(cmd_list.function!=0) 35 | { 36 | 37 | CommandPrintf(ptrUart,(cmd_list.name)); 38 | CommandPrintf(ptrUart,PSTR(" - ")); 39 | CommandPrintf(ptrUart,(cmd_list.help)); 40 | CommandPrintf(ptrUart,PSTR("\n\r")); 41 | i=i+1; 42 | memcpy(&cmd_list, &KeyfobCmds[i], sizeof(sCommand)); 43 | } 44 | return 0; 45 | } 46 | 47 | uint8_t KeyfobCmdGetChar(void) 48 | { 49 | uint8_t c; 50 | if (UARTE0_getc(&c)!=0) 51 | { 52 | ERROR("Uart getchar failed"); 53 | return 0; 54 | } 55 | return c; 56 | } 57 | int KeyfobCmdInit(PIN tx_pin, PIN rx_pin, uint32_t baud) 58 | { 59 | LOG("UARTE0 init"); 60 | UARTE0_Init(tx_pin, rx_pin, baud); 61 | CommandInit(&KeyfobCmdUart, UARTE0_kbhit, KeyfobCmdGetChar, UARTE0_putc,NULL); //set up the UART structure 62 | return 0; 63 | } 64 | 65 | int KeyfobCmdProcess(void) 66 | { 67 | return CommandProcess(&KeyfobCmdUart,KeyfobCmds,' ',KEYFOB_CMD_PROMPT); 68 | } 69 | 70 | Advantages: 71 | 1. You can actually have more than one UART/device connected to same command line interface. 72 | 2. works with harvard machines to save SRAM space using the PSTR functionality 73 | 3. You can swap out commands "on the fly" 74 | 75 | 76 | */ 77 | #define MAX_CMD_LENGTH 60 78 | #define MAX_ARGS 10 79 | #define MAX_ARG_LENGTH 40 80 | #define CMD_HISTORY 3 //number of commands in history buffer 81 | #define ASCII_BACKSPACE 0x08 82 | #define ASCII_ESC 0x1B 83 | #define ASCII_UP_ARROW 0x9b 84 | #define ANSI_UP "\x1B[A\0" 85 | 86 | #define MAX_STRING 255 87 | //const char ANSI_UP[]= {ASCII_ESC,'[','A',0}; 88 | 89 | typedef struct { 90 | uint8_t (*kbhit)(void); 91 | uint8_t (*getch)(void); 92 | uint8_t (*putch)(char data); 93 | uint8_t (*puts)(uint8_t *buffer, uint8_t size); 94 | uint8_t data; 95 | char buffer[MAX_CMD_LENGTH]; 96 | 97 | char bufferHist[CMD_HISTORY][MAX_CMD_LENGTH]; 98 | uint8_t histIndex; 99 | uint8_t buffIndex; 100 | uint8_t lastChar; 101 | }sCmdUart; 102 | 103 | 104 | #define COMMAND(NAME) { NAME ## _str, NAME ## _cmd, NAME ## _help} 105 | 106 | 107 | #ifdef PGM_P //check and see if the PGM_P is defined for the AVR 108 | 109 | //If so then we use the strings in flash not SRAM 110 | #define CMD_STR(NAME,STR) static const char NAME ## _help[] PROGMEM = STR; static const char NAME ## _str[] PROGMEM = #NAME; static int NAME ##_cmd(sCmdUart *ptrUart,int, char **); 111 | //Command structure 112 | typedef struct 113 | { 114 | PGM_P name; 115 | int (*function) (sCmdUart *ptrUart,int, char **); 116 | PGM_P help; 117 | } sCommand; 118 | int CommandPrintf(sCmdUart *ptrUart, const char *fmt, ...); 119 | 120 | #else 121 | 122 | #define CMD_STR(NAME,STR) static char NAME ## _help[] = STR; static char NAME ## _str[] = #NAME; static int NAME ##_cmd(sCmdUart *ptrUart,int, char **); 123 | 124 | //Command structure 125 | typedef struct 126 | { 127 | char *name; 128 | int (*function) (sCmdUart *ptrUart,int, char **); 129 | char *help; 130 | } sCommand; 131 | 132 | int CommandPrintf(sCmdUart *ptrUart, char *fmt, ...); 133 | #endif 134 | 135 | 136 | int CommandInit(sCmdUart *ptrUart, uint8_t (*kbhit)(void), uint8_t (*getch)(void),uint8_t (*putch)(char data),uint8_t (*puts)(uint8_t *buffer, uint8_t size)); 137 | unsigned int CommandParse(sCmdUart *ptrUart,sCommand *ptrCmds, char *str, char delimitor); 138 | int CommandProcess(sCmdUart *ptrUart,sCommand *ptrCmds, char delimitor, char *cmdPrompt); 139 | 140 | 141 | 142 | #endif 143 | 144 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/nonvolatile.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include "nonvolatile.h" 13 | #include "Flash.h" //thanks to Kent Larsen for pointing out the lower case error 14 | #include 15 | 16 | 17 | 18 | 19 | //we use this so we can hard code calibration table 20 | // be sure to set the last word as status flag 21 | // this save time calibrating each time we do a code build 22 | #ifdef NZS_FAST_CAL 23 | __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[16767]={ //allocates 33280 bytes 24 | #else 25 | __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint16_t NVM_flash[256]={ //allocates 512 bytes 26 | #endif 27 | //insert the getcal command calibration data here 28 | // 5154,5482,5812,6142,6469,6801,7129,7464,7790,8125,8456,8792,9119,9455,9787,10122,10453,10787,11118,11452,11780,12111,12441,12773,13097,13429,13755,14082,14407,14732,15056,15382,15703,16027,16346,16674,16991,17317,17637,17960,18279,18605,18929,19253,19572,19900,20226,20552,20874,21204,21530,21861,22183,22516,22844,23176,23503,23841,24170,24505,24837,25170,25502,25838,26167,26505,26838,27173,27500,27836,28168,28503,28830,29161,29492,29822,30147,30477,30801,31130,31451,31775,32099,32426,32743,33067,33388,33709,34026,34350,34671,34995,35312,35637,35957,36281,36602,36929,37249,37577,37896,38222,38544,38875,39195,39528,39855,40182,40509,40838,41166,41501,41826,42161,42488,42822,43149,43484,43810,44144,44470,44801,45128,45458,45784,46112,46436,46763,47085,47408,47732,48054,48377,48699,49020,49343,49662,49983,50306,50631,50949,51272,51596,51922,52242,52570,52892,53221,53545,53874,54200,54530,54855,55187,55515,55850,56179,56514,56847,57183,57516,57852,58189,58527,58863,59202,59533,59871,60203,60542,60877,61214,61544,61878,62211,62544,62872,63201,63531,63862,64185,64515,64837,65168,65489,283,604,930,1252,1578,1902,2226,2551,2875,3201,3528,3848,4176,4499,4829, 29 | 30 | 0xFFFF 31 | }; 32 | 33 | 34 | 35 | static_assert (sizeof(nvm_t)CalibrationTable,ptrData,size); 47 | return true; 48 | } 49 | 50 | bool nvmWrite_sPID(float Kp, float Ki, float Kd) 51 | { 52 | PIDparams_t pid; 53 | 54 | pid.Kp=Kp; 55 | pid.Ki=Ki; 56 | pid.Kd=Kd; 57 | pid.parametersVaild=true; 58 | 59 | flashWrite((void *)&NVM->sPID,&pid,sizeof(pid)); 60 | return true; 61 | } 62 | 63 | bool nvmWrite_vPID(float Kp, float Ki, float Kd) 64 | { 65 | PIDparams_t pid; 66 | 67 | pid.Kp=Kp; 68 | pid.Ki=Ki; 69 | pid.Kd=Kd; 70 | pid.parametersVaild=true; 71 | 72 | flashWrite((void *)&NVM->vPID,&pid,sizeof(pid)); 73 | return true; 74 | } 75 | 76 | bool nvmWrite_pPID(float Kp, float Ki, float Kd) 77 | { 78 | PIDparams_t pid; 79 | 80 | pid.Kp=Kp; 81 | pid.Ki=Ki; 82 | pid.Kd=Kd; 83 | pid.parametersVaild=true; 84 | 85 | flashWrite((void *)&NVM->pPID,&pid,sizeof(pid)); 86 | return true; 87 | } 88 | 89 | bool nvmWriteSystemParms(SystemParams_t &systemParams) 90 | { 91 | systemParams.parametersVaild=true; 92 | 93 | flashWrite((void *)&NVM->SystemParams,&systemParams,sizeof(systemParams)); 94 | return true; 95 | } 96 | 97 | bool nvmWriteMotorParms(MotorParams_t &motorParams) 98 | { 99 | motorParams.parametersVaild=true; 100 | 101 | flashWrite((void *)&NVM->motorParams,&motorParams,sizeof(motorParams)); 102 | return true; 103 | } 104 | 105 | bool nvmErase(void) 106 | { 107 | bool data=false; 108 | uint16_t cs=0; 109 | 110 | flashWrite((void *)&NVM->CalibrationTable.status,&data,sizeof(data)); 111 | flashWrite((void *)&NVM->sPID.parametersVaild ,&data,sizeof(data)); 112 | flashWrite((void *)&NVM->vPID.parametersVaild ,&data,sizeof(data)); 113 | flashWrite((void *)&NVM->pPID.parametersVaild ,&data,sizeof(data)); 114 | flashWrite((void *)&NVM->motorParams.parametersVaild ,&data,sizeof(data)); 115 | flashWrite((void *)&NVM->SystemParams.parametersVaild ,&data,sizeof(data)); 116 | #ifdef NZS_FAST_CAL 117 | flashWrite((void *)&NVM->FastCal.checkSum,&cs,sizeof(cs)); 118 | #endif 119 | } 120 | 121 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/Flash.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include "Flash.h" 13 | #include "syslog.h" 14 | 15 | bool flashInit(void){ 16 | if (NVMCTRL->PARAM.bit.PSZ != 3) 17 | { 18 | ERROR("FLASH PAGE SIZE is not 64 bytes"); 19 | return false; 20 | } 21 | return true; 22 | } 23 | 24 | 25 | static void erase(const volatile void *flash_ptr) 26 | { 27 | NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr) / 2; 28 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER; 29 | while (!NVMCTRL->INTFLAG.bit.READY) { } 30 | } 31 | 32 | bool flashErase(const volatile void *flash_ptr, uint32_t size) 33 | { 34 | const uint8_t *ptr = (const uint8_t *)flash_ptr; 35 | while (size > FLASH_ROW_SIZE) { 36 | erase(ptr); 37 | ptr += FLASH_ROW_SIZE; 38 | size -= FLASH_ROW_SIZE; 39 | } 40 | if (size>0) 41 | { 42 | erase(ptr); 43 | } 44 | return true; //TODO should verify the erase 45 | } 46 | 47 | static inline uint32_t read_unaligned_uint32(const void *data) 48 | { 49 | union { 50 | uint32_t u32; 51 | uint8_t u8[4]; 52 | } res; 53 | const uint8_t *d = (const uint8_t *)data; 54 | res.u8[0] = d[0]; 55 | res.u8[1] = d[1]; 56 | res.u8[2] = d[2]; 57 | res.u8[3] = d[3]; 58 | return res.u32; 59 | } 60 | 61 | 62 | void flashWrite(const volatile void *flash_ptr,const void *data, uint32_t size) 63 | { 64 | uint32_t *ptrPage; 65 | uint8_t *destPtr; 66 | uint8_t *srcPtr; 67 | uint32_t bytesInBlock; 68 | __attribute__((__aligned__(4))) uint8_t buffer[FLASH_ROW_SIZE]; 69 | uint32_t offset; 70 | 71 | destPtr=(uint8_t *)flash_ptr; 72 | srcPtr=(uint8_t *)data; 73 | 74 | LOG("flash write called"); 75 | while(size>0) 76 | { 77 | uint32_t i,j; 78 | 79 | //calculate the maximum number of bytes we can write in page 80 | offset=((uint32_t)destPtr)%(FLASH_ROW_SIZE); //offset into page 81 | bytesInBlock=FLASH_ROW_SIZE-offset; //this is how many bytes we need to overwrite in this page 82 | 83 | LOG("offset %d, bytesInBlock %d size %d", offset, bytesInBlock,size); 84 | //get pointer to start of page 85 | ptrPage=(uint32_t *) ((((uint32_t)destPtr)/(FLASH_ROW_SIZE)) * FLASH_ROW_SIZE); 86 | 87 | LOG("pointer to page %d(0x%08x) %d",(uint32_t)ptrPage,(uint32_t)ptrPage,destPtr); 88 | 89 | //fill page buffer with data from flash 90 | memcpy(buffer,ptrPage,FLASH_ROW_SIZE); 91 | 92 | //now fill buffer with new data that needs changing 93 | i=bytesInBlock; 94 | if (sizeCTRLB.bit.MANW = 1; 142 | 143 | // Do writes in pages 144 | while (size) 145 | { 146 | // Execute "PBC" Page Buffer Clear 147 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_PBC; 148 | while (NVMCTRL->INTFLAG.bit.READY == 0) { } 149 | 150 | // Fill page buffer 151 | uint32_t i; 152 | for (i=0; i<(FLASH_PAGE_SIZE/4) && size; i++) //we write 4 bytes at a time 153 | { 154 | *dst_addr = read_unaligned_uint32(src_addr); 155 | src_addr += 4; 156 | dst_addr++; 157 | size--; //size is set to number of 32bit words in first line above 158 | } 159 | 160 | // Execute "WP" Write Page 161 | NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP; 162 | while (NVMCTRL->INTFLAG.bit.READY == 0) { } 163 | } 164 | 165 | 166 | } 167 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/steppin.cpp: -------------------------------------------------------------------------------- 1 | #include "steppin.h" 2 | #include "stepper_controller.h" 3 | #include "wiring_private.h" 4 | 5 | extern StepperCtrl stepperCtrl; 6 | 7 | volatile int32_t stepsChanged=0; 8 | #define WAIT_TC32_REGS_SYNC(x) while(x->COUNT32.STATUS.bit.SYNCBUSY); 9 | 10 | #if (PIN_STEP_INPUT != 0) 11 | #error "this code only works with step pin being D0 (PA11, EXTINT11)" 12 | #endif 13 | 14 | int32_t getSteps(void) 15 | { 16 | //#ifndef USE_NEW_STEP 17 | // return 0; 18 | //#endif 19 | int32_t x; 20 | #ifdef USE_TC_STEP 21 | int32_t y; 22 | 23 | WAIT_TC32_REGS_SYNC(TC4) 24 | x=(int32_t)TC4->COUNT32.COUNT.reg; 25 | y=x-stepsChanged; 26 | stepsChanged=x; 27 | return y; 28 | 29 | #else 30 | EIC->INTENCLR.reg = EIC_INTENCLR_EXTINT11; 31 | x=stepsChanged; 32 | stepsChanged=0; 33 | EIC->INTENSET.reg = EIC_INTENSET_EXTINT11; 34 | return x; 35 | #endif 36 | } 37 | //this function is called on the rising edge of a step from external device 38 | static void stepInput(void) 39 | { 40 | static int dir; 41 | //read our direction pin 42 | dir = digitalRead(PIN_DIR_INPUT); 43 | 44 | if (CW_ROTATION == NVM->SystemParams.dirPinRotation) 45 | { 46 | dir=!dir; //reverse the rotation 47 | } 48 | 49 | #ifndef USE_NEW_STEP 50 | stepperCtrl.requestStep(dir,1); 51 | #else 52 | if (dir) 53 | { 54 | stepsChanged++; 55 | }else 56 | { 57 | stepsChanged--; 58 | } 59 | #endif 60 | } 61 | 62 | void enableEIC(void) 63 | { 64 | if (EIC->CTRL.bit.ENABLE == 0) 65 | { 66 | // Enable GCLK for IEC (External Interrupt Controller) 67 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_EIC)); 68 | 69 | // Enable EIC 70 | EIC->CTRL.bit.ENABLE = 1; 71 | while (EIC->STATUS.bit.SYNCBUSY == 1) { } 72 | } 73 | } 74 | 75 | 76 | void setupStepEvent(void) 77 | { 78 | //we will set up the EIC to generate an even on rising edge of step pin 79 | //make sure EIC is setup 80 | enableEIC(); 81 | 82 | 83 | // Assign step pin to EIC 84 | // Step pin is PA11, EXTINT11 85 | pinPeripheral(PIN_STEP_INPUT, PIO_EXTINT); 86 | 87 | //***** setup EIC ****** 88 | EIC->EVCTRL.bit.EXTINTEO11=1; //enable event for EXTINT11 89 | //setup up external interurpt 11 to be rising edge triggered 90 | EIC->CONFIG[1].reg |= EIC_CONFIG_SENSE3(EIC_CONFIG_SENSE3_HIGH_Val); 91 | 92 | //diable actually generating an interrupt, we only want event triggered 93 | EIC->INTENCLR.reg = EIC_INTENCLR_EXTINT11; 94 | 95 | //**** setup the event system *** 96 | // Enable GCLK for EVSYS channel 0 97 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_EVSYS_CHANNEL_0)); 98 | while (GCLK->STATUS.bit.SYNCBUSY); 99 | EVSYS->CHANNEL.reg=EVSYS_CHANNEL_CHANNEL(0) 100 | | EVSYS_CHANNEL_EDGSEL_RISING_EDGE 101 | | EVSYS_CHANNEL_EVGEN(EVSYS_ID_GEN_EIC_EXTINT_11) 102 | | EVSYS_CHANNEL_PATH_ASYNCHRONOUS; 103 | 104 | EVSYS->USER.reg = EVSYS_USER_CHANNEL(1) 105 | | EVSYS_USER_USER(EVSYS_ID_USER_TC4_EVU); 106 | 107 | //**** setup the Timer counter ****** 108 | // Enable GCLK for TC4 and TC5 (timer counter input clock) 109 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5)); 110 | while (GCLK->STATUS.bit.SYNCBUSY); 111 | 112 | TC4->COUNT32.CTRLA.reg = TC_CTRLA_SWRST; //reset TC4 113 | WAIT_TC32_REGS_SYNC(TC4) 114 | 115 | TC4->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 // Set Timer counter Mode to 32 bits 116 | | TC_CTRLA_WAVEGEN_MFRQ //normal counting mode (not using waveforms) 117 | | TC_CTRLA_PRESCALER_DIV1; //count each pulse 118 | WAIT_TC32_REGS_SYNC(TC4) 119 | 120 | TC4->COUNT32.CTRLBCLR.reg=0xFF; //clear all values. 121 | WAIT_TC32_REGS_SYNC(TC4) 122 | 123 | TC4->COUNT32.EVCTRL.reg=TC_EVCTRL_TCEI | TC_EVCTRL_EVACT_COUNT; //enable event input and count 124 | WAIT_TC32_REGS_SYNC(TC4) 125 | 126 | TC4->COUNT32.COUNT.reg=0; 127 | WAIT_TC32_REGS_SYNC(TC4) 128 | 129 | // Enable TC 130 | TC4->COUNT32.CTRLA.reg |= TC_CTRLA_ENABLE; 131 | WAIT_TC32_REGS_SYNC(TC4) 132 | } 133 | 134 | static void dirChanged_ISR(void) 135 | { 136 | static int dir; 137 | //read our direction pin 138 | dir = digitalRead(PIN_DIR_INPUT); 139 | 140 | if (CW_ROTATION == NVM->SystemParams.dirPinRotation) 141 | { 142 | dir=!dir; //reverse the rotation 143 | } 144 | if (dir) 145 | { 146 | TC4->COUNT32.CTRLBSET.bit.DIR=1; 147 | WAIT_TC32_REGS_SYNC(TC4) 148 | } else 149 | { 150 | TC4->COUNT32.CTRLBCLR.bit.DIR=1; 151 | WAIT_TC32_REGS_SYNC(TC4) 152 | } 153 | } 154 | 155 | 156 | void stepPinSetup(void) 157 | { 158 | 159 | 160 | #ifdef USE_TC_STEP 161 | //attachInterrupt configures the EIC as highest priority interrupts. 162 | attachInterrupt(digitalPinToInterrupt(PIN_DIR_INPUT), dirChanged_ISR, CHANGE); 163 | setupStepEvent(); 164 | 165 | 166 | #else 167 | attachInterrupt(digitalPinToInterrupt(PIN_STEP_INPUT), stepInput, RISING); 168 | NVIC_SetPriority(EIC_IRQn, 0); //set port A interrupt as highest priority 169 | #endif 170 | 171 | } 172 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/planner.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include "planner.h" 13 | 14 | #include "board.h" 15 | #include "wiring_private.h" 16 | #include "syslog.h" 17 | #include "angle.h" 18 | #include "Arduino.h" 19 | 20 | #define WAIT_TC16_REGS_SYNC(x) while(x->COUNT16.STATUS.bit.SYNCBUSY); 21 | 22 | //define the planner class as being global 23 | Planner SmartPlanner; 24 | 25 | static bool enterTC3CriticalSection() 26 | { 27 | bool state=NVIC_IS_IRQ_ENABLED(TC3_IRQn); 28 | NVIC_DisableIRQ(TC3_IRQn); 29 | return state; 30 | } 31 | 32 | static void exitTC3CriticalSection(bool prevState) 33 | { 34 | if (prevState) 35 | { 36 | NVIC_EnableIRQ(TC3_IRQn); 37 | } //else do nothing 38 | } 39 | 40 | 41 | 42 | 43 | void TC3_Init(void) 44 | { 45 | // Enable GCLK for TC3 46 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC2_TC3 )) ; 47 | while (GCLK->STATUS.bit.SYNCBUSY); 48 | 49 | TC3->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE; // Disable TCx 50 | WAIT_TC16_REGS_SYNC(TC3) // wait for sync 51 | 52 | TC3->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16; // Set Timer counter Mode to 16 bits 53 | WAIT_TC16_REGS_SYNC(TC3) 54 | 55 | TC3->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ; // Set TC as normal Normal Frq 56 | WAIT_TC16_REGS_SYNC(TC3) 57 | 58 | TC3->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV2; // Set perscaler 59 | WAIT_TC16_REGS_SYNC(TC3) 60 | 61 | 62 | TC3->COUNT16.CC[0].reg = F_CPU/PLANNER_UPDATE_RATE_HZ/2; //divide by two because of prescaler 63 | 64 | WAIT_TC16_REGS_SYNC(TC3) 65 | 66 | 67 | TC3->COUNT16.INTENSET.reg = 0; // disable all interrupts 68 | TC3->COUNT16.INTENSET.bit.OVF = 1; // enable overfollow 69 | 70 | 71 | 72 | NVIC_SetPriority(TC3_IRQn, 3); 73 | 74 | 75 | // Enable InterruptVector 76 | NVIC_EnableIRQ(TC3_IRQn); 77 | 78 | 79 | // Enable TC 80 | TC3->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; 81 | WAIT_TC16_REGS_SYNC(TC3); 82 | } 83 | 84 | 85 | void TC3_Handler(void) 86 | { 87 | //do the planner tick 88 | SmartPlanner.tick(); 89 | //SerialUSB.println('x'); 90 | TC3->COUNT16.INTFLAG.bit.OVF = 1; //clear interrupt by writing 1 to flag 91 | } 92 | 93 | void Planner::begin(StepperCtrl *ptrStepper) 94 | { 95 | 96 | ptrStepperCtrl=ptrStepper; 97 | currentMode=PLANNER_NONE; 98 | //setup the timer and interrupt as the last thing 99 | TC3_Init(); 100 | } 101 | 102 | void Planner::tick(void) 103 | { 104 | if (currentMode == PLANNER_NONE) 105 | { 106 | return; //do nothing 107 | } 108 | 109 | if (PLANNER_CV == currentMode) 110 | { 111 | // SerialUSB.println(currentSetAngle); 112 | // SerialUSB.println(endAngle); 113 | // SerialUSB.println(tickIncrement); 114 | // SerialUSB.println(fabs(currentSetAngle-endAngle)); 115 | // SerialUSB.println(fabs(tickIncrement*2)); 116 | // SerialUSB.println(); 117 | int32_t x; 118 | if (fabs(currentSetAngle-endAngle) >= fabs(tickIncrement)) 119 | { 120 | currentSetAngle+=tickIncrement; 121 | x=ANGLE_FROM_DEGREES(currentSetAngle); 122 | ptrStepperCtrl->moveToAbsAngle(x); 123 | }else 124 | { 125 | //we are done, make sure we end at the right point 126 | //SerialUSB.println("done"); 127 | x=ANGLE_FROM_DEGREES(endAngle); 128 | ptrStepperCtrl->moveToAbsAngle(x); 129 | currentMode=PLANNER_NONE; 130 | } 131 | } 132 | 133 | 134 | } 135 | 136 | void Planner::stop(void) 137 | { 138 | bool state; 139 | state = enterTC3CriticalSection(); 140 | currentMode=PLANNER_NONE; 141 | exitTC3CriticalSection(state); 142 | } 143 | 144 | bool Planner::moveConstantVelocity(float finalAngle, float rpm) 145 | { 146 | bool state; 147 | state = enterTC3CriticalSection(); 148 | 149 | //first determine if operation is in progress 150 | if (PLANNER_NONE != currentMode) 151 | { 152 | //we are in operation return false 153 | SerialUSB.println("planner operational"); 154 | exitTC3CriticalSection(state); 155 | return false; 156 | } 157 | 158 | //get current posistion 159 | startAngle = ANGLE_T0_DEGREES(ptrStepperCtrl->getCurrentAngle()); 160 | 161 | //deterime the tick increment 162 | tickIncrement=360.0*fabs(rpm)/60/PLANNER_UPDATE_RATE_HZ; 163 | 164 | 165 | 166 | //set the desired end angle 167 | endAngle=finalAngle; 168 | 169 | 170 | //set the current angle 171 | currentSetAngle=startAngle; 172 | 173 | if (startAngle>endAngle) 174 | { 175 | SerialUSB.println("reverse"); 176 | tickIncrement=-tickIncrement; 177 | } 178 | 179 | // SerialUSB.println(currentSetAngle); 180 | // SerialUSB.println(endAngle); 181 | // SerialUSB.println(tickIncrement); 182 | // SerialUSB.println(); 183 | 184 | currentMode=PLANNER_CV; 185 | 186 | exitTC3CriticalSection(state); 187 | return true; 188 | } 189 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/Adafruit_GFX.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADAFRUIT_GFX_H 2 | #define _ADAFRUIT_GFX_H 3 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 4 | #define ARDUINO 101 //mks mark 2018-01 5 | #endif 6 | 7 | #if ARDUINO >= 100 8 | #include "Arduino.h" 9 | #include "Print.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include "gfxfont.h" 15 | 16 | class Adafruit_GFX : public Print { 17 | 18 | public: 19 | 20 | Adafruit_GFX(int16_t w, int16_t h); // Constructor 21 | 22 | // This MUST be defined by the subclass: 23 | virtual void drawPixel(int16_t x, int16_t y, uint16_t color) = 0; 24 | 25 | // These MAY be overridden by the subclass to provide device-specific 26 | // optimized code. Otherwise 'generic' versions are used. 27 | virtual void 28 | drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color), 29 | drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color), 30 | drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color), 31 | drawRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color), 32 | fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color), 33 | fillScreen(uint16_t color), 34 | invertDisplay(boolean i); 35 | 36 | // These exist only with Adafruit_GFX (no subclass overrides) 37 | void 38 | drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color), 39 | drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, 40 | uint16_t color), 41 | fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color), 42 | fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername, 43 | int16_t delta, uint16_t color), 44 | drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, 45 | int16_t x2, int16_t y2, uint16_t color), 46 | fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1, 47 | int16_t x2, int16_t y2, uint16_t color), 48 | drawRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, 49 | int16_t radius, uint16_t color), 50 | fillRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h, 51 | int16_t radius, uint16_t color), 52 | drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap, 53 | int16_t w, int16_t h, uint16_t color), 54 | drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap, 55 | int16_t w, int16_t h, uint16_t color, uint16_t bg), 56 | drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, 57 | int16_t w, int16_t h, uint16_t color), 58 | drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, 59 | int16_t w, int16_t h, uint16_t color, uint16_t bg), 60 | drawXBitmap(int16_t x, int16_t y, const uint8_t *bitmap, 61 | int16_t w, int16_t h, uint16_t color), 62 | drawChar(int16_t x, int16_t y, unsigned char c, uint16_t color, 63 | uint16_t bg, uint8_t size), 64 | setCursor(int16_t x, int16_t y), 65 | setTextColor(uint16_t c), 66 | setTextColor(uint16_t c, uint16_t bg), 67 | setTextSize(uint8_t s), 68 | setTextWrap(boolean w), 69 | setRotation(uint8_t r), 70 | cp437(boolean x=true), 71 | setFont(const GFXfont *f = NULL), 72 | getTextBounds(char *string, int16_t x, int16_t y, 73 | int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h), 74 | getTextBounds(const __FlashStringHelper *s, int16_t x, int16_t y, 75 | int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h); 76 | 77 | #if ARDUINO >= 100 78 | virtual size_t write(uint8_t); 79 | #else 80 | virtual void write(uint8_t); 81 | #endif 82 | 83 | int16_t height(void) const; 84 | int16_t width(void) const; 85 | 86 | uint8_t getRotation(void) const; 87 | 88 | // get current cursor position (get rotation safe maximum values, using: width() for x, height() for y) 89 | int16_t getCursorX(void) const; 90 | int16_t getCursorY(void) const; 91 | 92 | protected: 93 | const int16_t 94 | WIDTH, HEIGHT; // This is the 'raw' display w/h - never changes 95 | int16_t 96 | _width, _height, // Display w/h as modified by current rotation 97 | cursor_x, cursor_y; 98 | uint16_t 99 | textcolor, textbgcolor; 100 | uint8_t 101 | textsize, 102 | rotation; 103 | boolean 104 | wrap, // If set, 'wrap' text at right edge of display 105 | _cp437; // If set, use correct CP437 charset (default is off) 106 | GFXfont 107 | *gfxFont; 108 | }; 109 | 110 | class Adafruit_GFX_Button { 111 | 112 | public: 113 | Adafruit_GFX_Button(void); 114 | void initButton(Adafruit_GFX *gfx, int16_t x, int16_t y, 115 | uint8_t w, uint8_t h, uint16_t outline, uint16_t fill, 116 | uint16_t textcolor, char *label, uint8_t textsize); 117 | void drawButton(boolean inverted = false); 118 | boolean contains(int16_t x, int16_t y); 119 | 120 | void press(boolean p); 121 | boolean isPressed(); 122 | boolean justPressed(); 123 | boolean justReleased(); 124 | 125 | private: 126 | Adafruit_GFX *_gfx; 127 | int16_t _x, _y; 128 | uint16_t _w, _h; 129 | uint8_t _textsize; 130 | uint16_t _outlinecolor, _fillcolor, _textcolor; 131 | char _label[10]; 132 | 133 | boolean currstate, laststate; 134 | }; 135 | 136 | class GFXcanvas1 : public Adafruit_GFX { 137 | 138 | public: 139 | GFXcanvas1(uint16_t w, uint16_t h); 140 | ~GFXcanvas1(void); 141 | void drawPixel(int16_t x, int16_t y, uint16_t color), 142 | fillScreen(uint16_t color); 143 | uint8_t *getBuffer(void); 144 | private: 145 | uint8_t *buffer; 146 | }; 147 | 148 | class GFXcanvas16 : public Adafruit_GFX { 149 | GFXcanvas16(uint16_t w, uint16_t h); 150 | ~GFXcanvas16(void); 151 | void drawPixel(int16_t x, int16_t y, uint16_t color), 152 | fillScreen(uint16_t color); 153 | uint16_t *getBuffer(void); 154 | private: 155 | uint16_t *buffer; 156 | }; 157 | 158 | #endif // _ADAFRUIT_GFX_H 159 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/syslog.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * syslog.c 3 | * 4 | * Created on: Sep 14, 2011 5 | * Author: trampas.stern 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | #include "syslog.h" 16 | #include 17 | 18 | 19 | #define ANSI_WHITE "\033[37m" 20 | #define ANSI_NORMAL "\033[0m" 21 | #define ANSI_BLINK "\033[5m" 22 | #define ANSI_BLUE "\033[34m" 23 | #define ANSI_MAGENTA "\033[35m" 24 | #define ANSI_CYAN "\033[36m" 25 | #define ANSI_WHITE "\033[37m" 26 | #define ANSI_RED "\033[31m" 27 | #define ANSI_GREEN "\033[32m" 28 | #define ANSI_PINK "\033[35m\033[1m" 29 | #define ANSI_BROWN "\033[33m" 30 | #define ANSI_YELLOW "\033[33m\033[1m" 31 | #define ANSI_BLACK "\033[30m" 32 | #define ANSI_BELL_AND_RED "\a\033[31m" 33 | 34 | #define NEW_LINE "\n\r" 35 | 36 | Uart *ptrSerial=NULL; 37 | eLogLevel SyslogLevelToWrite; 38 | 39 | static char buffer[SYSLOG_BUFFER_SIZE]; 40 | static unsigned int BufIndex=0; 41 | 42 | static int SysLog_Enabled=1; 43 | 44 | int SysLogDisable(void) 45 | { 46 | SysLog_Enabled=0; 47 | return 0; 48 | } 49 | 50 | int SysLogEnable(void) 51 | { 52 | SysLog_Enabled=1; 53 | return 0; 54 | } 55 | 56 | int SysLogIsEnabled(void) 57 | { 58 | return SysLog_Enabled; 59 | } 60 | 61 | void SysLogPuts(const char *ptrStr) 62 | { 63 | if (!SysLog_Enabled) 64 | return; 65 | 66 | if (NULL == ptrSerial) 67 | { 68 | while(*ptrStr) 69 | { 70 | 71 | SYSLOG_PUTC(*ptrStr); 72 | ptrStr++; 73 | } 74 | } else 75 | { 76 | ptrSerial->write(ptrStr); 77 | } 78 | } 79 | 80 | int SysLogInitDone=0; 81 | void SysLogInit(Uart *ptrSerialObj, eLogLevel LevelToWrite) 82 | { 83 | ptrSerial=ptrSerialObj; 84 | SyslogLevelToWrite=LevelToWrite; 85 | 86 | SysLogInitDone=1; 87 | BufIndex=0; 88 | memset(buffer,0,SYSLOG_BUFFER_SIZE); 89 | } 90 | 91 | 92 | int SysLogProcessing=0; // this is used such that syslog can be reentrent 93 | int SysLogMissed=0; 94 | 95 | 96 | void SysLog(eLogLevel priorty, const char *fmt, ...) 97 | { 98 | //UINT32 ret; 99 | int previousState=SysLog_Enabled; 100 | char vastr[MAX_SYSLOG_STRING]={0}; 101 | //char outstr[MAX_SYSLOG_STRING]={0}; 102 | 103 | 104 | va_list ap; 105 | 106 | if (SysLogProcessing) 107 | { 108 | //we have a syslog from a syslog call thus return as not much we can do... 109 | //memset(buffer,0,SYSLOG_BUFFER_SIZE); 110 | va_start(ap,fmt); 111 | vsnprintf(&buffer[BufIndex],SYSLOG_BUFFER_SIZE-BufIndex,(char *)fmt,ap); 112 | BufIndex=strlen(buffer); 113 | snprintf(&buffer[BufIndex],SYSLOG_BUFFER_SIZE-BufIndex,NEW_LINE); 114 | BufIndex=strlen(buffer); 115 | SysLogMissed++; //set flag that we missed a call 116 | return; 117 | } 118 | 119 | SysLogProcessing=1; 120 | 121 | //stop the watch dog will doing a SysLog print 122 | Sys_WDogHoldOn(); 123 | 124 | if(!SysLogInitDone) 125 | { 126 | SysLogInit(NULL, LOG_WARNING); //not sure who is reseting serial port but before we print set it up 127 | WARNING("You should init SysLog"); 128 | //SysLogInitDone=0; 129 | } 130 | 131 | //Send out a * that we missed a SysLog Message before this current message 132 | if (SysLogMissed) 133 | { 134 | //SysLogPuts(ANSI_RED); 135 | SysLogPuts("*** Reentrant Log call possible loss of message(s):"); 136 | SysLogPuts(NEW_LINE); 137 | if (BufIndex>0) 138 | { 139 | SysLogPuts(buffer); 140 | memset(buffer,0,SYSLOG_BUFFER_SIZE); 141 | BufIndex=0; 142 | } 143 | //SysLogPuts(ANSI_RED); 144 | SysLogPuts("***********"); 145 | SysLogPuts(NEW_LINE); 146 | SysLogMissed=0; 147 | } 148 | memset(vastr,0,MAX_SYSLOG_STRING); 149 | va_start(ap,fmt); 150 | //#ifndef PGM_P 151 | #if 1 152 | vsnprintf(vastr,MAX_SYSLOG_STRING,(char *)fmt,ap); 153 | #else 154 | vsprintf_P(vastr,(const char *)fmt,ap); 155 | #endif 156 | //get time and store in datetimestr if desired 157 | //sprintf(outstr, "[%s] %s\r\n", datetimestr, vastr); 158 | 159 | 160 | 161 | if (priorty<=LOG_ERROR) 162 | { 163 | SysLog_Enabled=1; 164 | SysLogPuts(ANSI_RED); 165 | 166 | }else if (priorty==LOG_DEBUG) 167 | { 168 | SysLogPuts(ANSI_WHITE); 169 | }else if (priorty==LOG_WARNING) 170 | { 171 | SysLogPuts(ANSI_BLUE); 172 | } 173 | 174 | #ifdef RTC_H_ 175 | #ifdef TIME_H_ 176 | { 177 | struct tm tp; 178 | RTC_Time_s tm; 179 | time_t secs; 180 | char datetimestr[MAX_SYSLOG_STRING]={0}; 181 | 182 | RTC_ReadTime(&tm); 183 | secs=tm.seconds; 184 | convertFlexNetTime((time_t *)&secs, &tp); 185 | time_str(datetimestr,&tp); 186 | SysLogPuts(datetimestr); 187 | 188 | if (priorty<=SyslogLevelToWrite && SysLogWriteFunc!=NULL) 189 | { 190 | SysLogWriteFunc(datetimestr,strlen(datetimestr)); 191 | } 192 | } 193 | #endif 194 | #endif 195 | 196 | SysLogPuts(vastr); 197 | // 198 | // if (priorty<=SyslogLevelToWrite && SysLogWriteFunc!=NULL) 199 | // { 200 | // SysLogWriteFunc(vastr,strlen(vastr)); 201 | // SysLogWriteFunc(NEW_LINE,strlen(NEW_LINE)); 202 | // } 203 | 204 | 205 | SysLogPuts(ANSI_NORMAL); 206 | SysLogPuts(NEW_LINE); 207 | 208 | 209 | 210 | if (priorty == LOG_EMERG) { 211 | //you can reboot processor here 212 | } 213 | 214 | //start the watch dog where left off.. 215 | Sys_WDogHoldOff(); 216 | SysLogProcessing=0; 217 | SysLog_Enabled=previousState; 218 | return; 219 | } 220 | 221 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/Adafruit_SSD1306.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | This is a library for our Monochrome OLEDs based on SSD1306 drivers 3 | 4 | Pick one up today in the adafruit shop! 5 | ------> http://www.adafruit.com/category/63_98 6 | 7 | These displays use SPI to communicate, 4 or 5 pins are required to 8 | interface 9 | 10 | Adafruit invests time and resources providing this open source code, 11 | please support Adafruit and open-source hardware by purchasing 12 | products from Adafruit! 13 | 14 | Written by Limor Fried/Ladyada for Adafruit Industries. 15 | BSD license, check license.txt for more information 16 | All text above, and the splash screen must be included in any redistribution 17 | *********************************************************************/ 18 | #ifndef _Adafruit_SSD1306_H_ 19 | #define _Adafruit_SSD1306_H_ 20 | 21 | #if ARDUINO >= 100 22 | #include "Arduino.h" 23 | #define WIRE_WRITE Wire.write 24 | #else 25 | #include "WProgram.h" 26 | #define WIRE_WRITE Wire.send 27 | #endif 28 | 29 | #if defined(__SAM3X8E__) 30 | typedef volatile RwReg PortReg; 31 | typedef uint32_t PortMask; 32 | #define HAVE_PORTREG 33 | #elif defined(ARDUINO_ARCH_SAMD) 34 | // not supported 35 | #elif defined(ESP8266) || defined(ARDUINO_STM32_FEATHER) 36 | typedef volatile uint32_t PortReg; 37 | typedef uint32_t PortMask; 38 | #else 39 | typedef volatile uint8_t PortReg; 40 | typedef uint8_t PortMask; 41 | #define HAVE_PORTREG 42 | #endif 43 | 44 | #include 45 | #include "Adafruit_GFX.h" 46 | 47 | #define BLACK 0 48 | #define WHITE 1 49 | #define INVERSE 2 50 | 51 | #define SSD1306_I2C_ADDRESS 0x3C // 011110+SA0+RW - 0x3C or 0x3D 52 | // Address for 128x32 is 0x3C 53 | // Address for 128x64 is 0x3D (default) or 0x3C (if SA0 is grounded) 54 | 55 | /*========================================================================= 56 | SSD1306 Displays 57 | ----------------------------------------------------------------------- 58 | The driver is used in multiple displays (128x64, 128x32, etc.). 59 | Select the appropriate display below to create an appropriately 60 | sized framebuffer, etc. 61 | 62 | SSD1306_128_64 128x64 pixel display 63 | 64 | SSD1306_128_32 128x32 pixel display 65 | 66 | SSD1306_96_16 67 | 68 | -----------------------------------------------------------------------*/ 69 | #define SSD1306_128_64 70 | // #define SSD1306_128_32 71 | // #define SSD1306_96_16 72 | /*=========================================================================*/ 73 | 74 | #if defined SSD1306_128_64 && defined SSD1306_128_32 75 | #error "Only one SSD1306 display can be specified at once in SSD1306.h" 76 | #endif 77 | #if !defined SSD1306_128_64 && !defined SSD1306_128_32 && !defined SSD1306_96_16 78 | #error "At least one SSD1306 display must be specified in SSD1306.h" 79 | #endif 80 | 81 | #if defined SSD1306_128_64 82 | #define SSD1306_LCDWIDTH 128 83 | #define SSD1306_LCDHEIGHT 64 84 | #endif 85 | #if defined SSD1306_128_32 86 | #define SSD1306_LCDWIDTH 128 87 | #define SSD1306_LCDHEIGHT 32 88 | #endif 89 | #if defined SSD1306_96_16 90 | #define SSD1306_LCDWIDTH 96 91 | #define SSD1306_LCDHEIGHT 16 92 | #endif 93 | 94 | #define SSD1306_SETCONTRAST 0x81 95 | #define SSD1306_DISPLAYALLON_RESUME 0xA4 96 | #define SSD1306_DISPLAYALLON 0xA5 97 | #define SSD1306_NORMALDISPLAY 0xA6 98 | #define SSD1306_INVERTDISPLAY 0xA7 99 | #define SSD1306_DISPLAYOFF 0xAE 100 | #define SSD1306_DISPLAYON 0xAF 101 | 102 | #define SSD1306_SETDISPLAYOFFSET 0xD3 103 | #define SSD1306_SETCOMPINS 0xDA 104 | 105 | #define SSD1306_SETVCOMDETECT 0xDB 106 | 107 | #define SSD1306_SETDISPLAYCLOCKDIV 0xD5 108 | #define SSD1306_SETPRECHARGE 0xD9 109 | 110 | #define SSD1306_SETMULTIPLEX 0xA8 111 | 112 | #define SSD1306_SETLOWCOLUMN 0x00 113 | #define SSD1306_SETHIGHCOLUMN 0x10 114 | 115 | #define SSD1306_SETSTARTLINE 0x40 116 | 117 | #define SSD1306_MEMORYMODE 0x20 118 | #define SSD1306_COLUMNADDR 0x21 119 | #define SSD1306_PAGEADDR 0x22 120 | 121 | #define SSD1306_COMSCANINC 0xC0 122 | #define SSD1306_COMSCANDEC 0xC8 123 | 124 | #define SSD1306_SEGREMAP 0xA0 125 | 126 | #define SSD1306_CHARGEPUMP 0x8D 127 | 128 | #define SSD1306_EXTERNALVCC 0x1 129 | #define SSD1306_SWITCHCAPVCC 0x2 130 | 131 | // Scrolling #defines 132 | #define SSD1306_ACTIVATE_SCROLL 0x2F 133 | #define SSD1306_DEACTIVATE_SCROLL 0x2E 134 | #define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3 135 | #define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26 136 | #define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27 137 | #define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29 138 | #define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A 139 | 140 | class Adafruit_SSD1306 : public Adafruit_GFX { 141 | public: 142 | Adafruit_SSD1306(int8_t SID, int8_t SCLK, int8_t DC, int8_t RST, int8_t CS); 143 | Adafruit_SSD1306(int8_t DC, int8_t RST, int8_t CS); 144 | Adafruit_SSD1306(int8_t RST = -1); 145 | 146 | bool begin(uint8_t switchvcc = SSD1306_SWITCHCAPVCC, uint8_t i2caddr = SSD1306_I2C_ADDRESS, bool reset=true); 147 | uint8_t ssd1306_command(uint8_t c); 148 | 149 | void clearDisplay(void); 150 | void invertDisplay(uint8_t i); 151 | void display(); 152 | 153 | void startscrollright(uint8_t start, uint8_t stop); 154 | void startscrollleft(uint8_t start, uint8_t stop); 155 | 156 | void startscrolldiagright(uint8_t start, uint8_t stop); 157 | void startscrolldiagleft(uint8_t start, uint8_t stop); 158 | void stopscroll(void); 159 | 160 | void dim(boolean dim); 161 | 162 | void drawPixel(int16_t x, int16_t y, uint16_t color); 163 | 164 | virtual void drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color); 165 | virtual void drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color); 166 | 167 | private: 168 | int8_t _i2caddr, _vccstate, sid, sclk, dc, rst, cs; 169 | void fastSPIwrite(uint8_t c); 170 | 171 | boolean hwSPI; 172 | #ifdef HAVE_PORTREG 173 | PortReg *mosiport, *clkport, *csport, *dcport; 174 | PortMask mosipinmask, clkpinmask, cspinmask, dcpinmask; 175 | #endif 176 | 177 | inline void drawFastVLineInternal(int16_t x, int16_t y, int16_t h, uint16_t color) __attribute__((always_inline)); 178 | inline void drawFastHLineInternal(int16_t x, int16_t y, int16_t w, uint16_t color) __attribute__((always_inline)); 179 | 180 | }; 181 | 182 | #endif /* _Adafruit_SSD1306_H_ */ 183 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/stepper_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #ifndef __STEPPER_CONTROLLER_H__ 13 | #define __STEPPER_CONTROLLER_H__ 14 | 15 | #include "syslog.h" 16 | #include "board.h" 17 | #include "as5047d.h" 18 | #include "a1333.h" 19 | #include "calibration.h" 20 | #include "A4954.h" 21 | #include "A5995.h" 22 | #include "nonvolatile.h" 23 | #include "fet_driver.h" //for the NEMA23 10A 24 | 25 | 26 | #define N_DATA (1024) 27 | 28 | 29 | typedef enum { 30 | STEPCTRL_NO_ERROR=0, 31 | STEPCTRL_NO_POWER=1, //no power to motor 32 | STEPCTRL_NO_CAL=2, //calibration not set 33 | STEPCTRL_NO_ENCODER=3, //encoder not working 34 | } stepCtrlError_t; 35 | 36 | 37 | typedef struct { 38 | int32_t Kp; 39 | int32_t Ki; 40 | int32_t Kd; 41 | } PID_t; 42 | 43 | 44 | typedef __attribute__((packed, aligned(4))) struct { 45 | int32_t microSecs; 46 | int32_t desiredLoc; 47 | int32_t actualLoc; 48 | int32_t angle; 49 | int32_t ma; 50 | } Location_t; 51 | 52 | 53 | typedef struct { 54 | int32_t angle; 55 | int32_t ma; 56 | }Control_t; 57 | 58 | #define MAX_NUM_LOCATIONS (64) //maximum number of locations to buffer 59 | 60 | 61 | //this scales the PID parameters from Flash to floating point 62 | // to fixed point int32_t values 63 | #define CTRL_PID_SCALING (1024) 64 | 65 | class StepperCtrl 66 | { 67 | private: 68 | volatile bool enableFeedback; //true if we are using PID control algorithm 69 | 70 | #if !defined(USE_A1333_ENCODER) 71 | AS5047D encoder; 72 | #else 73 | A1333 encoder; 74 | #endif 75 | 76 | #ifdef NEMA_23_10A_HW 77 | FetDriver stepperDriver; 78 | #else 79 | #ifdef A5995_DRIVER 80 | A5995 stepperDriver; 81 | #else 82 | A4954 stepperDriver; 83 | #endif 84 | #endif 85 | uint16_t startUpEncoder; 86 | volatile int32_t ticks=0; 87 | volatile Location_t locs[MAX_NUM_LOCATIONS]; 88 | volatile int32_t locReadIndx=0; 89 | volatile int32_t locWriteIndx=0; 90 | 91 | volatile MotorParams_t motorParams; 92 | volatile SystemParams_t systemParams; 93 | volatile bool enabled; 94 | 95 | 96 | 97 | volatile int32_t loopTimeus; //time to run loop in microseconds 98 | 99 | volatile PID_t sPID; //simple control loop PID parameters 100 | volatile PID_t pPID; //positional current based PID control parameters 101 | volatile PID_t vPID; //velocity PID control parameters 102 | 103 | volatile int64_t numSteps; //this is the number of steps we have taken from our start angle 104 | 105 | volatile int32_t loopError; 106 | 107 | volatile int64_t currentLocation; //estimate of the current location from encoder feedback 108 | // the current location lower 16 bits is angle (0-360 degrees in 65536 steps) while upper 109 | // bits is the number of full rotations. 110 | 111 | //this is used for the velocity PID feedback 112 | // units are in Angles/sec where 1 Angle=360deg/65536 113 | volatile int64_t velocity; 114 | 115 | int64_t zeroAngleOffset=0; 116 | 117 | 118 | //volatile int16_t data[N_DATA]; 119 | 120 | //does linear interpolation of the encoder calibration table 121 | int32_t getAngleCalibration(int32_t encoderAngle); 122 | 123 | //updates the currentMeasuredAngle with our best guess where we are 124 | Angle sampleAngle(void); 125 | Angle sampleMeanEncoder(int32_t numSamples); 126 | 127 | float measureStepSize(void); //steps motor and estimates step size 128 | uint32_t measureMaxCalibrationError(void); 129 | void setLocationFromEncoder(void); 130 | 131 | void motorReset(void); 132 | void updateStep(int dir, uint16_t steps); 133 | 134 | 135 | bool pidFeedback(int64_t desiredLoc, int64_t currentLoc, Control_t *ptrCtrl); 136 | bool simpleFeedback(int64_t desiredLoc, int64_t currentLoc,Control_t *ptrCtrl); 137 | bool vpidFeedback(int64_t desiredLoc, int64_t currentLoc,Control_t *ptrCtrl); 138 | int64_t getCurrentLocation(void); 139 | int64_t getDesiredLocation(void); 140 | void updateLocTable(int64_t desiredLoc, int64_t currentLoc,Control_t *ptrCtrl); 141 | 142 | int64_t calculatePhasePrediction(int64_t currentLoc); 143 | bool determineError(int64_t currentLoc, int64_t error); 144 | 145 | public: 146 | uint16_t getStartupEncoder(void) {return startUpEncoder;} 147 | int32_t getLocation(Location_t *ptrLoc); 148 | 149 | //int32_t getSteps(void); 150 | Angle getEncoderAngle(void); 151 | 152 | void setAngle(int64_t loc); 153 | 154 | int64_t getZeroAngleOffset(void); 155 | void PrintData(void); 156 | void setVelocity(int64_t vel); //set velocity for vPID mode 157 | int64_t getVelocity(void); 158 | int32_t getLoopError(void) {return loopError;}; //assume atomic read 159 | 160 | bool calibrationValid(void) { return calTable.calValid();} //returns true if calbiration is good 161 | 162 | void updateParamsFromNVM(void); //updates the parameters from NVM 163 | CalibrationTable calTable; 164 | //void printData(void); 165 | 166 | bool calibrateEncoder(void); //do manual calibration of the encoder 167 | Angle maxCalibrationError(void); //measures the maximum calibration error as an angle 168 | 169 | void moveToAbsAngle(int32_t a); 170 | void moveToAngle(int32_t a, uint32_t ma); 171 | 172 | stepCtrlError_t begin(void); //returns false if we can not use motor 173 | 174 | bool processFeedback(void); // does the feedback loop 175 | 176 | feedbackCtrl_t getControlMode(void) { return systemParams.controllerMode;}; 177 | 178 | void requestStep(int dir, uint16_t steps); //requests a step, if feedback controller is off motor does not move 179 | 180 | void feedback(bool enable); 181 | bool getFeedback(void) {return enableFeedback;} 182 | 183 | void encoderDiagnostics(char *ptrStr); 184 | int32_t measureError(void); 185 | 186 | //these two functions are compenstated by the zero offset 187 | int64_t getCurrentAngle(void); 188 | int64_t getDesiredAngle(void); 189 | 190 | void move(int dir, uint16_t steps); //forces motor to move even if feedback controller is turned off. 191 | void enable(bool enable); 192 | bool getEnable(void) {return enabled;} 193 | 194 | int32_t getLoopTime(void) { return loopTimeus;} 195 | 196 | void PID_Autotune(void); 197 | void setZero(void); 198 | }; 199 | 200 | #endif //__STEPPER_CONTROLLER_H__ 201 | 202 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A5995.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * A5995.cpp 3 | * 4 | * Created on: Feb 2, 2017 5 | * Author: tstern 6 | */ 7 | 8 | 9 | #include "A5995.h" 10 | #include "wiring_private.h" 11 | #include "syslog.h" 12 | #include "angle.h" 13 | #include "Arduino.h" 14 | #include "sine.h" 15 | 16 | static uint8_t pinState=0; 17 | #ifndef MKS_GCC_O0 //mks mark ("-O0") 18 | #pragma GCC push_options 19 | #pragma GCC optimize ("-Ofast") 20 | #endif 21 | 22 | 23 | 24 | #define DAC_MAX (0x01FFL) 25 | // Wait for synchronization of registers between the clock domains 26 | static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); 27 | static void syncTCC(Tcc* TCCx) { 28 | //int32_t t0=1000; 29 | while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK) 30 | { 31 | // t0--; 32 | // if (t0==0) 33 | // { 34 | // break; 35 | // } 36 | // delay(1); 37 | } 38 | } 39 | 40 | 41 | 42 | static void setDAC(uint32_t DAC1, uint32_t DAC2) 43 | { 44 | TCC1->CC[1].reg = (uint32_t)DAC1; //D9 PA07 - VREF2 45 | syncTCC(TCC1); 46 | TCC1->CC[0].reg = (uint32_t)DAC2; //D4 - VREF1 47 | syncTCC(TCC1); 48 | 49 | } 50 | 51 | static void setupDAC(void) 52 | { 53 | Tcc* TCCx = TCC1 ; 54 | 55 | 56 | pinPeripheral(PIN_A5995_VREF1, PIO_TIMER_ALT); 57 | pinPeripheral(PIN_A5995_VREF2, PIO_TIMER); 58 | 59 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ; 60 | 61 | while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ; 62 | 63 | //ERROR("Setting TCC %d %d",ulValue,ulPin); 64 | TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE; 65 | syncTCC(TCCx); 66 | 67 | // Set TCx as normal PWM 68 | TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; 69 | syncTCC(TCCx); 70 | 71 | // Set TCx in waveform mode Normal PWM 72 | TCCx->CC[1].reg = (uint32_t)0; 73 | syncTCC(TCCx); 74 | 75 | TCCx->CC[0].reg = (uint32_t)0; 76 | syncTCC(TCCx); 77 | 78 | // Set PER to maximum counter value (resolution : 0xFFF = 12 bits) 79 | // =48e6/2^12=11kHz frequency 80 | TCCx->PER.reg = DAC_MAX; 81 | syncTCC(TCCx); 82 | 83 | // Enable TCCx 84 | TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; 85 | syncTCC(TCCx); 86 | 87 | } 88 | 89 | 90 | void A5995::begin() 91 | { 92 | //setup the A5995 pins 93 | digitalWrite(PIN_A5995_ENABLE1,LOW); 94 | pinMode(PIN_A5995_ENABLE1,OUTPUT); 95 | digitalWrite(PIN_A5995_ENABLE2,LOW); 96 | pinMode(PIN_A5995_ENABLE2,OUTPUT); 97 | digitalWrite(PIN_A5995_MODE1,LOW); 98 | pinMode(PIN_A5995_MODE1,OUTPUT); 99 | digitalWrite(PIN_A5995_MODE2,LOW); 100 | pinMode(PIN_A5995_MODE2,OUTPUT); 101 | digitalWrite(PIN_A5995_PHASE1,LOW); 102 | pinMode(PIN_A5995_PHASE1,OUTPUT); 103 | digitalWrite(PIN_A5995_PHASE2,LOW); 104 | pinMode(PIN_A5995_PHASE2,OUTPUT); 105 | 106 | digitalWrite(PIN_A5995_SLEEPn,HIGH); 107 | pinMode(PIN_A5995_SLEEPn,OUTPUT); 108 | 109 | 110 | 111 | //setup the PWM for current on the A4954, set for low current 112 | digitalWrite(PIN_A5995_VREF1,LOW); 113 | digitalWrite(PIN_A5995_VREF2,LOW); 114 | pinMode(PIN_A5995_VREF1, OUTPUT); 115 | pinMode(PIN_A5995_VREF2, OUTPUT); 116 | 117 | enabled=true; 118 | lastStepMicros=0; 119 | forwardRotation=true; 120 | 121 | setupDAC(); 122 | 123 | 124 | // 125 | // GPIO_HIGH(PIN_A5995_ENABLE1); 126 | // GPIO_HIGH(PIN_A5995_ENABLE2); 127 | // GPIO_LOW(PIN_A5995_MODE1); 128 | // GPIO_LOW(PIN_A5995_MODE2); 129 | // GPIO_HIGH(PIN_A5995_PHASE1); 130 | // GPIO_HIGH(PIN_A5995_PHASE2); 131 | // int i=0;; 132 | // while (1) 133 | // { 134 | // int32_t x; 135 | // WARNING("MA %d",i); 136 | // x=(int32_t)((int64_t)i*(DAC_MAX))/3300; 137 | // setDAC(x,x); 138 | // delay(1000); 139 | // i=i+10; 140 | // if (i>1000) 141 | // { 142 | // i=0; 143 | // } 144 | // 145 | // } 146 | 147 | 148 | return; 149 | } 150 | 151 | 152 | 153 | void A5995::enable(bool enable) 154 | { 155 | enabled=enable; 156 | if (enabled == false) 157 | { 158 | WARNING("A4954 disabled"); 159 | setDAC(0,0); //turn current off 160 | GPIO_LOW(PIN_A5995_ENABLE1); 161 | GPIO_LOW(PIN_A5995_ENABLE2); 162 | GPIO_LOW(PIN_A5995_MODE1); 163 | GPIO_LOW(PIN_A5995_MODE2); 164 | GPIO_LOW(PIN_A5995_PHASE1); 165 | GPIO_LOW(PIN_A5995_PHASE2); 166 | } 167 | } 168 | 169 | 170 | 171 | //this is precise move and modulo of A4954_NUM_MICROSTEPS is a full step. 172 | // stepAngle is in A4954_NUM_MICROSTEPS units.. 173 | // The A4954 has no idea where the motor is, so the calling function has to 174 | // to tell the A4954 what phase to drive motor coils. 175 | // A4954_NUM_MICROSTEPS is 256 by default so stepAngle of 1024 is 360 degrees 176 | // Note you can only move up to +/-A4954_NUM_MICROSTEPS from where you 177 | // currently are. 178 | int32_t A5995::move(int32_t stepAngle, uint32_t mA) 179 | { 180 | uint16_t angle; 181 | int32_t cos,sin; 182 | int32_t dacSin,dacCos; 183 | static int32_t lastSin=0,lastCos=0; 184 | static int i=1; 185 | 186 | if (enabled == false) 187 | { 188 | WARNING("A4954 disabled"); 189 | setDAC(0,0); //turn current off 190 | GPIO_LOW(PIN_A5995_ENABLE1); 191 | GPIO_LOW(PIN_A5995_ENABLE2); 192 | GPIO_LOW(PIN_A5995_MODE1); 193 | GPIO_LOW(PIN_A5995_MODE2); 194 | GPIO_LOW(PIN_A5995_PHASE1); 195 | GPIO_LOW(PIN_A5995_PHASE2); 196 | return stepAngle; 197 | } 198 | 199 | //WARNING("move %d %d",stepAngle,mA); 200 | 201 | stepAngle=(stepAngle) % SINE_STEPS; 202 | //figure out our sine Angle 203 | // note our SINE_STEPS is 4x of microsteps for a reason 204 | //angle=(stepAngle+(SINE_STEPS/8)) % SINE_STEPS; 205 | angle=stepAngle; 206 | 207 | if (i==0) 208 | { 209 | WARNING("angle %d ",angle); 210 | } 211 | //calculate the sine and cosine of our angle 212 | sin=sine(angle); 213 | cos=cosine(angle); 214 | 215 | //if we are reverse swap the sign of one of the angels 216 | if (false == forwardRotation) 217 | { 218 | cos=-cos; 219 | } 220 | 221 | //scale sine result by current(mA) 222 | dacSin=((int32_t)mA*(int64_t)(sin))/SINE_MAX; 223 | 224 | if (i==0) 225 | { 226 | WARNING("dacsine %d ",dacSin); 227 | } 228 | // if ((lastSin-dacSin)>100) //decreasing current 229 | // { 230 | // GPIO_LOW(PIN_A5995_MODE2); //fast decay 231 | // } else 232 | // { 233 | // GPIO_HIGH(PIN_A5995_MODE2); //slow decay 234 | // } 235 | lastSin=dacSin; 236 | 237 | //convert value into DAC scaled to 3300mA max 238 | dacSin=(int32_t)((int64_t)abs(dacSin)*(DAC_MAX))/3300; 239 | 240 | 241 | //scale cosine result by current(mA) 242 | dacCos=((int32_t)mA*(int64_t)(cos))/SINE_MAX; 243 | 244 | if (i==0) 245 | { 246 | WARNING("daccos %d ",dacCos); 247 | } 248 | // if ((lastCos-dacCos)>100) //decreasing current 249 | // { 250 | // GPIO_LOW(PIN_A5995_MODE1); //fast decay 251 | // } else 252 | // { 253 | // GPIO_HIGH(PIN_A5995_MODE1); //slow decay 254 | // } 255 | lastCos=dacCos; 256 | 257 | //convert value into DAC scaled to 3300mA max 258 | dacCos=(int32_t)((int64_t)abs(dacCos)*(DAC_MAX))/3300; 259 | 260 | 261 | if (i==0) 262 | { 263 | WARNING("dacs are %d %d",dacSin,dacCos); 264 | } 265 | setDAC(dacSin,dacCos); 266 | 267 | GPIO_HIGH(PIN_A5995_ENABLE1); 268 | GPIO_HIGH(PIN_A5995_ENABLE2); 269 | GPIO_LOW(PIN_A5995_MODE1); 270 | GPIO_LOW(PIN_A5995_MODE2); 271 | 272 | 273 | if (i==0) 274 | { 275 | WARNING("sins are %d %d",sin,cos); 276 | } 277 | 278 | if (sin>0) 279 | { 280 | GPIO_HIGH(PIN_A5995_PHASE2); 281 | }else 282 | { 283 | GPIO_LOW(PIN_A5995_PHASE2); 284 | 285 | } 286 | if (cos>0) 287 | { 288 | GPIO_HIGH(PIN_A5995_PHASE1); 289 | 290 | }else 291 | { 292 | GPIO_LOW(PIN_A5995_PHASE1); 293 | 294 | } 295 | 296 | // i++; 297 | // if (i>3000) i=0; 298 | // YELLOW_LED(led); 299 | // led=(led+1) & 0x01; 300 | lastStepMicros=micros(); 301 | return stepAngle; 302 | } 303 | #pragma GCC pop_options 304 | 305 | 306 | 307 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/eeprom.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * eeprom.cpp 3 | * 4 | * Created on: May 30, 2017 5 | * Author: tstern 6 | */ 7 | #include "eeprom.h" 8 | #include "calibration.h" 9 | #include "Flash.h" 10 | #include "board.h" //for divide with rounding macro 11 | #include 12 | #include "syslog.h" 13 | 14 | //since we will write the following structure into each page, we need to find our latest page 15 | // to do this we will use the header to contain a checksum and write counter. 16 | #define EEPROM_SIZE (FLASH_ROW_SIZE*2) 17 | 18 | typedef struct { 19 | uint16_t checksum; 20 | uint16_t count; 21 | }eepromHeader_t; 22 | 23 | #define EEPROM_DATA_SIZE (FLASH_PAGE_SIZE_NZS-sizeof(eepromHeader_t)) 24 | typedef struct { 25 | eepromHeader_t header; 26 | uint8_t data[EEPROM_DATA_SIZE]; 27 | } eepromData_t; 28 | 29 | 30 | 31 | static eepromData_t EEPROMCache; 32 | 33 | static int32_t NextPageWrite=-1; 34 | 35 | //we need to reserve two pages for EEPROM 36 | __attribute__((__aligned__(FLASH_ROW_SIZE))) const uint8_t NVM_eeprom[EEPROM_SIZE]={0xFF}; 37 | 38 | 39 | static uint16_t checksum(uint8_t *ptrData, uint32_t nBytes) 40 | { 41 | uint16_t sum=0; 42 | uint32_t i; 43 | i=0; 44 | //LOG("running checksum %d",nBytes); 45 | while(idata, EEPROM_DATA_SIZE); 61 | //LOG("checksum is %d %d",cs,ptrData->header.checksum); 62 | 63 | if (cs==ptrData->header.checksum) 64 | { 65 | //LOG("Page good %d",page); 66 | return true; 67 | } 68 | //LOG("page bad %d",page); 69 | return false; 70 | } 71 | 72 | static void printEEPROM(uint32_t page) 73 | { 74 | eepromData_t *ptrData; 75 | int i; 76 | ptrData=(eepromData_t *)&NVM_eeprom[page]; 77 | LOG("count %d", ptrData->header.count); 78 | LOG("checksum %d", ptrData->header.checksum); 79 | for (i=0; i<10; i++) 80 | { 81 | LOG("Data[%d]=%02X",i,ptrData->data[i]); 82 | } 83 | } 84 | 85 | static uint32_t findLastGoodPage(void) 86 | { 87 | uint32_t lastGoodPage=0; 88 | uint32_t page; 89 | uint16_t lastCnt=0; 90 | eepromData_t *ptrData; 91 | 92 | page=0; 93 | while(page < (EEPROM_SIZE)) 94 | { 95 | //LOG("checking page %d",page); 96 | if (isPageGood(page)) 97 | { 98 | ptrData=(eepromData_t *)&NVM_eeprom[page]; 99 | 100 | //check for roll over which is OK 101 | if (lastCnt==16534 && ptrData->header.count==1) 102 | { 103 | lastCnt=ptrData->header.count; 104 | lastGoodPage=page; 105 | } 106 | if (ptrData->header.count>lastCnt) 107 | { 108 | //make sure we have not rolled over. 109 | if ((ptrData->header.count-lastCnt)<(16534/2)) 110 | { 111 | lastCnt=ptrData->header.count; 112 | lastGoodPage=page; 113 | } 114 | } 115 | } 116 | page=page + FLASH_PAGE_SIZE_NZS; 117 | } 118 | //LOG("last good page %d",lastGoodPage); 119 | return lastGoodPage; 120 | } 121 | 122 | //find the next page to write 123 | static uint32_t eepromGetNextWritPage(void) 124 | { 125 | eepromHeader_t *ptrHeader; 126 | uint32_t page; 127 | uint32_t row; 128 | int blockCount; 129 | int done=0; 130 | 131 | //start at first address: 132 | page=0; 133 | 134 | while(page < (EEPROM_SIZE)) 135 | { 136 | //LOG("checking page %d",page); 137 | ptrHeader=(eepromHeader_t *) &NVM_eeprom[page]; 138 | if (ptrHeader->count == 0xFFFF) 139 | { 140 | uint32_t i; 141 | uint8_t *ptrData; 142 | //uint8_t erasedByte=(uint8_t)ptrHeader->count; 143 | bool erased=true; 144 | 145 | //verify page is erased 146 | ptrData= (uint8_t *)&NVM_eeprom[page]; 147 | 148 | for (i=0; i=EEPROM_SIZE) 175 | { 176 | row=0; 177 | //TODO we should make sure this not where good data is 178 | // however if it is what should we do? 179 | } 180 | 181 | //now we need to erase that row 182 | //WARNING("Erasing page %d",row*FLASH_ROW_SIZE); 183 | flashErase(&NVM_eeprom[row*FLASH_ROW_SIZE],FLASH_ROW_SIZE); 184 | page=row*FLASH_ROW_SIZE; 185 | //LOG("Next free page is %d",page); 186 | return page; 187 | } 188 | 189 | 190 | eepromError_t eepromInit(void) 191 | { 192 | uint32_t page; 193 | 194 | 195 | //find the last good page offset in flash 196 | page=findLastGoodPage(); 197 | LOG("EEPROM Init found page %d",page); 198 | if (isPageGood(page)) 199 | { 200 | LOG("EEPROM page good %d",page); 201 | memcpy(&EEPROMCache, &NVM_eeprom[page], sizeof(EEPROMCache)); 202 | 203 | NextPageWrite=eepromGetNextWritPage(); 204 | return EEPROM_OK; 205 | } 206 | //ERROR("page is bad"); 207 | memset(&EEPROMCache, 0, sizeof(EEPROMCache)); 208 | NextPageWrite=eepromGetNextWritPage(); 209 | return EEPROM_CORRUPT; 210 | } 211 | 212 | 213 | int eepromWriteCache(uint8_t *ptrData, uint32_t size) 214 | { 215 | //LOG("Cache write %d",size); 216 | if (NextPageWrite==-1) //some one did not init the module 217 | { 218 | //lets handle gracefully and do it ourselves 219 | eepromInit(); 220 | } 221 | if (size>EEPROM_DATA_SIZE) 222 | { 223 | size =EEPROM_DATA_SIZE; 224 | } 225 | memcpy(EEPROMCache.data, ptrData, size); 226 | EEPROMCache.header.checksum=checksum(EEPROMCache.data,EEPROM_DATA_SIZE); 227 | 228 | 229 | return size; 230 | } 231 | 232 | int eepromRead(uint8_t *ptrData, uint32_t size) //returns number of bytes actually read, whcih could be less than size requested 233 | { 234 | if (NextPageWrite==-1) //some one did not init the module 235 | { 236 | //lets handle gracefully and do it ourselves 237 | eepromInit(); 238 | } 239 | if (size>EEPROM_DATA_SIZE) 240 | { 241 | size =EEPROM_DATA_SIZE; 242 | } 243 | if (EEPROMCache.header.count == 0) 244 | { 245 | return 0; //cache is new/corrupt 246 | } 247 | memcpy(ptrData, EEPROMCache.data, size); 248 | return size; 249 | } 250 | 251 | eepromError_t eepromFlush(void) //flush the cache to flash memory 252 | { 253 | if (NextPageWrite==-1) 254 | { 255 | ERROR("EEPROM WRITE FAILED"); 256 | return EEPROM_FAILED; //most likely no one has written to cache 257 | } 258 | EEPROMCache.header.count++; 259 | if (EEPROMCache.header.count>=16535) 260 | { 261 | EEPROMCache.header.count=1; 262 | } 263 | //WARNING("Writting to Page %d",NextPageWrite); 264 | flashWrite(&NVM_eeprom[NextPageWrite], &EEPROMCache, sizeof(EEPROMCache)); 265 | 266 | // printEEPROM(NextPageWrite); 267 | 268 | if (!SYSCTRL->PCLKSR.bit.BOD33DET) //if not in brown out condition find next write location 269 | { 270 | //LOG("getting next page to write"); 271 | NextPageWrite=eepromGetNextWritPage(); //find next write location and erase if needed 272 | } else 273 | { 274 | //LOG("BOD active"); 275 | NextPageWrite=-1; //else we will just clear NextPageWrite location just in case we recover from brown out 276 | } 277 | return EEPROM_OK; 278 | } 279 | 280 | 281 | 282 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/as5047d.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include 13 | #include "syslog.h" 14 | #include "as5047d.h" 15 | #include "SPI.h" 16 | #include 17 | #include "board.h" 18 | 19 | #define AS5047D_CMD_NOP (0x0000) 20 | #define AS5047D_CMD_ERRFL (0x0001) 21 | #define AS5047D_CMD_PROG (0x0003) 22 | #define AS5047D_CMD_DIAAGC (0x3FFC) 23 | #define AS5047D_CMD_MAG (0x3FFD) 24 | #define AS5047D_CMD_ANGLEUNC (0x3FFE) 25 | #define AS5047D_CMD_ANGLECOM (0x3FFF) 26 | 27 | 28 | #define AS5048A_CMD_NOP (0x0000) 29 | #define AS5048A_CMD_ERRFL (0x0001) 30 | #define AS5048A_CMD_PROG (0x0003) 31 | #define AS5048A_CMD_DIAAGC (0x3FFD) 32 | #define AS5048A_CMD_MAG (0x3FFE) 33 | #define AS5048A_CMD_ANGLE (0x3FFF) 34 | #ifndef MKS_GCC_O0 //mks mark ("-O0") 35 | #pragma GCC push_options 36 | #pragma GCC optimize ("-Ofast") 37 | #endif 38 | static int getBit(int16_t data, int bit) 39 | { 40 | return (data>>bit) & 0x01; 41 | } 42 | 43 | static int getParity(uint16_t data) 44 | { 45 | int i,bits; 46 | data=data & 0x7FFF; //mask out upper bit 47 | 48 | //count number of bits, brute force 49 | bits=0; 50 | for(i=0; i<16; i++) 51 | { 52 | if (0 != (data & ((0x0001)<0) 98 | { 99 | delay(1); 100 | t0--; 101 | if (t0==0) 102 | { 103 | ERROR("LF bit not set"); 104 | error=true; 105 | break; 106 | //return false; 107 | } 108 | LOG("AS5047D diag data is 0x%04X",data); 109 | data=readAddress(AS5047D_CMD_DIAAGC); 110 | } 111 | 112 | if (error) 113 | { 114 | error=false; 115 | uint16_t data=0,t0=100; 116 | while (getBit(data,8)==0 && t0>0) 117 | { 118 | delay(1); 119 | t0--; 120 | if (t0==0) 121 | { 122 | ERROR("AS5048A OCF bit not set"); 123 | error=true; 124 | return false; 125 | } 126 | data=readAddress(AS5048A_CMD_DIAAGC); 127 | LOG("AS5048A diag data is 0x%04X",data); 128 | } 129 | as5047d=false; 130 | 131 | } 132 | 133 | 134 | #ifdef NZS_AS5047_PIPELINE 135 | //read encoder a few times to flush the pipeline 136 | readEncoderAnglePipeLineRead(); 137 | readEncoderAnglePipeLineRead(); 138 | #endif 139 | return true; 140 | } 141 | 142 | 143 | //read the encoders 144 | int16_t AS5047D::readAddress(uint16_t addr) 145 | { 146 | uint16_t data; 147 | error=false; 148 | //make sure it is a read by setting bit 14 149 | addr=addr | 0x4000; 150 | 151 | //add the parity to the command 152 | if (1 == getParity(addr)) 153 | { 154 | addr=(addr & 0x7FFF) | 0x8000; //add parity bit to make command even number of bits 155 | } 156 | 157 | digitalWrite(chipSelectPin, LOW); 158 | delayMicroseconds(1); 159 | //clock out the address to read 160 | SPI.transfer16(addr); 161 | digitalWrite(chipSelectPin, HIGH); 162 | delayMicroseconds(1); 163 | digitalWrite(chipSelectPin, LOW); 164 | //clock out zeros to read in the data from address 165 | data=SPI.transfer16(0x00); 166 | 167 | digitalWrite(chipSelectPin, HIGH); 168 | 169 | if (data & (1<<14)) 170 | { 171 | //if bit 14 is set then we have an error 172 | ERROR("read command 0x%04X failed",addr); 173 | error=true; 174 | return -1; 175 | } 176 | 177 | if (data>>15 != getParity(data)) 178 | { 179 | //parity did not match 180 | ERROR("read command parity error 0x%04X ",addr); 181 | error=true; 182 | return -2; 183 | } 184 | 185 | data=data & 0x3FFF; //mask off the error and parity bits 186 | 187 | return data; 188 | } 189 | 190 | //read the encoders 191 | int16_t AS5047D::readEncoderAngle(void) 192 | { 193 | if (as5047d) 194 | { 195 | return readAddress(AS5047D_CMD_ANGLECOM); 196 | } 197 | return readAddress(AS5048A_CMD_ANGLE); 198 | } 199 | 200 | //pipelined read of the encoder angle used for high speed reads, but value is always one read behind 201 | int16_t AS5047D::readEncoderAnglePipeLineRead(void) 202 | { 203 | 204 | int16_t data; 205 | int error, t0=10; 206 | GPIO_LOW(chipSelectPin);//(chipSelectPin, LOW); 207 | //delayMicroseconds(1); 208 | do { 209 | 210 | // doing two 8 bit transfers is faster than one 16 bit 211 | data =(uint16_t)SPI.transfer(0xFF)<<8 | ((uint16_t)SPI.transfer(0xFF) & 0x0FF); 212 | t0--; 213 | if (t0<=0) 214 | { 215 | ERROR("AS5047D problem"); 216 | break; 217 | } 218 | //data=SPI.transfer16(0xFFFF); //to speed things up we know the parity and address for the read 219 | }while(data & (1<<14)); //while error bit is set 220 | 221 | data=data & 0x3FFF; //mask off the error and parity bits 222 | GPIO_HIGH(chipSelectPin); 223 | //digitalWrite(chipSelectPin, HIGH); 224 | //TODO we really should check for errors and return a negative result or something 225 | return data; 226 | } 227 | 228 | 229 | void AS5047D::diagnostics(char *ptrStr) 230 | { 231 | int16_t data; 232 | int m,d; 233 | 234 | if (as5047d) 235 | { 236 | 237 | data=readAddress(AS5047D_CMD_DIAAGC); 238 | 239 | if (NULL == ptrStr) 240 | { 241 | LOG("DIAAGC: 0x%04X", data); 242 | LOG("MAGL: %d", getBit(data,11)); 243 | LOG("MAGH: %d", getBit(data,10)); 244 | LOG("COF: %d", getBit(data,9)); 245 | LOG("LFGL: %d", getBit(data,8)); 246 | LOG("AGC: %d", data & 0x0FF); 247 | 248 | data=readAddress(AS5047D_CMD_MAG); 249 | LOG("CMAG: 0x%04X(%d)",data,data); 250 | 251 | data=readAddress(AS5047D_CMD_ANGLEUNC); 252 | m=(int)((float)data*AS5047D_DEGREES_PER_BIT); 253 | d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100); 254 | LOG("CORDICANG: 0x%04X(%d) %d.%02d deg(est)",data,data,m,d); 255 | 256 | data=readAddress(AS5047D_CMD_ANGLECOM); 257 | m=(int)((float)data*AS5047D_DEGREES_PER_BIT); 258 | d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100); 259 | LOG("DAECANG: 0x%04X(%d) %d.%02d deg(est)",data,data,m,d); 260 | }else 261 | { 262 | sprintf(ptrStr,"DIAAGC: 0x%04X\n\r", data); 263 | sprintf(ptrStr,"%sMAGL: %d\n\r", ptrStr,getBit(data,11)); 264 | sprintf(ptrStr,"%sMAGH: %d\n\r", ptrStr,getBit(data,10)); 265 | sprintf(ptrStr,"%sCOF: %d\n\r", ptrStr, getBit(data,9)); 266 | sprintf(ptrStr,"%sLFGL: %d\n\r", ptrStr, getBit(data,8)); 267 | sprintf(ptrStr,"%sAGC: %d\n\r", ptrStr,data & 0x0FF); 268 | 269 | data=readAddress(AS5047D_CMD_MAG); 270 | sprintf(ptrStr,"%sCMAG: 0x%04X(%d)\n\r", ptrStr,data,data); 271 | 272 | data=readAddress(AS5047D_CMD_ANGLEUNC); 273 | m=(int)((float)data*AS5047D_DEGREES_PER_BIT); 274 | d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100); 275 | sprintf(ptrStr,"%sCORDICANG: 0x%04X(%d) %d.%02d deg(est)\n\r", ptrStr,data,data,m,d); 276 | 277 | data=readAddress(AS5047D_CMD_ANGLECOM); 278 | m=(int)((float)data*AS5047D_DEGREES_PER_BIT); 279 | d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100); 280 | sprintf(ptrStr,"%sDAECANG: 0x%04X(%d) %d.%02d deg(est)\n\r", ptrStr,data,data,m,d); 281 | 282 | } 283 | } else 284 | { 285 | data=readAddress(AS5048A_CMD_DIAAGC); 286 | sprintf(ptrStr,"AS5048A DIAAGC: 0x%04X\n\r", data); 287 | data=readAddress(AS5048A_CMD_MAG); 288 | sprintf(ptrStr,"%sMagnitude: %d\n\r", ptrStr,data); 289 | data=readAddress(AS5048A_CMD_ANGLE); 290 | sprintf(ptrStr,"%sAngle: %d\n\r", ptrStr,data); 291 | } 292 | 293 | } 294 | 295 | #pragma GCC pop_options 296 | 297 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/syslog.h: -------------------------------------------------------------------------------- 1 | /* 2 | * syslog.h 3 | * 4 | * Created on: Sep 14, 2011 5 | * Author: trampas.stern 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | #ifndef SYSLOG_H_ 17 | #define SYSLOG_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | #include "Arduino.h" 23 | #include "variant.h" 24 | 25 | #ifdef __cplusplus 26 | extern "C" 27 | { 28 | #endif // __cplusplus 29 | 30 | #define SYSLOG_BUFFER_SIZE (250) 31 | 32 | #define MAX_SYSLOG_STRING (250) 33 | #define __FILENAME1__ (__builtin_strrchr(__FILE__, '\\') ? __builtin_strrchr(__FILE__, '\\') + 1 : __FILE__) 34 | #define __FILENAME__ (__builtin_strrchr(__FILENAME1__, '/') ? __builtin_strrchr(__FILENAME1__, '/') + 1 : __FILENAME1__) 35 | 36 | #define SYSLOG_WRITE(buffer,nBytes) 37 | 38 | #ifdef CMD_SERIAL_PORT 39 | #define SYSLOG_PUTC(x) 40 | #else 41 | #define SYSLOG_PUTC(x) //SerialUSB.write(x) 42 | #endif 43 | 44 | #define Sys_WDogHoldOn() 45 | #define Sys_WDogHoldOff() 46 | /* 47 | * priorities/facilities are encoded into a single 32-bit quantity, where the 48 | * bottom 3 bits are the priority (0-7) and the top 28 bits are the facility 49 | * (0-big number). Both the priorities and the facilities map roughly 50 | * one-to-one to strings in the syslogd(8) source code. This mapping is 51 | * included in this file. 52 | * 53 | * priorities (these are ordered) 54 | */ 55 | 56 | typedef enum _eLogLevel { 57 | LOG_EMERG = 0, // system is unusable 58 | LOG_ALERT = 1, // action must be taken immediately 59 | LOG_CRIT = 2, // critical conditions 60 | LOG_ERROR = 3, // error conditions 61 | LOG_WARNING = 4, // warning conditions 62 | LOG_NOTICE = 5, // normal but significant condition 63 | LOG_INFO = 6, // informational 64 | LOG_DEBUG = 7, // debug-level messages 65 | LOG_DISABLED = 8 // disabled messages 66 | } eLogLevel; 67 | 68 | #if 0 69 | #define CONCAT(x, y) CONCAT_(x, y) 70 | #define CONCAT_(x, y) x##y 71 | 72 | #define ID(...) __VA_ARGS__ 73 | 74 | #define IFMULTIARG(if,then,else) \ 75 | CONCAT(IFMULTIARG_, IFMULTIARG_(if, \ 76 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 77 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 78 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 79 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 80 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 81 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, \ 82 | 1, 1, 0, ))(then,else) 83 | #define IFMULTIARG_(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, \ 84 | _10, _11, _12, _13, _14, _15, _16, _17, _18, _19, \ 85 | _20, _21, _22, _23, _24, _25, _26, _27, _28, _29, \ 86 | _30, _31, _32, _33, _34, _35, _36, _37, _38, _39, \ 87 | _40, _41, _42, _43, _44, _45, _46, _47, _48, _49, \ 88 | _50, _51, _52, _53, _54, _55, _56, _57, _58, _59, \ 89 | _60, _61, _62, _63, ...) _63 90 | #define IFMULTIARG_0(then, else) else 91 | #define IFMULTIARG_1(then, else) then 92 | 93 | #define PROVIDE_SECOND_ARGUMENT(x, ...) CONCAT( IFMULTIARG(ID(__VA_ARGS__), INSERT_, ADD_), SECOND_ARGUMENT ) (x, __VA_ARGS__) 94 | #define PROVIDE_SECOND_ARGUMENT2(x, y, ...) CONCAT( IFMULTIARG(ID(__VA_ARGS__), INSERT_, ADD_), SECOND_ARGUMENT2 ) (x, y, __VA_ARGS__) 95 | 96 | #define ADD_SECOND_ARGUMENT(x, y) y, x 97 | #define INSERT_SECOND_ARGUMENT(x, y, ...) y, x, __VA_ARGS__ 98 | 99 | #define ADD_SECOND_ARGUMENT2(x, z, y) y, x, z 100 | #define INSERT_SECOND_ARGUMENT2(x, z, y, ...) y, x, z, __VA_ARGS__ 101 | 102 | #endif 103 | //#define DEBUG1(...) printf( "DEBUG %s %s: " 104 | //PROVIDE_SECOND_ARGUMENT2(__FILE__, __LINE__, __VA_ARGS__)) 105 | 106 | 107 | //TXT(x) macro is used for system which can store strings in flash, like AVR processors 108 | #ifndef TXT 109 | #define TXT(x) x 110 | #endif 111 | 112 | void SysLog(eLogLevel priorty, const char *fmt, ...); 113 | 114 | 115 | 116 | static inline const char * __file__( const char *filename ) { 117 | char const *p = strrchr( filename, '/' ); 118 | if ( p ) 119 | return p+1; 120 | else 121 | return filename; 122 | } // __file__ 123 | 124 | 125 | //These macros abstract the logging and append the file and line number to errors. 126 | #ifndef SYSLOG_DISABLE 127 | //#ifndef PGM_P 128 | #if 1 129 | //EMERG means system is unstable thus will force a reboot! 130 | #define EMERG(fmt, ...) SysLog( LOG_EMERG, "EMERG: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 131 | #define ALERT(fmt, ...) SysLog( LOG_ALERT, "ALERT: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 132 | #define CRITICAL(fmt, ...) SysLog( LOG_CRIT, "CRITICAL: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 133 | #define ERROR(fmt, ...) SysLog( LOG_ERROR, "ERROR: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 134 | #define WARNING(fmt, ...) SysLog( LOG_WARNING, "WARNING: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 135 | #define NOTICE(fmt, ...) SysLog( LOG_NOTICE, "NOTICE: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 136 | #define INFO(fmt, ...) SysLog( LOG_INFO, "INFO: %s %4d: " fmt, __FILENAME__, __LINE__, ## __VA_ARGS__ ) 137 | #define LOG(fmt, ...) SysLog( LOG_DEBUG, "%s %4d: " fmt, __FILENAME__ , __LINE__, ## __VA_ARGS__ ) 138 | // 139 | //#define EMERG(...) SysLog( LOG_EMERG, "EMERG: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 140 | //#define ALERT(...) SysLog( LOG_ALERT, "ALERT: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 141 | //#define CRITICAL(...) SysLog( LOG_CRIT, "CRITICAL: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 142 | //#define ERROR(...) SysLog( LOG_ERROR, "ERROR: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 143 | //#define WARNING(...) SysLog( LOG_WARNING, "WARNING: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 144 | //#define NOTICE(...) SysLog( LOG_NOTICE, "NOTICE: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 145 | //#define INFO(...) SysLog( LOG_INFO, "INFO: %15s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 146 | //#define LOG(...) SysLog( LOG_DEBUG, "%s %4d: " PROVIDE_SECOND_ARGUMENT2(BASE_FILE_NAME, __LINE__,__VA_ARGS__ ) ) 147 | #else 148 | //EMERG means system is unstable thus will force a reboot! 149 | #define EMERG(fmt, ...) SysLog( LOG_EMERG, PSTR("EMERG: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 150 | #define ALERT(fmt, ...) SysLog( LOG_ALERT, PSTR("ALERT: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 151 | #define CRITICAL(fmt, ...) SysLog( LOG_CRIT, PSTR("CRITICAL: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 152 | #define ERROR(fmt, ...) SysLog( LOG_ERROR, PSTR("ERROR: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 153 | #define WARNING(fmt, ...) SysLog( LOG_WARNING, PSTR("WARNING: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 154 | #define NOTICE(fmt, ...) SysLog( LOG_NOTICE, PSTR("NOTICE: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 155 | #define INFO(fmt, ...) SysLog( LOG_INFO, PSTR("INFO: %15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 156 | #define LOG(fmt, ...) SysLog( LOG_DEBUG, PSTR("%15s %4d: " fmt), __file__(__FILE__), __LINE__, ## __VA_ARGS__ ) 157 | 158 | #endif 159 | #else 160 | #define EMERG(fmt, ...) 161 | #define ALERT(fmt, ...) 162 | #define CRITICAL(fmt, ...) 163 | #define ERROR(fmt, ...) 164 | #define WARNING(fmt, ...) 165 | #define NOTICE(fmt, ...) 166 | #define INFO(fmt, ...) 167 | #define LOG(fmt, ...) 168 | 169 | #endif //SYSLOG_DIABLE 170 | 171 | //Note that if you are running debug code with JTAG the assert will stop 172 | // However you might want to run release code with syslog enabled for testing 173 | // where you want error logging, but asserts are not enabled. 174 | // Thus this macro does error logging and an assert. 175 | //This macro assumed to take a constant string as argument 176 | 177 | 178 | //this can be enabled to log asserts to the history file, if you have code space to do it. 179 | #ifdef ASSERT_HISTORY 180 | #define ASSERT(x) {if(!(x)){ERROR(#x); HISTORY_ASSERT();} assert(x);} 181 | #define ASSERT_ERROR(x) {HISTORY_ASSERT(); ERROR(x); ASSERT_FAIL(x);} 182 | #else 183 | #define ASSERT(x) {if(!(x)){ERROR(#x);} assert(x);} 184 | #define ASSERT_ERROR(x) {ERROR(x); ASSERT_FAIL(x);} 185 | #endif 186 | 187 | 188 | void SysLogInit(Uart *ptrSerialObj, eLogLevel LevelToWrite); 189 | int SysLogDisable(void); 190 | int SysLogEnable(void); 191 | int SysLogIsEnabled(void); 192 | 193 | #ifdef __cplusplus 194 | } 195 | #endif // __cplusplus 196 | 197 | #endif /* SYSLOG_H_ */ 198 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/glcdfont.c: -------------------------------------------------------------------------------- 1 | // This is the 'classic' fixed-space bitmap font for Adafruit_GFX since 1.0. 2 | // See gfxfont.h for newer custom bitmap font info. 3 | 4 | #ifndef FONT5X7_H 5 | #define FONT5X7_H 6 | 7 | #ifdef __AVR__ 8 | #include 9 | #include 10 | #elif defined(ESP8266) 11 | #include 12 | #else 13 | #define PROGMEM 14 | #endif 15 | 16 | // Standard ASCII 5x7 font 17 | 18 | static const unsigned char font[] PROGMEM = { 19 | 0x00, 0x00, 0x00, 0x00, 0x00, 20 | 0x3E, 0x5B, 0x4F, 0x5B, 0x3E, 21 | 0x3E, 0x6B, 0x4F, 0x6B, 0x3E, 22 | 0x1C, 0x3E, 0x7C, 0x3E, 0x1C, 23 | 0x18, 0x3C, 0x7E, 0x3C, 0x18, 24 | 0x1C, 0x57, 0x7D, 0x57, 0x1C, 25 | 0x1C, 0x5E, 0x7F, 0x5E, 0x1C, 26 | 0x00, 0x18, 0x3C, 0x18, 0x00, 27 | 0xFF, 0xE7, 0xC3, 0xE7, 0xFF, 28 | 0x00, 0x18, 0x24, 0x18, 0x00, 29 | 0xFF, 0xE7, 0xDB, 0xE7, 0xFF, 30 | 0x30, 0x48, 0x3A, 0x06, 0x0E, 31 | 0x26, 0x29, 0x79, 0x29, 0x26, 32 | 0x40, 0x7F, 0x05, 0x05, 0x07, 33 | 0x40, 0x7F, 0x05, 0x25, 0x3F, 34 | 0x5A, 0x3C, 0xE7, 0x3C, 0x5A, 35 | 0x7F, 0x3E, 0x1C, 0x1C, 0x08, 36 | 0x08, 0x1C, 0x1C, 0x3E, 0x7F, 37 | 0x14, 0x22, 0x7F, 0x22, 0x14, 38 | 0x5F, 0x5F, 0x00, 0x5F, 0x5F, 39 | 0x06, 0x09, 0x7F, 0x01, 0x7F, 40 | 0x00, 0x66, 0x89, 0x95, 0x6A, 41 | 0x60, 0x60, 0x60, 0x60, 0x60, 42 | 0x94, 0xA2, 0xFF, 0xA2, 0x94, 43 | 0x08, 0x04, 0x7E, 0x04, 0x08, 44 | 0x10, 0x20, 0x7E, 0x20, 0x10, 45 | 0x08, 0x08, 0x2A, 0x1C, 0x08, 46 | 0x08, 0x1C, 0x2A, 0x08, 0x08, 47 | 0x1E, 0x10, 0x10, 0x10, 0x10, 48 | 0x0C, 0x1E, 0x0C, 0x1E, 0x0C, 49 | 0x30, 0x38, 0x3E, 0x38, 0x30, 50 | 0x06, 0x0E, 0x3E, 0x0E, 0x06, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0x00, 0x00, 0x5F, 0x00, 0x00, 53 | 0x00, 0x07, 0x00, 0x07, 0x00, 54 | 0x14, 0x7F, 0x14, 0x7F, 0x14, 55 | 0x24, 0x2A, 0x7F, 0x2A, 0x12, 56 | 0x23, 0x13, 0x08, 0x64, 0x62, 57 | 0x36, 0x49, 0x56, 0x20, 0x50, 58 | 0x00, 0x08, 0x07, 0x03, 0x00, 59 | 0x00, 0x1C, 0x22, 0x41, 0x00, 60 | 0x00, 0x41, 0x22, 0x1C, 0x00, 61 | 0x2A, 0x1C, 0x7F, 0x1C, 0x2A, 62 | 0x08, 0x08, 0x3E, 0x08, 0x08, 63 | 0x00, 0x80, 0x70, 0x30, 0x00, 64 | 0x08, 0x08, 0x08, 0x08, 0x08, 65 | 0x00, 0x00, 0x60, 0x60, 0x00, 66 | 0x20, 0x10, 0x08, 0x04, 0x02, 67 | 0x3E, 0x51, 0x49, 0x45, 0x3E, 68 | 0x00, 0x42, 0x7F, 0x40, 0x00, 69 | 0x72, 0x49, 0x49, 0x49, 0x46, 70 | 0x21, 0x41, 0x49, 0x4D, 0x33, 71 | 0x18, 0x14, 0x12, 0x7F, 0x10, 72 | 0x27, 0x45, 0x45, 0x45, 0x39, 73 | 0x3C, 0x4A, 0x49, 0x49, 0x31, 74 | 0x41, 0x21, 0x11, 0x09, 0x07, 75 | 0x36, 0x49, 0x49, 0x49, 0x36, 76 | 0x46, 0x49, 0x49, 0x29, 0x1E, 77 | 0x00, 0x00, 0x14, 0x00, 0x00, 78 | 0x00, 0x40, 0x34, 0x00, 0x00, 79 | 0x00, 0x08, 0x14, 0x22, 0x41, 80 | 0x14, 0x14, 0x14, 0x14, 0x14, 81 | 0x00, 0x41, 0x22, 0x14, 0x08, 82 | 0x02, 0x01, 0x59, 0x09, 0x06, 83 | 0x3E, 0x41, 0x5D, 0x59, 0x4E, 84 | 0x7C, 0x12, 0x11, 0x12, 0x7C, 85 | 0x7F, 0x49, 0x49, 0x49, 0x36, 86 | 0x3E, 0x41, 0x41, 0x41, 0x22, 87 | 0x7F, 0x41, 0x41, 0x41, 0x3E, 88 | 0x7F, 0x49, 0x49, 0x49, 0x41, 89 | 0x7F, 0x09, 0x09, 0x09, 0x01, 90 | 0x3E, 0x41, 0x41, 0x51, 0x73, 91 | 0x7F, 0x08, 0x08, 0x08, 0x7F, 92 | 0x00, 0x41, 0x7F, 0x41, 0x00, 93 | 0x20, 0x40, 0x41, 0x3F, 0x01, 94 | 0x7F, 0x08, 0x14, 0x22, 0x41, 95 | 0x7F, 0x40, 0x40, 0x40, 0x40, 96 | 0x7F, 0x02, 0x1C, 0x02, 0x7F, 97 | 0x7F, 0x04, 0x08, 0x10, 0x7F, 98 | 0x3E, 0x41, 0x41, 0x41, 0x3E, 99 | 0x7F, 0x09, 0x09, 0x09, 0x06, 100 | 0x3E, 0x41, 0x51, 0x21, 0x5E, 101 | 0x7F, 0x09, 0x19, 0x29, 0x46, 102 | 0x26, 0x49, 0x49, 0x49, 0x32, 103 | 0x03, 0x01, 0x7F, 0x01, 0x03, 104 | 0x3F, 0x40, 0x40, 0x40, 0x3F, 105 | 0x1F, 0x20, 0x40, 0x20, 0x1F, 106 | 0x3F, 0x40, 0x38, 0x40, 0x3F, 107 | 0x63, 0x14, 0x08, 0x14, 0x63, 108 | 0x03, 0x04, 0x78, 0x04, 0x03, 109 | 0x61, 0x59, 0x49, 0x4D, 0x43, 110 | 0x00, 0x7F, 0x41, 0x41, 0x41, 111 | 0x02, 0x04, 0x08, 0x10, 0x20, 112 | 0x00, 0x41, 0x41, 0x41, 0x7F, 113 | 0x04, 0x02, 0x01, 0x02, 0x04, 114 | 0x40, 0x40, 0x40, 0x40, 0x40, 115 | 0x00, 0x03, 0x07, 0x08, 0x00, 116 | 0x20, 0x54, 0x54, 0x78, 0x40, 117 | 0x7F, 0x28, 0x44, 0x44, 0x38, 118 | 0x38, 0x44, 0x44, 0x44, 0x28, 119 | 0x38, 0x44, 0x44, 0x28, 0x7F, 120 | 0x38, 0x54, 0x54, 0x54, 0x18, 121 | 0x00, 0x08, 0x7E, 0x09, 0x02, 122 | 0x18, 0xA4, 0xA4, 0x9C, 0x78, 123 | 0x7F, 0x08, 0x04, 0x04, 0x78, 124 | 0x00, 0x44, 0x7D, 0x40, 0x00, 125 | 0x20, 0x40, 0x40, 0x3D, 0x00, 126 | 0x7F, 0x10, 0x28, 0x44, 0x00, 127 | 0x00, 0x41, 0x7F, 0x40, 0x00, 128 | 0x7C, 0x04, 0x78, 0x04, 0x78, 129 | 0x7C, 0x08, 0x04, 0x04, 0x78, 130 | 0x38, 0x44, 0x44, 0x44, 0x38, 131 | 0xFC, 0x18, 0x24, 0x24, 0x18, 132 | 0x18, 0x24, 0x24, 0x18, 0xFC, 133 | 0x7C, 0x08, 0x04, 0x04, 0x08, 134 | 0x48, 0x54, 0x54, 0x54, 0x24, 135 | 0x04, 0x04, 0x3F, 0x44, 0x24, 136 | 0x3C, 0x40, 0x40, 0x20, 0x7C, 137 | 0x1C, 0x20, 0x40, 0x20, 0x1C, 138 | 0x3C, 0x40, 0x30, 0x40, 0x3C, 139 | 0x44, 0x28, 0x10, 0x28, 0x44, 140 | 0x4C, 0x90, 0x90, 0x90, 0x7C, 141 | 0x44, 0x64, 0x54, 0x4C, 0x44, 142 | 0x00, 0x08, 0x36, 0x41, 0x00, 143 | 0x00, 0x00, 0x77, 0x00, 0x00, 144 | 0x00, 0x41, 0x36, 0x08, 0x00, 145 | 0x02, 0x01, 0x02, 0x04, 0x02, 146 | 0x3C, 0x26, 0x23, 0x26, 0x3C, 147 | 0x1E, 0xA1, 0xA1, 0x61, 0x12, 148 | 0x3A, 0x40, 0x40, 0x20, 0x7A, 149 | 0x38, 0x54, 0x54, 0x55, 0x59, 150 | 0x21, 0x55, 0x55, 0x79, 0x41, 151 | 0x22, 0x54, 0x54, 0x78, 0x42, // a-umlaut 152 | 0x21, 0x55, 0x54, 0x78, 0x40, 153 | 0x20, 0x54, 0x55, 0x79, 0x40, 154 | 0x0C, 0x1E, 0x52, 0x72, 0x12, 155 | 0x39, 0x55, 0x55, 0x55, 0x59, 156 | 0x39, 0x54, 0x54, 0x54, 0x59, 157 | 0x39, 0x55, 0x54, 0x54, 0x58, 158 | 0x00, 0x00, 0x45, 0x7C, 0x41, 159 | 0x00, 0x02, 0x45, 0x7D, 0x42, 160 | 0x00, 0x01, 0x45, 0x7C, 0x40, 161 | 0x7D, 0x12, 0x11, 0x12, 0x7D, // A-umlaut 162 | 0xF0, 0x28, 0x25, 0x28, 0xF0, 163 | 0x7C, 0x54, 0x55, 0x45, 0x00, 164 | 0x20, 0x54, 0x54, 0x7C, 0x54, 165 | 0x7C, 0x0A, 0x09, 0x7F, 0x49, 166 | 0x32, 0x49, 0x49, 0x49, 0x32, 167 | 0x3A, 0x44, 0x44, 0x44, 0x3A, // o-umlaut 168 | 0x32, 0x4A, 0x48, 0x48, 0x30, 169 | 0x3A, 0x41, 0x41, 0x21, 0x7A, 170 | 0x3A, 0x42, 0x40, 0x20, 0x78, 171 | 0x00, 0x9D, 0xA0, 0xA0, 0x7D, 172 | 0x3D, 0x42, 0x42, 0x42, 0x3D, // O-umlaut 173 | 0x3D, 0x40, 0x40, 0x40, 0x3D, 174 | 0x3C, 0x24, 0xFF, 0x24, 0x24, 175 | 0x48, 0x7E, 0x49, 0x43, 0x66, 176 | 0x2B, 0x2F, 0xFC, 0x2F, 0x2B, 177 | 0xFF, 0x09, 0x29, 0xF6, 0x20, 178 | 0xC0, 0x88, 0x7E, 0x09, 0x03, 179 | 0x20, 0x54, 0x54, 0x79, 0x41, 180 | 0x00, 0x00, 0x44, 0x7D, 0x41, 181 | 0x30, 0x48, 0x48, 0x4A, 0x32, 182 | 0x38, 0x40, 0x40, 0x22, 0x7A, 183 | 0x00, 0x7A, 0x0A, 0x0A, 0x72, 184 | 0x7D, 0x0D, 0x19, 0x31, 0x7D, 185 | 0x26, 0x29, 0x29, 0x2F, 0x28, 186 | 0x26, 0x29, 0x29, 0x29, 0x26, 187 | 0x30, 0x48, 0x4D, 0x40, 0x20, 188 | 0x38, 0x08, 0x08, 0x08, 0x08, 189 | 0x08, 0x08, 0x08, 0x08, 0x38, 190 | 0x2F, 0x10, 0xC8, 0xAC, 0xBA, 191 | 0x2F, 0x10, 0x28, 0x34, 0xFA, 192 | 0x00, 0x00, 0x7B, 0x00, 0x00, 193 | 0x08, 0x14, 0x2A, 0x14, 0x22, 194 | 0x22, 0x14, 0x2A, 0x14, 0x08, 195 | 0x55, 0x00, 0x55, 0x00, 0x55, // #176 (25% block) missing in old code 196 | 0xAA, 0x55, 0xAA, 0x55, 0xAA, // 50% block 197 | 0xFF, 0x55, 0xFF, 0x55, 0xFF, // 75% block 198 | 0x00, 0x00, 0x00, 0xFF, 0x00, 199 | 0x10, 0x10, 0x10, 0xFF, 0x00, 200 | 0x14, 0x14, 0x14, 0xFF, 0x00, 201 | 0x10, 0x10, 0xFF, 0x00, 0xFF, 202 | 0x10, 0x10, 0xF0, 0x10, 0xF0, 203 | 0x14, 0x14, 0x14, 0xFC, 0x00, 204 | 0x14, 0x14, 0xF7, 0x00, 0xFF, 205 | 0x00, 0x00, 0xFF, 0x00, 0xFF, 206 | 0x14, 0x14, 0xF4, 0x04, 0xFC, 207 | 0x14, 0x14, 0x17, 0x10, 0x1F, 208 | 0x10, 0x10, 0x1F, 0x10, 0x1F, 209 | 0x14, 0x14, 0x14, 0x1F, 0x00, 210 | 0x10, 0x10, 0x10, 0xF0, 0x00, 211 | 0x00, 0x00, 0x00, 0x1F, 0x10, 212 | 0x10, 0x10, 0x10, 0x1F, 0x10, 213 | 0x10, 0x10, 0x10, 0xF0, 0x10, 214 | 0x00, 0x00, 0x00, 0xFF, 0x10, 215 | 0x10, 0x10, 0x10, 0x10, 0x10, 216 | 0x10, 0x10, 0x10, 0xFF, 0x10, 217 | 0x00, 0x00, 0x00, 0xFF, 0x14, 218 | 0x00, 0x00, 0xFF, 0x00, 0xFF, 219 | 0x00, 0x00, 0x1F, 0x10, 0x17, 220 | 0x00, 0x00, 0xFC, 0x04, 0xF4, 221 | 0x14, 0x14, 0x17, 0x10, 0x17, 222 | 0x14, 0x14, 0xF4, 0x04, 0xF4, 223 | 0x00, 0x00, 0xFF, 0x00, 0xF7, 224 | 0x14, 0x14, 0x14, 0x14, 0x14, 225 | 0x14, 0x14, 0xF7, 0x00, 0xF7, 226 | 0x14, 0x14, 0x14, 0x17, 0x14, 227 | 0x10, 0x10, 0x1F, 0x10, 0x1F, 228 | 0x14, 0x14, 0x14, 0xF4, 0x14, 229 | 0x10, 0x10, 0xF0, 0x10, 0xF0, 230 | 0x00, 0x00, 0x1F, 0x10, 0x1F, 231 | 0x00, 0x00, 0x00, 0x1F, 0x14, 232 | 0x00, 0x00, 0x00, 0xFC, 0x14, 233 | 0x00, 0x00, 0xF0, 0x10, 0xF0, 234 | 0x10, 0x10, 0xFF, 0x10, 0xFF, 235 | 0x14, 0x14, 0x14, 0xFF, 0x14, 236 | 0x10, 0x10, 0x10, 0x1F, 0x00, 237 | 0x00, 0x00, 0x00, 0xF0, 0x10, 238 | 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 239 | 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 240 | 0xFF, 0xFF, 0xFF, 0x00, 0x00, 241 | 0x00, 0x00, 0x00, 0xFF, 0xFF, 242 | 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 243 | 0x38, 0x44, 0x44, 0x38, 0x44, 244 | 0xFC, 0x4A, 0x4A, 0x4A, 0x34, // sharp-s or beta 245 | 0x7E, 0x02, 0x02, 0x06, 0x06, 246 | 0x02, 0x7E, 0x02, 0x7E, 0x02, 247 | 0x63, 0x55, 0x49, 0x41, 0x63, 248 | 0x38, 0x44, 0x44, 0x3C, 0x04, 249 | 0x40, 0x7E, 0x20, 0x1E, 0x20, 250 | 0x06, 0x02, 0x7E, 0x02, 0x02, 251 | 0x99, 0xA5, 0xE7, 0xA5, 0x99, 252 | 0x1C, 0x2A, 0x49, 0x2A, 0x1C, 253 | 0x4C, 0x72, 0x01, 0x72, 0x4C, 254 | 0x30, 0x4A, 0x4D, 0x4D, 0x30, 255 | 0x30, 0x48, 0x78, 0x48, 0x30, 256 | 0xBC, 0x62, 0x5A, 0x46, 0x3D, 257 | 0x3E, 0x49, 0x49, 0x49, 0x00, 258 | 0x7E, 0x01, 0x01, 0x01, 0x7E, 259 | 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, 260 | 0x44, 0x44, 0x5F, 0x44, 0x44, 261 | 0x40, 0x51, 0x4A, 0x44, 0x40, 262 | 0x40, 0x44, 0x4A, 0x51, 0x40, 263 | 0x00, 0x00, 0xFF, 0x01, 0x03, 264 | 0xE0, 0x80, 0xFF, 0x00, 0x00, 265 | 0x08, 0x08, 0x6B, 0x6B, 0x08, 266 | 0x36, 0x12, 0x36, 0x24, 0x36, 267 | 0x06, 0x0F, 0x09, 0x0F, 0x06, 268 | 0x00, 0x00, 0x18, 0x18, 0x00, 269 | 0x00, 0x00, 0x10, 0x10, 0x00, 270 | 0x30, 0x40, 0xFF, 0x01, 0x01, 271 | 0x00, 0x1F, 0x01, 0x01, 0x1E, 272 | 0x00, 0x19, 0x1D, 0x17, 0x12, 273 | 0x00, 0x3C, 0x3C, 0x3C, 0x3C, 274 | 0x00, 0x00, 0x00, 0x00, 0x00 // #255 NBSP 275 | }; 276 | #endif // FONT5X7_H 277 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/command.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) Trampas Stern name of author 3 | 4 | This program is free software; you can redistribute it and/or 5 | modify it under the terms of the GNU General Public License 6 | as published by the Free Software Foundation; either version 2 7 | of the License. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program; if not, write to the Free Software 16 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 17 | */ 18 | 19 | #include "command.h" 20 | #include 21 | 22 | 23 | #define ASCII_BACKSPACE 0x08 24 | #define ASCII_ESC 0x1B 25 | #define ASCII_UP_ARROW 0x9b 26 | //const char CMD_ANSI_UP[]= {ASCII_ESC,'[','A',0}; 27 | 28 | int strcicmp(char const *a, char const *b) 29 | { 30 | for (;; a++, b++) { 31 | int d = tolower(*a) - tolower(*b); 32 | if (d != 0 || !*a) 33 | return d; 34 | } 35 | } 36 | 37 | int CommandInit(sCmdUart *ptrUart, uint8_t (*kbhit)(void), uint8_t (*getch)(void),uint8_t (*putch)(char data),uint8_t (*puts)(uint8_t *buffer, uint8_t size) ) 38 | { 39 | ptrUart->kbhit=kbhit; 40 | ptrUart->getch=getch; 41 | ptrUart->putch=putch; 42 | ptrUart->puts=puts; 43 | ptrUart->histIndex=0; 44 | ptrUart->buffIndex=0; 45 | return 0; 46 | } 47 | 48 | #ifdef PGM_P //check and see if the PGM_P is defined for the AVR 49 | 50 | int CommandPrintf(sCmdUart *ptrUart, const char *fmt, ...) 51 | { 52 | int ret=0; 53 | char vastr[MAX_STRING]={0}; 54 | //char str[MAX_STRING]={0}; 55 | char *ptr; 56 | va_list ap; 57 | 58 | //LOG("Command printf"); 59 | memset(vastr,0,MAX_STRING); 60 | va_start(ap,fmt); 61 | ret=vsprintf(vastr,(const char *)fmt,ap); 62 | //ret=sprintf(vastr,"%s\r\n",str); 63 | //LOG("%s",vastr); 64 | if (ptrUart->puts!=NULL) 65 | { 66 | return ptrUart->puts((uint8_t *)vastr, (uint8_t)ret); 67 | } 68 | 69 | if (ptrUart->putch!=NULL) 70 | { 71 | ptr=vastr; 72 | while(*ptr) 73 | { 74 | ptrUart->putch(*ptr++); 75 | } 76 | 77 | return ret; 78 | } 79 | return 0; 80 | } 81 | 82 | 83 | #else 84 | int CommandPrintf(sCmdUart *ptrUart, char *fmt, ...) 85 | { 86 | int ret=0; 87 | char vastr[MAX_STRING]={0}; 88 | char *ptr; 89 | va_list ap; 90 | 91 | 92 | memset(vastr,0,MAX_STRING); 93 | va_start(ap,fmt); 94 | ret=vsprintf(vastr,(char *)fmt,ap); 95 | if (ptrUart->puts!=NULL) 96 | { 97 | return ptrUart->puts((uint8_t *)vastr, (uint8_t)ret); 98 | } 99 | 100 | if (ptrUart->putch!=NULL) 101 | { 102 | ptr=vastr; 103 | while(*ptr) 104 | { 105 | ptrUart->putch(*ptr++); 106 | } 107 | 108 | return ret; 109 | } 110 | return 0; 111 | } 112 | #endif 113 | 114 | 115 | // the delimiter is command/parameter delimiter 116 | // by default a ' '0x20 is used but for the TDR with GUI a ':' was preferred, not sure why 117 | // set to ' '/0x20 if you want normal command parsing, like DOS 118 | unsigned int CommandParse(sCmdUart *ptrUart,sCommand *ptrCmds, char *str, char delimitor ) 119 | { 120 | char *ptr; 121 | char *ptr2; 122 | unsigned int i; 123 | //char cmd[MAX_STRING]; 124 | char buff[MAX_CMD_LENGTH]; 125 | char argv[MAX_ARGS][MAX_ARG_LENGTH]; 126 | char *ptrArgv[MAX_ARGS]; 127 | unsigned int numArgs; 128 | int emptyArg=0; 129 | 130 | sCommand cmd_list; 131 | 132 | 133 | while (*str==0x20 || *str=='\n' || *str=='\r' || *str=='\t') str++; 134 | //first we need find command and arguments 135 | ptr=strchr(str,delimitor); //find first char 136 | 137 | //LOG("2parsing %s",str); 138 | 139 | 140 | if (ptr==0) 141 | { 142 | //we have two options, frist whole thing is command 143 | //second bad command 144 | if(strlen(str)>0) 145 | ptr=str+strlen(str); 146 | else 147 | return 0; //bad command 148 | } 149 | 150 | //copy string to command buffer. 151 | i=0; 152 | ptr2=str; 153 | while(ptr!=0 && ptr!=ptr2 && i<(MAX_CMD_LENGTH-1)) 154 | { 155 | //if (*ptr2!='\n' && *ptr2!='\r') //do not include newlines 156 | { 157 | buff[i++]=*ptr2; 158 | } 159 | ptr2++; 160 | } 161 | buff[i]=0; 162 | 163 | //now buff contains the command let's get the args 164 | numArgs=0; 165 | while(*ptr!=0 && (*ptr==' ' || *ptr==delimitor)) 166 | ptr++; //increment pointer past ' ' 167 | if (*ptr!=0) 168 | { 169 | if (*ptr==34) // " char 170 | { 171 | ptr++; 172 | ptr2=strchr(ptr,34); //find match 173 | } else if (*ptr==39) // 'char 174 | { 175 | ptr++; 176 | ptr2=strchr(ptr,39); //find match 177 | } else 178 | { 179 | ptr2=strchr(ptr,delimitor); 180 | } 181 | if (ptr2==0) 182 | { 183 | //we have two options, frist whole thing is command 184 | //second bad command 185 | //LOG("strlen ptr is %d",strlen(ptr)); 186 | if(strlen(ptr)>0) 187 | ptr2=ptr+strlen(ptr); 188 | } 189 | emptyArg=0; 190 | while((ptr2!=0 && numArgs0) 230 | ptr2=ptr+strlen(ptr); 231 | } 232 | } 233 | } 234 | } 235 | 236 | for(i=0; ikbhit()) 283 | { 284 | ptrUart->data=ptrUart->getch(); 285 | 286 | //echo the data 287 | ptrUart->putch(ptrUart->data); 288 | 289 | //if the data is the CR we need to process buffer 290 | if (ptrUart->data==0x0D) 291 | { 292 | ptrUart->putch(0x0A); 293 | if (strlen(ptrUart->buffer)>0) 294 | { 295 | if (ptrUart->lastChar!=ASCII_UP_ARROW) 296 | { 297 | strcpy(ptrUart->bufferHist[ptrUart->histIndex],ptrUart->buffer); 298 | ptrUart->histIndex=(ptrUart->histIndex+1) % CMD_HISTORY; 299 | } 300 | CommandParse(ptrUart,ptrCmds,ptrUart->buffer,delimitor); 301 | } 302 | 303 | CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt); 304 | ptrUart->buffIndex=0; 305 | ptrUart->buffer[ptrUart->buffIndex]=0; 306 | } 307 | 308 | if (ptrUart->data==ASCII_BACKSPACE) //backspace 309 | { 310 | if (ptrUart->buffIndex>0) 311 | { 312 | ptrUart->buffIndex--; 313 | ptrUart->buffer[ptrUart->buffIndex]='\0'; 314 | //Echo the backspace 315 | ptrUart->putch(' '); 316 | ptrUart->putch(ASCII_BACKSPACE); 317 | } 318 | }else if (ptrUart->data != 0x0A && ptrUart->data !=0x0D && ptrUart->data<127) 319 | { 320 | ptrUart->buffer[ptrUart->buffIndex++]=ptrUart->data; 321 | ptrUart->buffer[ptrUart->buffIndex]=0; 322 | } 323 | if (ptrUart->buffIndex>=(MAX_CMD_LENGTH-1)) 324 | { 325 | CommandPrintf(ptrUart,PSTR("\n\rERROR: Command buffer overflow\n\r"));\ 326 | ERROR("Command buffer overflow"); 327 | ptrUart->buffIndex=0; 328 | ptrUart->buffer[0]=0; 329 | CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt); 330 | } 331 | } 332 | 333 | 334 | if (strstr(ptrUart->buffer,ANSI_UP)) //up arrow 335 | { 336 | uint8_t i; 337 | 338 | CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt); 339 | i=CMD_HISTORY-1; 340 | if (ptrUart->histIndex>0) 341 | { 342 | i=ptrUart->histIndex-1; 343 | } 344 | if (strlen(ptrUart->bufferHist[i])>0) 345 | { 346 | strcpy(ptrUart->buffer,ptrUart->bufferHist[i]); 347 | ptrUart->buffIndex=strlen(ptrUart->buffer); 348 | CommandPrintf(ptrUart,PSTR("%s"),ptrUart->buffer); 349 | }else 350 | { 351 | ptrUart->buffIndex=0; 352 | ptrUart->buffer[0]=0; 353 | } 354 | ptrUart->data=ASCII_UP_ARROW; 355 | } 356 | 357 | 358 | ptrUart->lastChar=ptrUart->data; 359 | return 0; 360 | } 361 | 362 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/sine.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * since.cpp 3 | * 4 | * Created on: Dec 24, 2016 5 | * Author: tstern 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | #include "sine.h" 17 | 18 | #ifndef MKS_GCC_O0 //mks mark ("-O0") 19 | #pragma GCC push_options 20 | #pragma GCC optimize ("-Ofast") 21 | #endif 22 | 23 | #ifdef NZS_FAST_SINE 24 | static const int16_t sineTable[1280]={ 25 | 0,201,402,603,804,1005,1206,1407,1608,1808,2009,2210,2410,2611,2811,3011, 26 | 3212,3412,3611,3811,4011,4210,4410,4609,4808,5007,5205,5404,5602,5800,5998,6195, 27 | 6392,6589,6786,6983,7179,7375,7571,7766,7962,8156,8351,8545,8739,8933,9126,9319, 28 | 9512,9704,9896,10087,10278,10469,10659,10849,11039,11228,11417,11605,11793,11980,12167,12353, 29 | 12539,12725,12910,13094,13278,13462,13645,13828,14010,14191,14372,14553,14732,14912,15090,15269, 30 | 15446,15623,15800,15976,16151,16325,16499,16673,16846,17018,17189,17360,17530,17700,17869,18037, 31 | 18204,18371,18537,18703,18868,19032,19195,19357,19519,19680,19841,20000,20159,20317,20475,20631, 32 | 20787,20942,21097,21250,21403,21554,21705,21856,22005,22154,22301,22448,22594,22739,22884,23027, 33 | 23170,23312,23452,23592,23732,23870,24007,24143,24279,24413,24547,24680,24812,24942,25072,25201, 34 | 25329,25456,25583,25708,25832,25955,26077,26199,26319,26438,26556,26674,26790,26905,27019,27133, 35 | 27245,27356,27466,27575,27683,27791,27897,28001,28105,28208,28310,28411,28510,28609,28706,28803, 36 | 28898,28992,29085,29178,29268,29358,29447,29535,29621,29707,29791,29874,29956,30037,30117,30195, 37 | 30273,30349,30425,30499,30572,30643,30714,30783,30852,30919,30985,31050,31113,31176,31237,31297, 38 | 31356,31414,31471,31526,31580,31633,31685,31736,31785,31834,31881,31926,31971,32015,32057,32098, 39 | 32138,32176,32214,32250,32285,32319,32351,32382,32413,32441,32469,32496,32521,32545,32568,32589, 40 | 32609,32629,32646,32663,32678,32693,32706,32717,32728,32737,32745,32752,32757,32762,32765,32767, 41 | 32767,32767,32765,32762,32757,32752,32745,32737,32728,32717,32706,32693,32678,32663,32646,32629, 42 | 32609,32589,32568,32545,32521,32496,32469,32441,32413,32382,32351,32319,32285,32250,32214,32176, 43 | 32138,32098,32057,32015,31971,31926,31881,31834,31785,31736,31685,31633,31580,31526,31471,31414, 44 | 31356,31297,31237,31176,31113,31050,30985,30919,30852,30783,30714,30643,30572,30499,30425,30349, 45 | 30273,30195,30117,30037,29956,29874,29791,29707,29621,29535,29447,29358,29268,29178,29085,28992, 46 | 28898,28803,28706,28609,28510,28411,28310,28208,28105,28001,27897,27791,27683,27575,27466,27356, 47 | 27245,27133,27019,26905,26790,26674,26556,26438,26319,26199,26077,25955,25832,25708,25583,25456, 48 | 25329,25201,25072,24942,24812,24680,24547,24413,24279,24143,24007,23870,23732,23592,23452,23312, 49 | 23170,23027,22884,22739,22594,22448,22301,22154,22005,21856,21705,21554,21403,21250,21097,20942, 50 | 20787,20631,20475,20317,20159,20000,19841,19680,19519,19357,19195,19032,18868,18703,18537,18371, 51 | 18204,18037,17869,17700,17530,17360,17189,17018,16846,16673,16499,16325,16151,15976,15800,15623, 52 | 15446,15269,15090,14912,14732,14553,14372,14191,14010,13828,13645,13462,13278,13094,12910,12725, 53 | 12539,12353,12167,11980,11793,11605,11417,11228,11039,10849,10659,10469,10278,10087,9896,9704, 54 | 9512,9319,9126,8933,8739,8545,8351,8156,7962,7766,7571,7375,7179,6983,6786,6589, 55 | 6392,6195,5998,5800,5602,5404,5205,5007,4808,4609,4410,4210,4011,3811,3611,3412, 56 | 3212,3011,2811,2611,2410,2210,2009,1808,1608,1407,1206,1005,804,603,402,201, 57 | 0,-201,-402,-603,-804,-1005,-1206,-1407,-1608,-1809,-2010,-2210,-2411,-2611,-2812,-3012, 58 | -3212,-3412,-3612,-3812,-4011,-4211,-4410,-4609,-4808,-5007,-5206,-5404,-5602,-5800,-5998,-6196, 59 | -6393,-6590,-6787,-6983,-7180,-7376,-7571,-7767,-7962,-8157,-8352,-8546,-8740,-8933,-9127,-9320, 60 | -9512,-9704,-9896,-10088,-10279,-10470,-10660,-10850,-11039,-11228,-11417,-11605,-11793,-11980,-12167,-12354, 61 | -12540,-12725,-12910,-13095,-13279,-13463,-13646,-13828,-14010,-14192,-14373,-14553,-14733,-14912,-15091,-15269, 62 | -15447,-15624,-15800,-15976,-16151,-16326,-16500,-16673,-16846,-17018,-17190,-17361,-17531,-17700,-17869,-18037, 63 | -18205,-18372,-18538,-18703,-18868,-19032,-19195,-19358,-19520,-19681,-19841,-20001,-20160,-20318,-20475,-20632, 64 | -20788,-20943,-21097,-21250,-21403,-21555,-21706,-21856,-22006,-22154,-22302,-22449,-22595,-22740,-22884,-23028, 65 | -23170,-23312,-23453,-23593,-23732,-23870,-24007,-24144,-24279,-24414,-24548,-24680,-24812,-24943,-25073,-25202, 66 | -25330,-25457,-25583,-25708,-25832,-25956,-26078,-26199,-26319,-26439,-26557,-26674,-26790,-26906,-27020,-27133, 67 | -27245,-27357,-27467,-27576,-27684,-27791,-27897,-28002,-28106,-28209,-28310,-28411,-28511,-28609,-28707,-28803, 68 | -28899,-28993,-29086,-29178,-29269,-29359,-29448,-29535,-29622,-29707,-29791,-29875,-29957,-30038,-30117,-30196, 69 | -30273,-30350,-30425,-30499,-30572,-30644,-30715,-30784,-30852,-30919,-30985,-31050,-31114,-31176,-31238,-31298, 70 | -31357,-31415,-31471,-31527,-31581,-31634,-31686,-31736,-31786,-31834,-31881,-31927,-31972,-32015,-32057,-32098, 71 | -32138,-32177,-32214,-32250,-32285,-32319,-32352,-32383,-32413,-32442,-32470,-32496,-32521,-32545,-32568,-32590, 72 | -32610,-32629,-32647,-32664,-32679,-32693,-32706,-32718,-32728,-32738,-32746,-32752,-32758,-32762,-32765,-32767, 73 | -32768,-32767,-32765,-32762,-32758,-32752,-32746,-32738,-32728,-32718,-32706,-32693,-32679,-32664,-32647,-32629, 74 | -32610,-32590,-32568,-32545,-32521,-32496,-32470,-32442,-32413,-32383,-32352,-32319,-32285,-32250,-32214,-32177, 75 | -32138,-32098,-32057,-32015,-31972,-31927,-31881,-31834,-31786,-31736,-31686,-31634,-31581,-31527,-31471,-31415, 76 | -31357,-31298,-31238,-31176,-31114,-31050,-30985,-30919,-30852,-30784,-30715,-30644,-30572,-30499,-30425,-30350, 77 | -30273,-30196,-30117,-30038,-29957,-29875,-29791,-29707,-29622,-29535,-29448,-29359,-29269,-29178,-29086,-28993, 78 | -28899,-28803,-28707,-28609,-28511,-28411,-28310,-28209,-28106,-28002,-27897,-27791,-27684,-27576,-27467,-27357, 79 | -27245,-27133,-27020,-26906,-26790,-26674,-26557,-26439,-26319,-26199,-26078,-25956,-25832,-25708,-25583,-25457, 80 | -25330,-25202,-25073,-24943,-24812,-24680,-24548,-24414,-24279,-24144,-24007,-23870,-23732,-23593,-23453,-23312, 81 | -23170,-23028,-22884,-22740,-22595,-22449,-22302,-22154,-22006,-21856,-21706,-21555,-21403,-21250,-21097,-20943, 82 | -20788,-20632,-20475,-20318,-20160,-20001,-19841,-19681,-19520,-19358,-19195,-19032,-18868,-18703,-18538,-18372, 83 | -18205,-18037,-17869,-17700,-17531,-17361,-17190,-17018,-16846,-16673,-16500,-16326,-16151,-15976,-15800,-15624, 84 | -15447,-15269,-15091,-14912,-14733,-14553,-14373,-14192,-14010,-13828,-13646,-13463,-13279,-13095,-12910,-12725, 85 | -12540,-12354,-12167,-11980,-11793,-11605,-11417,-11228,-11039,-10850,-10660,-10470,-10279,-10088,-9896,-9704, 86 | -9512,-9320,-9127,-8933,-8740,-8546,-8352,-8157,-7962,-7767,-7571,-7376,-7180,-6983,-6787,-6590, 87 | -6393,-6196,-5998,-5800,-5602,-5404,-5206,-5007,-4808,-4609,-4410,-4211,-4011,-3812,-3612,-3412, 88 | -3212,-3012,-2812,-2611,-2411,-2210,-2010,-1809,-1608,-1407,-1206,-1005,-804,-603,-402,-201, 89 | 0,201,402,603,804,1005,1206,1407,1608,1808,2009,2210,2410,2611,2811,3011, 90 | 3212,3412,3611,3811,4011,4210,4410,4609,4808,5007,5205,5404,5602,5800,5998,6195, 91 | 6392,6589,6786,6983,7179,7375,7571,7766,7962,8156,8351,8545,8739,8933,9126,9319, 92 | 9512,9704,9896,10087,10278,10469,10659,10849,11039,11228,11417,11605,11793,11980,12167,12353, 93 | 12539,12725,12910,13094,13278,13462,13645,13828,14010,14191,14372,14553,14732,14912,15090,15269, 94 | 15446,15623,15800,15976,16151,16325,16499,16673,16846,17018,17189,17360,17530,17700,17869,18037, 95 | 18204,18371,18537,18703,18868,19032,19195,19357,19519,19680,19841,20000,20159,20317,20475,20631, 96 | 20787,20942,21097,21250,21403,21554,21705,21856,22005,22154,22301,22448,22594,22739,22884,23027, 97 | 23170,23312,23452,23592,23732,23870,24007,24143,24279,24413,24547,24680,24812,24942,25072,25201, 98 | 25329,25456,25583,25708,25832,25955,26077,26199,26319,26438,26556,26674,26790,26905,27019,27133, 99 | 27245,27356,27466,27575,27683,27791,27897,28001,28105,28208,28310,28411,28510,28609,28706,28803, 100 | 28898,28992,29085,29178,29268,29358,29447,29535,29621,29707,29791,29874,29956,30037,30117,30195, 101 | 30273,30349,30425,30499,30572,30643,30714,30783,30852,30919,30985,31050,31113,31176,31237,31297, 102 | 31356,31414,31471,31526,31580,31633,31685,31736,31785,31834,31881,31926,31971,32015,32057,32098, 103 | 32138,32176,32214,32250,32285,32319,32351,32382,32413,32441,32469,32496,32521,32545,32568,32589, 104 | 32609,32629,32646,32663,32678,32693,32706,32717,32728,32737,32745,32752,32757,32762,32765,32767 105 | }; 106 | #else 107 | static const uint16_t sineTable[257]={ 108 | 0,402,804,1206,1608,2010,2412,2814,3216,3617,4019,4420,4821,5222,5623,6023, 109 | 6424,6824,7223,7623,8022,8421,8820,9218,9616,10014,10411,10808,11204,11600,11996,12391, 110 | 12785,13179,13573,13966,14359,14751,15142,15533,15924,16313,16703,17091,17479,17866,18253,18639, 111 | 19024,19408,19792,20175,20557,20939,21319,21699,22078,22456,22834,23210,23586,23960,24334,24707, 112 | 25079,25450,25820,26189,26557,26925,27291,27656,28020,28383,28745,29106,29465,29824,30181,30538, 113 | 30893,31247,31600,31952,32302,32651,32999,33346,33692,34036,34379,34721,35061,35400,35738,36074, 114 | 36409,36743,37075,37406,37736,38064,38390,38715,39039,39361,39682,40001,40319,40635,40950,41263, 115 | 41575,41885,42194,42500,42806,43109,43411,43712,44011,44308,44603,44897,45189,45479,45768,46055, 116 | 46340,46624,46905,47185,47464,47740,48014,48287,48558,48827,49095,49360,49624,49885,50145,50403, 117 | 50659,50913,51166,51416,51664,51911,52155,52398,52638,52877,53113,53348,53580,53811,54039,54266, 118 | 54490,54713,54933,55151,55367,55582,55794,56003,56211,56417,56620,56822,57021,57218,57413,57606, 119 | 57797,57985,58171,58356,58537,58717,58895,59070,59243,59414,59582,59749,59913,60075,60234,60391, 120 | 60546,60699,60850,60998,61144,61287,61429,61567,61704,61838,61970,62100,62227,62352,62475,62595, 121 | 62713,62829,62942,63053,63161,63267,63371,63472,63571,63668,63762,63853,63943,64030,64114,64196, 122 | 64276,64353,64428,64500,64570,64638,64703,64765,64826,64883,64939,64992,65042,65090,65136,65179, 123 | 65219,65258,65293,65327,65357,65386,65412,65435,65456,65475,65491,65504,65515,65524,65530,65534, 124 | 65535, 125 | }; 126 | #endif 127 | 128 | 129 | 130 | 131 | int16_t sine(uint16_t angle) 132 | { 133 | #ifdef NZS_FAST_SINE 134 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 135 | if(angle >= 0 && angle<1280) //mks 2018-03 136 | return sineTable[angle]; 137 | else 138 | return(-1); 139 | #else 140 | return sineTable[angle]; 141 | #endif 142 | #else 143 | int sign=1; 144 | int16_t ret; 145 | //our sine table has 1024 points per rotation so convert angle to closest step 146 | 147 | if (angle>=(SINE_STEPS/2)) 148 | { 149 | sign=-1; 150 | } 151 | 152 | angle=angle % (SINE_STEPS/2); //limit to 0-180 as sign takes care of 180-360 153 | 154 | if (angle>(SINE_STEPS/4-1)) //if we are greater than 90 we need to look up table backwards 155 | { 156 | angle=(SINE_STEPS/2)-angle; 157 | } 158 | 159 | ret=(int16_t)(sineTable[angle]/2)*sign; 160 | return ret; 161 | #endif 162 | } 163 | 164 | int16_t cosine(uint16_t angle) 165 | { 166 | #ifdef NZS_FAST_SINE 167 | angle=angle+(SINE_STEPS/4); 168 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 169 | if(angle >= 0 && angle<1280) //mks 2018-03 170 | return sineTable[angle]; 171 | else 172 | return(-1); 173 | #else 174 | return sineTable[angle]; 175 | #endif 176 | #else 177 | 178 | int sign=1; 179 | int16_t ret; 180 | //our sine table has 1024 points per rotation so convert angle to closest step 181 | 182 | if (angle>=(SINE_STEPS/4) and angle<(3*(SINE_STEPS/4))) 183 | { 184 | sign=-1; 185 | } 186 | 187 | angle=angle % (SINE_STEPS/2); //limit to 0-180 as sign takes care of 180-360 188 | 189 | if (angle>(SINE_STEPS/4-1)) //if we are greater than 90 we need to look up table backwards 190 | { 191 | angle=(SINE_STEPS/2)-angle; 192 | } 193 | 194 | //for cosine we need 90 degree phase shift 195 | angle=(SINE_STEPS/4)-angle; 196 | 197 | ret=(int16_t)(sineTable[angle]/2)*sign; 198 | return ret; 199 | #endif 200 | } 201 | 202 | #pragma GCC pop_options 203 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/nzs_lcd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * nzs_lcd.cpp 3 | * 4 | * Created on: Dec 8, 2016 5 | * Author: trampas 6 | * 7 | * Misfit Tech invests time and resources providing this open source code, 8 | * please support Misfit Tech and open-source hardware by purchasing 9 | * products from Misfit Tech, www.misifittech.net! 10 | * 11 | * Written by Trampas Stern for Misfit Tech. 12 | * BSD license, check license.txt for more information 13 | * All text above, must be included in any redistribution 14 | *********************************************************************/ 15 | 16 | #include "nzs_lcd.h" 17 | #include 18 | #include 19 | #include 20 | 21 | 22 | #ifndef DISABLE_LCD 23 | void NZS_LCD::begin(StepperCtrl *ptrsCtrl) 24 | { 25 | #ifndef MECHADUINO_HARDWARE 26 | pinMode(PIN_SW1, INPUT_PULLUP); 27 | pinMode(PIN_SW3, INPUT_PULLUP); 28 | pinMode(PIN_SW4, INPUT_PULLUP); 29 | #endif 30 | buttonState=0; 31 | 32 | //we need access to the stepper controller 33 | ptrStepperCtrl=ptrsCtrl; //save a pointer to the stepper controller 34 | 35 | 36 | ptrMenu=NULL; 37 | menuIndex=0; 38 | menuActive=false; 39 | optionIndex=0; 40 | ptrOptions=NULL; 41 | displayEnabled=true; 42 | 43 | //check that the SCL and SDA are pulled high 44 | pinMode(PIN_SDA, INPUT); 45 | pinMode(PIN_SCL, INPUT); 46 | if (digitalRead(PIN_SDA)==0) 47 | { 48 | //pin is not pulled up 49 | displayEnabled=false; 50 | } 51 | if (digitalRead(PIN_SCL)==0) 52 | { 53 | //pin is not pulled up 54 | displayEnabled=false; 55 | } 56 | 57 | if (displayEnabled) 58 | { 59 | displayEnabled=display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 60 | }else 61 | { 62 | WARNING("SCL/SDA not pulled up"); 63 | } 64 | if (false == displayEnabled) 65 | { 66 | WARNING("NO display found, LCD will not be used"); 67 | } 68 | Wire.setClock(800000); 69 | 70 | //showSplash(); 71 | 72 | } 73 | 74 | 75 | void __attribute__ ((optimize("Ofast"))) NZS_LCD::lcdShow(const char *line1, const char *line2,const char *line3) 76 | { 77 | 78 | if (false == displayEnabled) 79 | { 80 | return; 81 | } 82 | display.clearDisplay(); 83 | display.setTextSize(2); 84 | display.setTextColor(WHITE); 85 | display.setCursor(0,0); 86 | display.println(line1); 87 | display.setCursor(0,20); 88 | display.println(line2); 89 | display.setCursor(0,40); 90 | display.println(line3); 91 | display.display(); 92 | 93 | } 94 | 95 | void NZS_LCD::showSplash(void) 96 | { 97 | if (false == displayEnabled) 98 | { 99 | return; 100 | } 101 | lcdShow("Misfit"," Tech", VERSION); 102 | } 103 | 104 | 105 | void NZS_LCD::setMenu(menuItem_t *pMenu) 106 | { 107 | if (false == displayEnabled) 108 | { 109 | return; 110 | } 111 | ptrMenu=pMenu; 112 | menuIndex=0; 113 | } 114 | 115 | 116 | void NZS_LCD::showOptions(void) 117 | { 118 | int32_t i,j; 119 | char str[3][26]={0}; 120 | if (false == displayEnabled) 121 | { 122 | return; 123 | } 124 | 125 | i=optionIndex; 126 | j=0; 127 | while(strlen(ptrOptions[i].str)>0 && j<3) 128 | { 129 | if (i == optionIndex) 130 | { 131 | sprintf(str[j],"*%s",ptrOptions[i].str); 132 | }else 133 | { 134 | sprintf(str[j]," %s",ptrOptions[i].str); 135 | } 136 | j++; 137 | i++; 138 | } 139 | 140 | lcdShow(str[0], str[1], str[2]); 141 | 142 | return; 143 | } 144 | 145 | 146 | void __attribute__ ((optimize("Ofast"))) NZS_LCD::showMenu(void) 147 | { 148 | int32_t i,j; 149 | char str[3][26]={0}; 150 | if (false == displayEnabled) 151 | { 152 | return; 153 | } 154 | 155 | i=menuIndex; 156 | j=0; 157 | while(ptrMenu[i].func!=NULL && j<3) 158 | { 159 | if (i == menuIndex) 160 | { 161 | sprintf(str[j],"*%s",ptrMenu[i].str); 162 | }else 163 | { 164 | sprintf(str[j]," %s",ptrMenu[i].str); 165 | } 166 | j++; 167 | i++; 168 | } 169 | 170 | //show exit if there is room 171 | if (j<3) 172 | { 173 | if (j==0) 174 | { 175 | sprintf(str[j],"*Exit"); 176 | }else 177 | { 178 | sprintf(str[j]," Exit"); 179 | } 180 | } 181 | 182 | lcdShow(str[0], str[1], str[2]); 183 | 184 | 185 | return; 186 | } 187 | 188 | 189 | void __attribute__ ((optimize("Ofast"))) NZS_LCD::updateMenu(void) 190 | { 191 | if (false == displayEnabled) 192 | { 193 | return; 194 | } 195 | 196 | if (ptrOptions != NULL) 197 | { 198 | showOptions(); 199 | }else 200 | { 201 | showMenu(); 202 | } 203 | 204 | //handle push buttons 205 | if (digitalRead(PIN_SW3)==0 && (buttonState & 0x02)==0) 206 | { 207 | buttonState |= 0x02; 208 | 209 | LOG("SW3 pressed"); 210 | if (ptrMenu[menuIndex].func == NULL) 211 | { 212 | //exit pressed 213 | menuIndex=0; //reset menu index 214 | menuActive=false; 215 | return; 216 | } 217 | 218 | if (ptrMenu[menuIndex].func != NULL) 219 | { 220 | LOG("Calling function for %s",ptrMenu[menuIndex].str); 221 | if (ptrOptions != NULL) 222 | { 223 | char *ptrArgV[1]; 224 | char str[25]={0}; 225 | ptrArgV[0]=str; 226 | sprintf(str,"%d",optionIndex); 227 | LOG("Calling function for %s %s",ptrMenu[menuIndex].str,str); 228 | ptrMenu[menuIndex].func(1,ptrArgV); 229 | ptrOptions=NULL; 230 | optionIndex=0; 231 | }else 232 | { 233 | int i; 234 | i=ptrMenu[menuIndex].func(0,NULL); 235 | if (ptrMenu[menuIndex].ptrOptions != NULL) 236 | { 237 | LOG("displaying options for %s %d",ptrMenu[menuIndex].str,i); 238 | ptrOptions=ptrMenu[menuIndex].ptrOptions; 239 | optionIndex=i; 240 | } 241 | } 242 | 243 | return; 244 | } 245 | 246 | } 247 | if (digitalRead(PIN_SW1)==0 && (buttonState & 0x01)==0) 248 | { 249 | buttonState |= 0x01; 250 | LOG("SW1 pressed"); 251 | if (ptrOptions != NULL) 252 | { 253 | optionIndex++; 254 | if (strlen(ptrOptions[optionIndex].str) == 0) 255 | { 256 | optionIndex=0; 257 | } 258 | } else 259 | { 260 | if (ptrMenu[menuIndex].func != NULL) 261 | { 262 | menuIndex++; 263 | } else 264 | { 265 | menuIndex=0; 266 | } 267 | } 268 | 269 | } 270 | 271 | if (digitalRead(PIN_SW1)) 272 | { 273 | buttonState &= ~0x01; 274 | } 275 | 276 | if (digitalRead(PIN_SW3)) 277 | { 278 | buttonState &= ~0x02; 279 | } 280 | } 281 | 282 | void NZS_LCD::forceMenuActive(void) 283 | { 284 | 285 | menuActive=true; 286 | } 287 | 288 | void __attribute__((optimize("Ofast")))NZS_LCD::process(void) 289 | { 290 | if (false == displayEnabled) 291 | { 292 | return; 293 | } 294 | 295 | if (false == menuActive || ptrMenu==NULL) 296 | { 297 | updateLCD(); 298 | }else 299 | { 300 | updateMenu(); 301 | } 302 | 303 | if (digitalRead(PIN_SW4)==0 && (buttonState & 0x04)==0) 304 | { 305 | buttonState |= 0x04; 306 | menuActive=!menuActive; 307 | } 308 | 309 | if (digitalRead(PIN_SW4)) 310 | { 311 | buttonState &= ~0x04; 312 | } 313 | } 314 | #endif 315 | /* 316 | //does the LCD menu system 317 | void StepperCtrl::menu(void) 318 | { 319 | 320 | bool done=false; 321 | int menuItem=0; 322 | char str[100]; 323 | int sw1State=0; 324 | int sw3State=0; 325 | 326 | pinMode(PIN_SW1, INPUT_PULLUP); 327 | pinMode(PIN_SW3, INPUT_PULLUP); 328 | pinMode(PIN_SW4, INPUT_PULLUP); 329 | 330 | 331 | while (!done) 332 | { 333 | display.clearDisplay(); 334 | display.setTextSize(2); 335 | display.setTextColor(WHITE); 336 | 337 | if (menuItem==0) 338 | { 339 | sprintf(str,"*Run Cal"); 340 | display.setCursor(0,0); 341 | display.println(str); 342 | }else 343 | { 344 | sprintf(str," Run Cal"); 345 | display.setCursor(0,0); 346 | display.println(str); 347 | } 348 | 349 | if (menuItem==1) 350 | { 351 | sprintf(str,"*Check Cal"); 352 | display.setCursor(0,20); 353 | display.println(str); 354 | }else 355 | { 356 | sprintf(str," Check Cal"); 357 | display.setCursor(0,20); 358 | display.println(str); 359 | } 360 | 361 | if (menuItem==2) 362 | { 363 | sprintf(str,"*Exit"); 364 | display.setCursor(0,40); 365 | display.println(str); 366 | }else 367 | { 368 | sprintf(str," Exit"); 369 | display.setCursor(0,40); 370 | display.println(str); 371 | } 372 | 373 | display.display(); 374 | 375 | if (sw1State==1) 376 | { 377 | while (digitalRead(PIN_SW1)==0); 378 | sw1State=0; 379 | } 380 | 381 | if (digitalRead(PIN_SW1)==0) 382 | { 383 | sw1State=1; 384 | menuItem=(menuItem+1)%3; 385 | } 386 | 387 | if (sw3State==1) 388 | { 389 | while (digitalRead(PIN_SW3)==0); 390 | sw3State=0; 391 | } 392 | 393 | if (digitalRead(PIN_SW3)==0) 394 | { 395 | sw3State=1; 396 | switch(menuItem) 397 | { 398 | case 0: 399 | display.clearDisplay(); 400 | display.setTextSize(2); 401 | display.setTextColor(WHITE); 402 | display.setCursor(0,0); 403 | display.println("Running"); 404 | display.setCursor(0,20); 405 | display.println("Cal"); 406 | display.display(); 407 | calibrateEncoder(); 408 | break; 409 | case 1: 410 | { 411 | display.clearDisplay(); 412 | display.setTextSize(2); 413 | display.setTextColor(WHITE); 414 | display.setCursor(0,0); 415 | display.println("Testing"); 416 | display.setCursor(0,20); 417 | display.println("Cal"); 418 | display.display(); 419 | int32_t error,x,y,m; 420 | error=maxCalibrationError(); 421 | x=(error*100 *360)/ANGLE_STEPS; 422 | m=x/100; 423 | y=abs(x-(m*100)); 424 | display.clearDisplay(); 425 | display.setTextSize(2); 426 | display.setTextColor(WHITE); 427 | display.setCursor(0,0); 428 | display.println("Error"); 429 | 430 | sprintf(str, "%02d.%02d deg",m,y); 431 | display.setCursor(0,20); 432 | display.println(str); 433 | display.display(); 434 | while (digitalRead(PIN_SW3)); 435 | break; 436 | } 437 | case 2: 438 | return; 439 | break; 440 | 441 | } 442 | 443 | } 444 | 445 | } 446 | 447 | } 448 | 449 | */ 450 | 451 | void NZS_LCD::updateLCD(void) 452 | { 453 | if (false == displayEnabled) 454 | { 455 | return; 456 | } 457 | char str[3][25]; 458 | static int highRPM=0; 459 | int32_t y,z,err; 460 | 461 | static int64_t lastAngle,deg; 462 | static int32_t RPM=0; 463 | static int32_t lasttime=0; 464 | 465 | bool state; 466 | static int32_t dt=40; 467 | static uint32_t t0=0; 468 | 469 | static bool rpmDone=false; 470 | 471 | if ((millis()-t0)>500) 472 | { 473 | 474 | int32_t x,d; 475 | 476 | //do first half of RPM measurement 477 | if (!rpmDone) 478 | { 479 | //LOG("loop time is %dus",ptrStepperCtrl->getLoopTime()); 480 | lastAngle=ptrStepperCtrl->getCurrentAngle(); 481 | lasttime=millis(); 482 | rpmDone=true; 483 | return; 484 | } 485 | 486 | //do the second half of rpm measurement and update LCD. 487 | if (rpmDone && (millis()-lasttime)>(dt)) 488 | { 489 | rpmDone=false; 490 | deg=ptrStepperCtrl->getCurrentAngle(); 491 | y=millis()-lasttime; 492 | err=ptrStepperCtrl->getLoopError(); 493 | 494 | t0=millis(); 495 | d=(int64_t)(lastAngle-deg); 496 | 497 | d=abs(d); 498 | 499 | x=0; 500 | if (d>0) 501 | { 502 | x=((int64_t)d*(60*1000UL))/((int64_t)y * ANGLE_STEPS); 503 | } 504 | 505 | lastAngle=deg; 506 | RPM=(int32_t)x; //(7*RPM+x)/8; //average RPMs 507 | if (RPM>500) 508 | { 509 | dt=10; 510 | } 511 | if (RPM<100) 512 | { 513 | dt=100; 514 | } 515 | str[0][0]='\0'; 516 | //LOG("RPMs is %d, %d, %d",(int32_t)x,(int32_t)d,(int32_t)y); 517 | switch(ptrStepperCtrl->getControlMode()) 518 | { 519 | case CTRL_SIMPLE: 520 | sprintf(str[0], "%dRPM simp",RPM); 521 | break; 522 | 523 | case CTRL_POS_PID: 524 | sprintf(str[0], "%dRPM pPID",RPM); 525 | break; 526 | 527 | case CTRL_POS_VELOCITY_PID: 528 | sprintf(str[0], "%dRPM vPID",RPM); 529 | break; 530 | 531 | case CTRL_OPEN: 532 | sprintf(str[0], "%dRPM open",RPM); 533 | break; 534 | case CTRL_OFF: 535 | sprintf(str[0], "%dRPM off",RPM); 536 | break; 537 | default: 538 | sprintf(str[0], "error %u",ptrStepperCtrl->getControlMode()); 539 | break; 540 | 541 | } 542 | 543 | 544 | err=(err*360*100)/(int32_t)ANGLE_STEPS; 545 | //LOG("error is %d %d %d",err,(int32_t)ptrStepperCtrl->getCurrentLocation(),(int32_t)ptrStepperCtrl->getDesiredLocation()); 546 | z=(err)/100; 547 | y=abs(err-(z*100)); 548 | 549 | sprintf(str[1],"%01d.%02d err", z,y); 550 | 551 | 552 | deg=ptrStepperCtrl->getDesiredAngle(); 553 | 554 | #ifndef NZS_LCD_ABSOULTE_ANGLE 555 | deg=deg & ANGLE_MAX; //limit to 360 degrees 556 | #endif 557 | 558 | deg=(deg*360*10)/(int32_t)ANGLE_STEPS; 559 | int K=0; 560 | if (abs(deg)>9999) 561 | { 562 | K=1; 563 | deg=deg/1000; 564 | } 565 | 566 | x=(deg)/10; 567 | y=abs(deg-(x*10)); 568 | 569 | if (K==1) 570 | { 571 | sprintf(str[2],"%03d.%01uKdeg", x,y); 572 | }else 573 | { 574 | sprintf(str[2],"%03d.%01udeg", x,y); 575 | } 576 | str[0][10]='\0'; 577 | str[1][10]='\0'; 578 | str[2][10]='\0'; 579 | lcdShow(str[0],str[1],str[2]); 580 | } 581 | } 582 | } 583 | 584 | 585 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A4954.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include "A4954.h" 13 | #include "wiring_private.h" 14 | #include "syslog.h" 15 | #include "angle.h" 16 | #include "Arduino.h" 17 | #include "sine.h" 18 | 19 | static uint8_t pinState=0; 20 | #ifndef MKS_GCC_O0 //mks mark ("-O0") 21 | #pragma GCC push_options 22 | #pragma GCC optimize ("-Ofast") 23 | #endif 24 | 25 | 26 | 27 | #define DAC_MAX (0x01FFL) 28 | // Wait for synchronization of registers between the clock domains 29 | static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); 30 | static void syncTCC(Tcc* TCCx) { 31 | //int32_t t0=1000; 32 | while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK) 33 | { 34 | // t0--; 35 | // if (t0==0) 36 | // { 37 | // break; 38 | // } 39 | // delay(1); 40 | } 41 | } 42 | 43 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 44 | #include "atmel_start_pins.h" 45 | 46 | static inline void bridge1(int state) 47 | { 48 | if (state==0) 49 | { 50 | /* 51 | PORT->Group[g_APinDescription[PIN_A4954_IN1].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN1].ulPin].bit.PMUXEN = 0; 52 | gpio_set_pin_level(IN1, true); 53 | gpio_set_pin_level(IN2, false); 54 | */ 55 | PORT->Group[PORTA].PINCFG[0x05].bit.PMUXEN = 0; //PIN_A4954_IN1 56 | ((Port *)0x60000000)->Group[0x00].OUTSET.reg = 0x00000020; //IN1 true 57 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00200000; //IN2 false 58 | 59 | pinState=(pinState & 0x0C) | 0x1; 60 | } 61 | if (state==1) 62 | { 63 | PORT->Group[PORTA].PINCFG[0x15].bit.PMUXEN = 0; //PIN_A4954_IN2 64 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00000020; //IN1 false 65 | ((Port *)0x60000000)->Group[0x00].OUTSET.reg = 0x00200000; //IN2 true 66 | pinState=(pinState & 0x0C) | 0x2; 67 | } 68 | if (state==3) 69 | { 70 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00000020; //IN1 false 71 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00200000; //IN2 false 72 | } 73 | } 74 | 75 | static inline void bridge2(int state) 76 | { 77 | if (state==0) 78 | { 79 | PORT->Group[PORTA].PINCFG[0x0f].bit.PMUXEN = 0; //PIN_A4954_IN3 80 | ((Port *)0x60000000)->Group[0x00].OUTSET.reg = 0x00008000; //IN3 true 81 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00100000; //IN 4false 82 | pinState=(pinState & 0x03) | 0x4; 83 | } 84 | if (state==1) 85 | { 86 | PORT->Group[PORTA].PINCFG[0x14].bit.PMUXEN = 0; //PIN_A4954_IN4 87 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00008000; //IN3 false 88 | ((Port *)0x60000000)->Group[0x00].OUTSET.reg = 0x00100000; //IN4 true 89 | pinState=(pinState & 0x03) | 0x8; 90 | } 91 | if (state==3) 92 | { 93 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00008000; //IN3 false 94 | ((Port *)0x60000000)->Group[0x00].OUTCLR.reg = 0x00100000; //IN 4false 95 | } 96 | } 97 | 98 | #else 99 | static inline void bridge1(int state) 100 | { 101 | if (state==0) 102 | { 103 | PORT->Group[g_APinDescription[PIN_A4954_IN1].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN1].ulPin].bit.PMUXEN = 0; 104 | GPIO_OUTPUT(PIN_A4954_IN1);//pinMode(PIN_A4954_IN1,OUTPUT); 105 | GPIO_OUTPUT(PIN_A4954_IN2);//pinMode(PIN_A4954_IN2,OUTPUT); 106 | GPIO_HIGH(PIN_A4954_IN1);// digitalWrite(PIN_A4954_IN1, HIGH); 107 | GPIO_LOW(PIN_A4954_IN2);//digitalWrite(PIN_A4954_IN2, LOW); 108 | //pinPeripheral(PIN_A4954_IN2, PIO_TIMER_ALT); 109 | pinState=(pinState & 0x0C) | 0x1; 110 | } 111 | if (state==1) 112 | { 113 | PORT->Group[g_APinDescription[PIN_A4954_IN2].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN2].ulPin].bit.PMUXEN = 0; 114 | GPIO_OUTPUT(PIN_A4954_IN2);//pinMode(PIN_A4954_IN2,OUTPUT); 115 | GPIO_OUTPUT(PIN_A4954_IN1);//pinMode(PIN_A4954_IN1,OUTPUT); 116 | GPIO_LOW(PIN_A4954_IN1);//digitalWrite(PIN_A4954_IN1, LOW); 117 | GPIO_HIGH(PIN_A4954_IN2);//digitalWrite(PIN_A4954_IN2, HIGH); 118 | //pinPeripheral(PIN_A4954_IN1, PIO_TIMER); 119 | pinState=(pinState & 0x0C) | 0x2; 120 | } 121 | if (state==3) 122 | { 123 | GPIO_LOW(PIN_A4954_IN1); 124 | GPIO_LOW(PIN_A4954_IN2); 125 | //digitalWrite(PIN_A4954_IN1, LOW); 126 | //digitalWrite(PIN_A4954_IN2, LOW); 127 | } 128 | } 129 | 130 | static inline void bridge2(int state) 131 | { 132 | if (state==0) 133 | { 134 | PORT->Group[g_APinDescription[PIN_A4954_IN3].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN3].ulPin].bit.PMUXEN = 0; 135 | GPIO_OUTPUT(PIN_A4954_IN3); //pinMode(PIN_A4954_IN3,OUTPUT); 136 | GPIO_OUTPUT(PIN_A4954_IN4);//pinMode(PIN_A4954_IN4,OUTPUT); 137 | GPIO_HIGH(PIN_A4954_IN3);//digitalWrite(PIN_A4954_IN3, HIGH); 138 | GPIO_LOW(PIN_A4954_IN4);//digitalWrite(PIN_A4954_IN4, LOW); 139 | //pinPeripheral(PIN_A4954_IN4, PIO_TIMER_ALT); 140 | pinState=(pinState & 0x03) | 0x4; 141 | } 142 | if (state==1) 143 | { 144 | PORT->Group[g_APinDescription[PIN_A4954_IN4].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN4].ulPin].bit.PMUXEN = 0; 145 | GPIO_OUTPUT(PIN_A4954_IN4);//pinMode(PIN_A4954_IN4,OUTPUT); 146 | GPIO_OUTPUT(PIN_A4954_IN3);//pinMode(PIN_A4954_IN3,OUTPUT); 147 | GPIO_LOW(PIN_A4954_IN3);//digitalWrite(PIN_A4954_IN3, LOW); 148 | GPIO_HIGH(PIN_A4954_IN4);//digitalWrite(PIN_A4954_IN4, HIGH); 149 | //pinPeripheral(PIN_A4954_IN3, PIO_TIMER_ALT); 150 | pinState=(pinState & 0x03) | 0x8; 151 | } 152 | if (state==3) 153 | { 154 | GPIO_LOW(PIN_A4954_IN3); 155 | GPIO_LOW(PIN_A4954_IN4); 156 | //digitalWrite(PIN_A4954_IN3, LOW); 157 | //digitalWrite(PIN_A4954_IN4, LOW); 158 | } 159 | } 160 | #endif 161 | 162 | static void enableTCC0(uint8_t percent) 163 | { 164 | #ifdef MECHADUINO_HARDWARE 165 | return; 166 | #else 167 | 168 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 169 | return; 170 | #endif 171 | Tcc* TCCx = TCC0 ; 172 | 173 | 174 | uint32_t ulValue=((uint32_t)(100-percent)*480)/100; 175 | //ERROR("Enable TCC0"); 176 | 177 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ; 178 | 179 | while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ; 180 | 181 | //ERROR("Setting TCC %d %d",ulValue,ulPin); 182 | TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE; 183 | syncTCC(TCCx); 184 | 185 | // Set TCx as normal PWM 186 | TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; 187 | syncTCC(TCCx); 188 | 189 | // Set TCx in waveform mode Normal PWM 190 | TCCx->CC[1].reg = (uint32_t)ulValue; //ch5 //IN3 191 | syncTCC(TCCx); 192 | 193 | TCCx->CC[2].reg = (uint32_t)ulValue; //ch6 //IN4 194 | syncTCC(TCCx); 195 | 196 | TCCx->CC[3].reg = (uint32_t)ulValue; //ch7 //IN2 197 | syncTCC(TCCx); 198 | 199 | TCCx->CC[1].reg = (uint32_t)ulValue; //ch1 == ch5 //IN1 200 | 201 | syncTCC(TCCx); 202 | 203 | // Set PER to maximum counter value (resolution : 0xFF) 204 | TCCx->PER.reg = DAC_MAX; 205 | syncTCC(TCCx); 206 | 207 | // Enable TCCx 208 | TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; 209 | syncTCC(TCCx); 210 | //ERROR("Enable TCC0 DONE"); 211 | #endif 212 | } 213 | 214 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 215 | void setDAC(uint32_t DAC1, uint32_t DAC2) 216 | #else 217 | static void setDAC(uint32_t DAC1, uint32_t DAC2) 218 | #endif 219 | { 220 | TCC1->CC[1].reg = (uint32_t)DAC1; //D9 PA07 - VREF12 221 | syncTCC(TCC1); 222 | TCC1->CC[0].reg = (uint32_t)DAC2; //D4 - VREF34 223 | syncTCC(TCC1); 224 | 225 | 226 | } 227 | #ifdef MKS_USE_AS //mks mark ("USE_AS") 228 | void setupDAC(void) 229 | #else 230 | static void setupDAC(void) 231 | #endif 232 | { 233 | Tcc* TCCx = TCC1 ; 234 | 235 | 236 | pinPeripheral(PIN_A4954_VREF34, PIO_TIMER_ALT); 237 | pinPeripheral(PIN_A4954_VREF12, PIO_TIMER); 238 | 239 | GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ; 240 | 241 | while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ; 242 | 243 | //ERROR("Setting TCC %d %d",ulValue,ulPin); 244 | TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE; 245 | syncTCC(TCCx); 246 | 247 | // Set TCx as normal PWM 248 | TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM; 249 | syncTCC(TCCx); 250 | 251 | // Set TCx in waveform mode Normal PWM 252 | TCCx->CC[1].reg = (uint32_t)0; 253 | syncTCC(TCCx); 254 | 255 | TCCx->CC[0].reg = (uint32_t)0; 256 | syncTCC(TCCx); 257 | 258 | // Set PER to maximum counter value (resolution : 0xFFF = 12 bits) 259 | // =48e6/2^12=11kHz frequency 260 | TCCx->PER.reg = DAC_MAX; 261 | syncTCC(TCCx); 262 | 263 | // Enable TCCx 264 | TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ; 265 | syncTCC(TCCx); 266 | 267 | } 268 | 269 | 270 | void A4954::begin() 271 | { 272 | //setup the A4954 pins 273 | digitalWrite(PIN_A4954_IN3,LOW); 274 | pinMode(PIN_A4954_IN3,OUTPUT); 275 | digitalWrite(PIN_A4954_IN4,LOW); 276 | pinMode(PIN_A4954_IN4,OUTPUT); 277 | digitalWrite(PIN_A4954_IN2,LOW); 278 | pinMode(PIN_A4954_IN2,OUTPUT); 279 | digitalWrite(PIN_A4954_IN1,LOW); 280 | pinMode(PIN_A4954_IN1,OUTPUT); 281 | 282 | //setup the PWM for current on the A4954, set for low current 283 | digitalWrite(PIN_A4954_VREF12,LOW); 284 | digitalWrite(PIN_A4954_VREF34,LOW); 285 | pinMode(PIN_A4954_VREF34, OUTPUT); 286 | pinMode(PIN_A4954_VREF12, OUTPUT); 287 | 288 | enabled=true; 289 | lastStepMicros=0; 290 | forwardRotation=true; 291 | 292 | enableTCC0(90); 293 | setupDAC(); 294 | // 295 | // int i=0; 296 | // bridge1(0); 297 | // bridge2(0); 298 | //while (1) 299 | // { 300 | // int32_t x; 301 | // WARNING("MA %d",i); 302 | // x=(int32_t)((int64_t)i*(DAC_MAX))/3300; 303 | // setDAC(x,x); 304 | // delay(1000); 305 | // i=i+10; 306 | // if (i>1000) 307 | // { 308 | // i=0; 309 | // } 310 | // 311 | // } 312 | 313 | // 314 | // WARNING("Setting DAC for 500mA output"); 315 | // setDAC((int32_t)((int64_t)1000*(DAC_MAX))/3300,(int32_t)((int64_t)1000*(DAC_MAX))/3300); 316 | // bridge1(0); 317 | // bridge2(0); 318 | // while(1) 319 | // { 320 | // 321 | // } 322 | return; 323 | } 324 | 325 | void A4954::limitCurrent(uint8_t percent) 326 | { 327 | #ifdef MECHADUINO_HARDWARE 328 | return; 329 | #else 330 | //WARNING("current limit %d",percent); 331 | enableTCC0(percent); 332 | if (pinState & 0x01) 333 | { 334 | pinPeripheral(PIN_A4954_IN2, PIO_TIMER_ALT); //TCC0 WO[7] 335 | } 336 | if (pinState & 0x02) 337 | { 338 | pinPeripheral(PIN_A4954_IN1, PIO_TIMER); //TCC0 WO[1] 339 | } 340 | if (pinState & 0x04) 341 | { 342 | pinPeripheral(PIN_A4954_IN4, PIO_TIMER_ALT); 343 | } 344 | if (pinState & 0x08) 345 | { 346 | pinPeripheral(PIN_A4954_IN3, PIO_TIMER_ALT); 347 | } 348 | #endif 349 | } 350 | 351 | 352 | void A4954::enable(bool enable) 353 | { 354 | enabled=enable; 355 | if (enabled == false) 356 | { 357 | WARNING("A4954 disabled"); 358 | setDAC(0,0); //turn current off 359 | bridge1(3); //tri state bridge outputs 360 | bridge2(3); //tri state bridge outputs 361 | } 362 | } 363 | 364 | 365 | 366 | //this is precise move and modulo of A4954_NUM_MICROSTEPS is a full step. 367 | // stepAngle is in A4954_NUM_MICROSTEPS units.. 368 | // The A4954 has no idea where the motor is, so the calling function has to 369 | // to tell the A4954 what phase to drive motor coils. 370 | // A4954_NUM_MICROSTEPS is 256 by default so stepAngle of 1024 is 360 degrees 371 | // Note you can only move up to +/-A4954_NUM_MICROSTEPS from where you 372 | // currently are. 373 | int32_t A4954::move(int32_t stepAngle, uint32_t mA) 374 | { 375 | uint16_t angle; 376 | int32_t cos,sin; 377 | int32_t dacSin,dacCos; 378 | //static int i=0; 379 | 380 | if (enabled == false) 381 | { 382 | //WARNING("A4954 disabled"); 383 | setDAC(0,0); //turn current off 384 | bridge1(3); //tri state bridge outputs 385 | bridge2(3); //tri state bridge outputs 386 | return stepAngle; 387 | } 388 | 389 | //WARNING("move %d %d",stepAngle,mA); 390 | //handle roll overs, could do with modulo operator 391 | stepAngle=stepAngle%SINE_STEPS; 392 | 393 | //figure out our sine Angle 394 | // note our SINE_STEPS is 4x of microsteps for a reason 395 | //angle=(stepAngle+(SINE_STEPS/8)) % SINE_STEPS; 396 | angle=(stepAngle); 397 | 398 | //calculate the sine and cosine of our angle 399 | sin=sine(angle); 400 | cos=cosine(angle); 401 | 402 | //if we are reverse swap the sign of one of the angels 403 | if (false == forwardRotation) 404 | { 405 | cos=-cos; 406 | } 407 | 408 | //scale sine result by current(mA) 409 | dacSin=((int32_t)mA*(int64_t)abs(sin))/SINE_MAX; 410 | 411 | //scale cosine result by current(mA) 412 | dacCos=((int32_t)mA*(int64_t)abs(cos))/SINE_MAX; 413 | 414 | // if (i==0) 415 | // { 416 | // WARNING("dacs are %d %d",dacSin,dacCos); 417 | // } 418 | 419 | //convert value into DAC scaled to 3300mA max 420 | dacCos=(int32_t)((int64_t)dacCos*(DAC_MAX))/3300; 421 | //convert value into DAC scaled to 3300mA max 422 | dacSin=(int32_t)((int64_t)dacSin*(DAC_MAX))/3300; 423 | 424 | //WARNING("dacs are %d %d ",dacSin,dacCos); 425 | 426 | setDAC(dacSin,dacCos); 427 | 428 | if (sin>0) 429 | { 430 | bridge1(1); 431 | }else 432 | { 433 | bridge1(0); 434 | } 435 | if (cos>0) 436 | { 437 | bridge2(1); 438 | }else 439 | { 440 | bridge2(0); 441 | } 442 | 443 | // if (i++>3000) 444 | // { 445 | // i=0; 446 | // } 447 | // YELLOW_LED(led); 448 | // led=(led+1) & 0x01; 449 | lastStepMicros=micros(); 450 | return stepAngle; 451 | } 452 | #pragma GCC pop_options 453 | 454 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/calibration.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include "calibration.h" 13 | #include "Flash.h" 14 | #include "nonvolatile.h" 15 | #include "board.h" //for divide with rounding macro 16 | #include "utils.h" 17 | 18 | 19 | static uint16_t getTableIndex(uint16_t value) 20 | { 21 | int32_t x; 22 | 23 | x=((int32_t)value*CALIBRATION_TABLE_SIZE)/CALIBRATION_STEPS; //the divide is a floor not a round which is what we want 24 | return (uint16_t)x; 25 | 26 | } 27 | static uint16_t interp(Angle x1, Angle y1, Angle x2, Angle y2, Angle x) 28 | { 29 | int32_t dx,dy,dx2,y; 30 | dx=x2-x1; 31 | dy=y2-y1; 32 | dx2=x-x1; 33 | y=(int32_t)y1+DIVIDE_WITH_ROUND((dx2*dy),dx); 34 | if (y<0) 35 | { 36 | y=y+CALIBRATION_STEPS; 37 | } 38 | return (uint16_t)y; 39 | } 40 | 41 | static void printData(int32_t *data, int32_t n) 42 | { 43 | int32_t i; 44 | Serial.print("\n\r"); 45 | for (i=0; iFastCal.angle[x]; 85 | }else 86 | { 87 | return reverseLookup(encoderAngle); 88 | } 89 | #else 90 | return reverseLookup(encoderAngle) 91 | #endif 92 | } 93 | 94 | Angle CalibrationTable::reverseLookup(Angle encoderAngle) 95 | { 96 | 97 | int32_t i=0; 98 | int32_t a1,a2; 99 | int32_t x; 100 | int16_t y; 101 | int32_t min,max; 102 | min=(uint16_t)table[0].value; 103 | max=min; 104 | 105 | 106 | 107 | for (i=0; imax) 115 | { 116 | max=x; 117 | } 118 | } 119 | 120 | 121 | x=(uint16_t)encoderAngle; 122 | if (xCALIBRATION_STEPS/2) 144 | { 145 | if (a1=a1 && x<=a2) || 158 | (x>=a2 && x<=a1) ) 159 | { 160 | //LOG("%d", i); 161 | // inerpolate results and return 162 | //LOG("%d %d %d",a1,a2,x); 163 | //LOG("%d,%d",(i*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE,((i+2)*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE); 164 | 165 | y=interp(a1, DIVIDE_WITH_ROUND((i*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), a2, DIVIDE_WITH_ROUND( ((i+1)*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), x); 166 | 167 | return y; 168 | } 169 | i++; 170 | } 171 | ERROR("WE did some thing wrong"); 172 | 173 | 174 | 175 | 176 | } 177 | 178 | 179 | void CalibrationTable::smoothTable(void) 180 | { 181 | uint16_t b[]={1,2,4,5,4,2,1}; 182 | uint16_t sum_b=19; //sum of b filter 183 | 184 | int32_t data[CALIBRATION_TABLE_SIZE]; 185 | int32_t table2[CALIBRATION_TABLE_SIZE]; 186 | 187 | int32_t i; 188 | int32_t offset=0; 189 | int32_t startNum; 190 | 191 | //first lets handle the wrap around in the table 192 | for (i=0; i0 && offset==0) 195 | { 196 | if(((uint16_t)table[i-1].value-(uint16_t)table[i].value) <-32768) 197 | { 198 | offset=-65536; 199 | } 200 | 201 | if (((uint16_t)table[i-1].value-(uint16_t)table[i].value) > 32768) 202 | { 203 | offset=65536; 204 | } 205 | } 206 | table2[i]=(int32_t)((uint16_t)table[i].value)+offset; 207 | } 208 | 209 | //Serial.print("after wrap\n\r"); 210 | //printData(table2,CALIBRATION_TABLE_SIZE); 211 | 212 | //remove the starting offset and compensate table for index 213 | startNum=table2[0]; 214 | for (i=0; i=CALIBRATION_TABLE_SIZE) 237 | { 238 | ix=ix-CALIBRATION_TABLE_SIZE; 239 | } 240 | if (i==0) 241 | { 242 | LOG("index %d",ix); 243 | } 244 | sum=sum+table2[ix]*b[ib]; 245 | ib++; 246 | } 247 | sum=DIVIDE_WITH_ROUND(sum,sum_b); 248 | data[i]=sum; 249 | } 250 | 251 | //Serial.print("after filter\n\r"); 252 | //printData(data,CALIBRATION_TABLE_SIZE); 253 | 254 | //add in offset and the phase compenstation 255 | for (i=0; i=65536) 267 | { 268 | data[i]=data[i]-65536; 269 | } 270 | } 271 | 272 | //Serial.print("after wrap added\n\r"); 273 | //printData(data,CALIBRATION_TABLE_SIZE); 274 | 275 | //save new table 276 | for (i=0; iCalibrationTable,sizeof(data)); 297 | createFastCal(); 298 | 299 | LOG("after writting status is %d",data.status); 300 | loadFromFlash(); 301 | 302 | } 303 | 304 | void CalibrationTable::loadFromFlash(void) 305 | { 306 | FlashCalData_t data; 307 | int i; 308 | LOG("Reading Calbiration to Flash"); 309 | memcpy(&data, &NVM->CalibrationTable,sizeof(data)); 310 | for (i=0; iCalibrationTable.status); 321 | return NVM->CalibrationTable.status; 322 | } 323 | 324 | 325 | void CalibrationTable::createFastCal(void) 326 | { 327 | #ifdef NZS_FAST_CAL 328 | int32_t i; 329 | uint16_t cs=0; 330 | uint16_t data[256]; 331 | int32_t j; 332 | j=0; 333 | cs=0; 334 | LOG("setting fast calibration"); 335 | for (i=0; i<16384; i++) 336 | { 337 | 338 | uint16_t x; 339 | x=reverseLookup(i*4); 340 | data[j]=x; 341 | j++; 342 | if (j>=256) 343 | { 344 | flashWrite(&NVM->FastCal.angle[i-255],data,256*sizeof(uint16_t)); 345 | //LOG("Wrote fastcal at index %d-%d", i-255, i); 346 | j=0; 347 | } 348 | cs+=x; 349 | } 350 | //update the checksum 351 | flashWrite(&NVM->FastCal.checkSum,&cs,sizeof(uint16_t)); 352 | fastCalVaild=true; 353 | 354 | //this is a quick test 355 | /* 356 | for (i=0; i<16384; i++) 357 | { 358 | LOG("fast Cal %d,%d,%d",i,NVM->FastCal.angle[i],(uint32_t)reverseLookup(i*4)); 359 | } 360 | */ 361 | #endif 362 | } 363 | void CalibrationTable::updateFastCal(void) 364 | { 365 | #ifdef NZS_FAST_CAL 366 | int32_t i; 367 | uint16_t cs=0; 368 | uint16_t data[256]; 369 | int32_t j; 370 | bool NonZero=false; 371 | for (i=0; i<16384; i++) 372 | { 373 | cs+=NVM->FastCal.angle[i]; 374 | if (cs != 0) 375 | { 376 | NonZero=true; 377 | } 378 | } 379 | if (cs!=NVM->FastCal.checkSum || NonZero==false) 380 | { 381 | createFastCal(); 382 | } 383 | else 384 | { 385 | LOG("fast cal is valid"); 386 | fastCalVaild=true; 387 | } 388 | #endif 389 | } 390 | 391 | void CalibrationTable::init(void) 392 | { 393 | int i; 394 | 395 | if (true == flashGood()) 396 | { 397 | loadFromFlash(); 398 | updateFastCal(); 399 | }else 400 | { 401 | for (i=0; iCALIBRATION_STEPS/2) 427 | { 428 | dist=dist-CALIBRATION_STEPS; 429 | } 430 | 431 | //if our distance is larger than size between calibration points in table we will ignore this sample 432 | if (dist>CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE) 433 | { 434 | //spans two or more table calibration points for this implementation we will not use 435 | lastIndex=(int32_t)index; 436 | lastValue=value; 437 | return; 438 | } 439 | 440 | //now lets see if the values are above and below a table calibration point 441 | dist= abs(getTableIndex(lastAngle)-getTableIndex(actualAngle)); 442 | if (dist != 0) //if the two indexs into table are not the same it spans a calibration point in table. 443 | { 444 | //the two span a set calibation table point. 445 | uint16_t newValue; 446 | newValue=interp(lastAngle, lastEncoderValue, actualAngle, encoderValue, getTableIndex(actualAngle)*(CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE)) 447 | //this new value is our best guess as to the correct calibration value. 448 | updateTableValue(getTableIndex(actualAngle),newValue); 449 | } else 450 | { 451 | //we should calibate the table value for the point the closest 452 | } 453 | 454 | 455 | 456 | 457 | 458 | } 459 | lastAngle=(int32_t)actualAngle; 460 | lastEncoderValue=encoderValue; 461 | 462 | } 463 | #endif 464 | 465 | //when we are microstepping and are in between steps the probability the stepper motor did not move 466 | // is high. That is the actualAngle will be correct but the encoderValue will be behind due to not having enough torque to move motor. 467 | // Therefore we only want to update the calibration on whole steps where we have highest probability of things being correct. 468 | void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue) 469 | { 470 | int32_t dist, index; 471 | Angle tableAngle; 472 | 473 | index = getTableIndex((uint32_t)actualAngle+CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE/2); //add half of distance to next entry to round to closest table index 474 | 475 | tableAngle=(index*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE; //calculate the angle for this index 476 | 477 | dist=tableAngle-actualAngle; //distance to calibration table angle 478 | 479 | //LOG("Dist is %d",dist); 480 | if (abs(dist)=CALIBRATION_TABLE_SIZE) 519 | { 520 | indexHigh -= CALIBRATION_TABLE_SIZE; 521 | } 522 | 523 | //LOG("AngleLow %d, AngleHigh %d",angleLow,angleHigh); 524 | //LOG("TableLow %u, TableHigh %d",(uint16_t)table[indexLow].value,(uint16_t)table[indexHigh].value); 525 | y1=table[indexLow].value; 526 | y2=table[indexHigh].value; 527 | 528 | //handle the wrap condition 529 | if (abs(y2-y1)>CALIBRATION_STEPS/2) 530 | { 531 | if (y2=CALIBRATION_STEPS) 544 | { 545 | value=value-CALIBRATION_STEPS; 546 | } 547 | 548 | err=table[indexLow].error; 549 | if (table[indexHigh].error > err) 550 | { 551 | err=table[indexHigh].error; 552 | } 553 | 554 | if (table[indexLow].error == CALIBRATION_ERROR_NOT_SET || 555 | table[indexHigh].error == CALIBRATION_ERROR_NOT_SET) 556 | { 557 | err=CALIBRATION_ERROR_NOT_SET; 558 | } 559 | ptrData->value=value; 560 | ptrData->error=err; 561 | 562 | return 0; 563 | 564 | } 565 | 566 | Angle CalibrationTable::getCal(Angle actualAngle) 567 | { 568 | CalData_t data; 569 | getValue(actualAngle, &data); 570 | return data.value; 571 | } 572 | 573 | 574 | -------------------------------------------------------------------------------- /Firmware/stepper_nano_zero/A1333.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * Author: tstern 3 | * 4 | * Misfit Tech invests time and resources providing this open source code, 5 | * please support Misfit Tech and open-source hardware by purchasing 6 | * products from Misfit Tech, www.misifittech.net! 7 | * 8 | * Written by Trampas Stern for Misfit Tech. 9 | * BSD license, check license.txt for more information 10 | * All text above, must be included in any redistribution 11 | *********************************************************************/ 12 | #include 13 | #include "syslog.h" 14 | #include "a1333.h" 15 | #include "SPI.h" 16 | #include 17 | #include "board.h" 18 | 19 | 20 | 21 | #ifndef MKS_GCC_O0 //mks mark ("-O0") 22 | #pragma GCC push_options 23 | #pragma GCC optimize ("-Ofast") 24 | #endif 25 | 26 | #define A1333_CMD_NOP (0x0000) 27 | 28 | #define kNOERROR 0 29 | #define kPRIMARYREADERROR 1 30 | #define kEXTENDEDREADTIMEOUTERROR 2 31 | #define kPRIMARYWRITEERROR 3 32 | #define kEXTENDEDWRITETIMEOUTERROR 4 33 | 34 | //const uint16_t ChipSelectPin = 10; 35 | const uint16_t LEDPin = 13; 36 | 37 | const uint32_t WRITE = 0x40; 38 | const uint32_t READ = 0x00; 39 | const uint32_t COMMAND_MASK = 0xC0; 40 | const uint32_t ADDRESS_MASK = 0x3F; 41 | 42 | unsigned long nextTime; 43 | bool ledOn = false; 44 | 45 | bool includeCRC = false; 46 | 47 | /* 48 | * PrimaryRead 49 | * 50 | * Read from the primary serial registers 51 | */ 52 | uint16_t PrimaryRead(uint16_t cs, uint16_t address, uint16_t& value) 53 | { 54 | #if 0 55 | if (includeCRC) 56 | { 57 | uint8_t crcValue; 58 | uint8_t crcCommand; 59 | 60 | SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3)); 61 | 62 | // On the Teensy, SPI0_CTAR0 is used to describe the SPI transaction 63 | // for transfer (byte) while SPI0_CTAR1 is used to describe the SPI 64 | // transaction for transfer16. 65 | // To do a 20 bit transfer, change the length of the transaction to 66 | // 4 bits for transfer and do a transfer16 followed by a transfer. 67 | uint32_t oldSPI0_CTAR0 = SPI0_CTAR0; 68 | SPI0_CTAR0 = (SPI0_CTAR0 & 0x87FFFFFF) | SPI_CTAR_FMSZ(3); // using SPI0_CTAR0 69 | // to send 4 bits 70 | 71 | // Combine the register address and the command into one byte 72 | uint16_t command = ((address & ADDRESS_MASK) | READ) << 8; 73 | 74 | crcCommand = CalculateCRC(command); 75 | 76 | // take the chip select low to select the device 77 | digitalWrite(cs, LOW); 78 | 79 | // send the device the register you want to read 80 | SPI.transfer16(command); 81 | SPI.transfer(crcCommand); 82 | 83 | digitalWrite(cs, HIGH); 84 | digitalWrite(cs, LOW); 85 | 86 | // send the command again to read the contents 87 | value = SPI.transfer16(command); 88 | crcValue = SPI.transfer(crcCommand); 89 | 90 | // take the chip select high to de-select 91 | digitalWrite(cs, HIGH); 92 | 93 | // Restore the 8 bit description 94 | SPI0_CTAR0 = oldSPI0_CTAR0; 95 | 96 | SPI.endTransaction(); 97 | 98 | // Check the CRC value 99 | if (CalculateCRC(value) != crcValue) 100 | { 101 | return kCRCERROR; 102 | } 103 | } 104 | else 105 | #endif 106 | { 107 | //SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3)); 108 | 109 | // Combine the register address and the command into one byte 110 | uint16_t command = ((address & ADDRESS_MASK) | READ) << 8; 111 | 112 | // take the chip select low to select the device 113 | GPIO_LOW(PIN_A1333_CS); 114 | 115 | // send the device the register you want to read 116 | SPI.transfer16(command); 117 | 118 | GPIO_HIGH(PIN_A1333_CS); 119 | GPIO_LOW(PIN_A1333_CS); 120 | 121 | // send the command again to read the contents 122 | value = SPI.transfer16(command); 123 | 124 | // take the chip select high to de-select 125 | GPIO_HIGH(PIN_A1333_CS); 126 | 127 | SPI.endTransaction(); 128 | } 129 | 130 | return kNOERROR; 131 | } 132 | 133 | /* 134 | * PrimaryWrite 135 | * 136 | * Write to the primary serial registers 137 | */ 138 | uint16_t PrimaryWrite(uint16_t cs, uint16_t address, uint16_t value) 139 | { 140 | #if 0 141 | if (includeCRC) 142 | { 143 | SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3)); 144 | 145 | // On the Teensy, SPI0_CTAR0 is used to describe the SPI transaction 146 | // for transfer (byte) while SPI0_CTAR1 is used to describe the SPI 147 | // transaction for transfer16. 148 | // To do a 20 bit transfer, change the length of the transaction to 149 | // 4 bits for transfer and do a transfer16 followed by a transfer. 150 | uint32_t oldSPI0_CTAR0 = SPI0_CTAR0; 151 | SPI0_CTAR0 = (SPI0_CTAR0 & 0x87FFFFFF) | SPI_CTAR_FMSZ(3); // using SPI0_CTAR0 152 | // to send 4 bits 153 | 154 | // Combine the register address and the command into one byte 155 | uint16_t command = (((address & ADDRESS_MASK) | WRITE) << 8) | ((value >> 8) & 0x0FF); 156 | uint8_t crcCommand = CalculateCRC(command); 157 | 158 | // take the chip select low to select the device: 159 | digitalWrite(cs, LOW); 160 | 161 | SPI.transfer16(command); // Send most significant byte of register data 162 | SPI.transfer(crcCommand); // Send the crc 163 | 164 | // take the chip select high to de-select: 165 | digitalWrite(cs, HIGH); 166 | 167 | command = ((((address + 1) & ADDRESS_MASK) | WRITE) << 8 ) | (value & 0x0FF); 168 | crcCommand = CalculateCRC(command); 169 | 170 | // take the chip select low to select the device: 171 | digitalWrite(cs, LOW); 172 | 173 | SPI.transfer16(command); // Send least significant byte of register data 174 | SPI.transfer(crcCommand); // Send the crc 175 | 176 | // take the chip select high to de-select: 177 | digitalWrite(cs, HIGH); 178 | 179 | // Restore the 8 bit description 180 | SPI0_CTAR0 = oldSPI0_CTAR0; 181 | 182 | SPI.endTransaction(); 183 | } 184 | else 185 | #endif 186 | { 187 | //SPI.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE3)); 188 | 189 | // Combine the register address and the command into one byte 190 | uint16_t command = ((address & ADDRESS_MASK) | WRITE) << 8; 191 | 192 | // take the chip select low to select the device: 193 | //digitalWrite(cs, LOW); 194 | GPIO_LOW(PIN_A1333_CS); 195 | SPI.transfer16(command | ((value >> 8) & 0x0FF)); // Send most significant 196 | // byte of register data 197 | 198 | // take the chip select high to de-select: 199 | //digitalWrite(cs, HIGH); 200 | GPIO_HIGH(PIN_A1333_CS); 201 | command = (((address + 1) & ADDRESS_MASK) | WRITE) << 8; 202 | // take the chip select low to select the device: 203 | //digitalWrite(cs, LOW); 204 | GPIO_LOW(PIN_A1333_CS); 205 | SPI.transfer16(command | (value & 0x0FF)); // Send least significant byte 206 | // of register data 207 | 208 | // take the chip select high to de-select: 209 | //digitalWrite(cs, HIGH); 210 | GPIO_HIGH(PIN_A1333_CS); 211 | SPI.endTransaction(); 212 | } 213 | 214 | return kNOERROR; 215 | } 216 | 217 | /* 218 | * ExtendedRead 219 | * 220 | * Read from the EEPROM, Shadow or AUX 221 | */ 222 | uint16_t ExtendedRead(uint16_t cs, uint16_t address, uint32_t& value) 223 | { 224 | uint16_t results; 225 | uint16_t readFlags; 226 | uint32_t timeout; 227 | uint16_t valueMSW; 228 | uint16_t valueLSW; 229 | uint32_t currentTime; 230 | 231 | // Write the address to the Extended Read Address register 232 | results = PrimaryWrite(cs, 0x0A, address & 0xFFFF); 233 | 234 | if (results != kNOERROR) 235 | { 236 | return results; 237 | } 238 | 239 | // Initiate the extended read 240 | results = PrimaryWrite(cs, 0x0C, 0x8000); 241 | 242 | if (results != kNOERROR) 243 | { 244 | return results; 245 | } 246 | 247 | timeout = millis() + 100L; 248 | 249 | do // Wait for the read to be complete 250 | { 251 | results = PrimaryRead(cs, 0x0C, readFlags); 252 | 253 | if (results != kNOERROR) 254 | { 255 | return results; 256 | } 257 | 258 | // Make sure the read is not taking too long 259 | currentTime = millis(); 260 | if (timeout < currentTime) 261 | { 262 | return kEXTENDEDREADTIMEOUTERROR; 263 | } 264 | } while ((readFlags & 0x0001) != 0x0001); 265 | 266 | // Read the most significant word from the extended read data 267 | results = PrimaryRead(cs, 0x0E, valueMSW); 268 | 269 | if (results != kNOERROR) 270 | { 271 | return results; 272 | } 273 | 274 | // Read the least significant word from the extended read data 275 | results = PrimaryRead(cs, 0x10, valueLSW); 276 | 277 | // Combine them 278 | value = ((uint32_t)valueMSW << 16) + valueLSW; 279 | 280 | return results; 281 | } 282 | 283 | /* 284 | * ExtendedWrite 285 | * 286 | * Write to the EEPROM, Shadow or AUX 287 | */ 288 | uint16_t ExtendedWrite(uint16_t cs, uint16_t address, uint32_t value) 289 | { 290 | uint16_t results; 291 | uint16_t writeFlags; 292 | uint32_t timeout; 293 | 294 | // Write into the extended address register 295 | results = PrimaryWrite(cs, 0x02, address & 0xFFFF); 296 | 297 | if (results != kNOERROR) 298 | { 299 | return results; 300 | } 301 | 302 | // Write the MSW (Most significant word) into the high order write data register 303 | results = PrimaryWrite(cs, 0x04, (value >> 16) & 0xFFFF); 304 | 305 | if (results != kNOERROR) 306 | { 307 | return results; 308 | } 309 | 310 | // Write the LSW (Least significant word) into the low order write data register 311 | results = PrimaryWrite(cs, 0x06, value & 0xFFFF); 312 | 313 | if (results != kNOERROR) 314 | { 315 | return results; 316 | } 317 | 318 | // Start the write process 319 | results = PrimaryWrite(cs, 0x08, 0x8000); 320 | 321 | if (results != kNOERROR) 322 | { 323 | return results; 324 | } 325 | 326 | timeout = millis() + 100; 327 | 328 | // Wait for the write to complete 329 | do 330 | { 331 | results = PrimaryRead(cs, 0x08, writeFlags); 332 | 333 | if (results != kNOERROR) 334 | { 335 | return results; 336 | } 337 | 338 | if (timeout < millis()) 339 | { 340 | return kEXTENDEDWRITETIMEOUTERROR; 341 | } 342 | } while ((writeFlags & 0x0001) != 0x0001); 343 | 344 | return results; 345 | } 346 | 347 | /* 348 | * CalculateParity 349 | * 350 | * From the 16 bit input, calculate the parity 351 | */ 352 | bool CalculateParity(uint16_t input) 353 | { 354 | uint16_t count = 0; 355 | 356 | // Count up the number of 1s in the input 357 | for (int index = 0; index < 16; ++index) 358 | { 359 | if ((input & 1) == 1) 360 | { 361 | ++count; 362 | } 363 | 364 | input >>= 1; 365 | } 366 | 367 | // return true if there is an odd number of 1s 368 | return (count & 1) != 0; 369 | } 370 | 371 | /* 372 | * CalculateCRC 373 | * 374 | * Take the 16 bit input and generate a 4bit CRC 375 | * Polynomial = x^4 + x^1 + 1 376 | * LFSR preset to all 1's 377 | */ 378 | uint8_t CalculateCRC(uint16_t input) 379 | { 380 | bool CRC0 = true; 381 | bool CRC1 = true; 382 | bool CRC2 = true; 383 | bool CRC3 = true; 384 | int i; 385 | bool DoInvert; 386 | uint16_t mask = 0x8000; 387 | 388 | for (i = 0; i < 16; ++i) 389 | { 390 | DoInvert = ((input & mask) != 0) ^ CRC3; // XOR required? 391 | 392 | CRC3 = CRC2; 393 | CRC2 = CRC1; 394 | CRC1 = CRC0 ^ DoInvert; 395 | CRC0 = DoInvert; 396 | mask >>= 1; 397 | } 398 | 399 | return (CRC3 ? 8U : 0U) + (CRC2 ? 4U : 0U) + (CRC1 ? 2U : 0U) + (CRC0 ? 1U : 0U); 400 | } 401 | 402 | /* 403 | * SignExtendBitfield 404 | * 405 | * Sign extend a bitfield which is right justified 406 | */ 407 | int16_t SignExtendBitfield(uint16_t data, int width) 408 | { 409 | int32_t x = (int32_t)data; 410 | int32_t mask = 1L << (width - 1); 411 | 412 | x = x & ((1 << width) - 1); // make sure the upper bits are zero 413 | 414 | return (int16_t)((x ^ mask) - mask); 415 | } 416 | 417 | uint16_t angle; 418 | uint16_t angle15; 419 | uint16_t temperature; 420 | uint16_t fieldStrength; 421 | uint16_t turnsCount; 422 | 423 | volatile uint16_t angle_t; 424 | volatile uint16_t angle15_t; 425 | volatile uint16_t temperature_t; 426 | volatile uint16_t fieldStrength_t; 427 | volatile uint16_t turnsCount_t; 428 | 429 | 430 | void A1333_TEST() 431 | { 432 | 433 | // Every second, read the angle, temperature, field strength and turns count 434 | while(1) 435 | { 436 | if (nextTime < millis()) 437 | { 438 | if (PrimaryRead(PIN_A1333_CS, 0x20, angle) == kNOERROR) 439 | { 440 | if (CalculateParity(angle)) 441 | { 442 | angle_t = (float)(angle & 0x0FFF) * 360.0 / 4096.0; 443 | Serial.print("Angle = "); 444 | Serial.print((float)(angle & 0x0FFF) * 360.0 / 4096.0); 445 | Serial.println(" Degrees"); 446 | } 447 | else 448 | { 449 | Serial.println("Parity error on Angle read"); 450 | } 451 | } 452 | else 453 | { 454 | Serial.println("Unable to read Angle"); 455 | } 456 | 457 | if (PrimaryRead(PIN_A1333_CS, 0x32, angle15) == kNOERROR) 458 | { 459 | angle15_t = (float)((angle15) & 0x7FFF) * 360.0 / 32768.0; 460 | Serial.print("Angle = "); 461 | Serial.print((float)(angle15 & 0x7FFF) * 360.0 / 32768.0); 462 | Serial.println(" Degrees"); 463 | } 464 | else 465 | { 466 | Serial.println("Unable to read Angle15"); 467 | } 468 | 469 | 470 | if (PrimaryRead(PIN_A1333_CS, 0x28, temperature) == kNOERROR) 471 | { 472 | temperature_t = ((float)(temperature & 0x0FFF) / 8.0) + 25.0; 473 | Serial.print("Temperature = "); 474 | Serial.print(((float)(temperature & 0x0FFF) / 8.0) + 25.0); 475 | Serial.println(" C"); 476 | } 477 | else 478 | { 479 | Serial.println("Unable to read Temperature"); 480 | } 481 | 482 | if (PrimaryRead(PIN_A1333_CS, 0x2A, fieldStrength) == kNOERROR) 483 | { 484 | fieldStrength_t = fieldStrength & 0x0FFF; 485 | Serial.print("Field Strength = "); 486 | Serial.print(fieldStrength & 0x0FFF); 487 | Serial.println(" Gauss"); 488 | } 489 | else 490 | { 491 | Serial.println("Unable to read Field Strength"); 492 | } 493 | 494 | if (PrimaryRead(PIN_A1333_CS, 0x2C, turnsCount) == kNOERROR) 495 | { 496 | if (CalculateParity(turnsCount)) 497 | { 498 | turnsCount_t = SignExtendBitfield(turnsCount, 12); 499 | Serial.print("Turns Count = "); 500 | Serial.println(SignExtendBitfield(turnsCount, 12)); 501 | } 502 | else 503 | { 504 | Serial.println("Parity error on Turns Count read"); 505 | } 506 | } 507 | else 508 | { 509 | Serial.println("Unable to read Turns Count"); 510 | } 511 | 512 | nextTime = millis() + 500L; 513 | // Blink the LED every half second 514 | if (ledOn) 515 | { 516 | digitalWrite(LEDPin, LOW); 517 | ledOn = false; 518 | } 519 | else 520 | { 521 | digitalWrite(LEDPin, HIGH); 522 | ledOn = true; 523 | } 524 | 525 | 526 | } 527 | } 528 | } 529 | 530 | 531 | 532 | boolean A1333::begin(int csPin) 533 | { 534 | 535 | uint16_t unused=0; 536 | uint16_t flags=0; 537 | uint16_t angle=0; 538 | uint32_t flagsAndZeroOffset=0; 539 | 540 | digitalWrite(PIN_A1333_CS,LOW); //pull CS LOW by default (chip powered off) 541 | digitalWrite(PIN_MOSI,LOW); 542 | digitalWrite(PIN_SCK,LOW); 543 | digitalWrite(PIN_MISO,LOW); 544 | pinMode(PIN_MISO,OUTPUT); 545 | delay(1000); 546 | 547 | 548 | digitalWrite(PIN_A1333_CS,HIGH); //pull CS high 549 | 550 | pinMode(PIN_MISO,INPUT); 551 | 552 | error=false; 553 | SPISettings settingsA(5000000, MSBFIRST, SPI_MODE3); ///400000, MSBFIRST, SPI_MODE3); 554 | chipSelectPin=csPin; 555 | 556 | LOG("csPin is %d",csPin); 557 | pinMode(chipSelectPin,OUTPUT); 558 | digitalWrite(chipSelectPin,HIGH); //pull CS high by default 559 | delay(1); 560 | SPI.begin(); //A1333 SPI uses mode=3 (CPOL=1, CPHA=1) 561 | LOG("Begin A1333..."); 562 | 563 | SPI.beginTransaction(settingsA); 564 | SPI.transfer16(A1333_CMD_NOP); 565 | delay(10); 566 | 567 | // Make sure all of the SPI pins are 568 | // ready by doing a read 569 | PrimaryRead(PIN_A1333_CS, 0x0, unused); 570 | 571 | // Unlock the device 572 | PrimaryWrite(PIN_A1333_CS, 0x3C, 0x2700); 573 | PrimaryWrite(PIN_A1333_CS, 0x3C, 0x8100); 574 | PrimaryWrite(PIN_A1333_CS, 0x3C, 0x1F00); 575 | PrimaryWrite(PIN_A1333_CS, 0x3C, 0x7700); 576 | 577 | // Make sure the device is unlocked 578 | uint16_t t0=100; 579 | PrimaryRead(PIN_A1333_CS, 0x3C, flags); 580 | while ((flags & 0x0001) != 0x0001) 581 | { 582 | delay(1); 583 | t0--; 584 | if (t0==0) 585 | { 586 | ERROR("Device is not Unlocked"); 587 | error=true; 588 | a1333 = false; 589 | return false; 590 | } 591 | PrimaryRead(PIN_A1333_CS, 0x3C, flags); 592 | } 593 | 594 | #if 0 595 | // Zero the angle 596 | // Extended location 0x06 contains flags in the MSW and the Zero Angle values in the LSW 597 | // so get both and zero out ZeroAngle 598 | ExtendedRead(PIN_A1333_CS, 0x5C, flagsAndZeroOffset); 599 | flagsAndZeroOffset = flagsAndZeroOffset & 0x00FFF000; 600 | ExtendedWrite(PIN_A1333_CS, 0x5C, flagsAndZeroOffset); // Zero out the Shadow 601 | 602 | // Get the current angle. It is now without the ZeroAngle correction 603 | PrimaryRead(PIN_A1333_CS, 0x20, angle); 604 | 605 | 606 | // Copy the read angle into location 0x5C preserving the flags 607 | flagsAndZeroOffset = flagsAndZeroOffset | (angle & 0x000FFF); 608 | ExtendedWrite(PIN_A1333_CS, 0x5C, flagsAndZeroOffset); 609 | #endif 610 | 611 | //A1333_TEST(); 612 | 613 | return true; 614 | 615 | } 616 | 617 | 618 | 619 | 620 | //read the encoders 621 | int16_t A1333::readAddress(uint16_t addr) 622 | { 623 | uint16_t data = 0; 624 | 625 | PrimaryRead(PIN_A1333_CS, addr, data); 626 | 627 | return data; 628 | } 629 | 630 | //read the encoders 631 | int16_t A1333::readEncoderAngle(void) 632 | { 633 | uint16_t read_angle=0; 634 | if (a1333) 635 | { 636 | PrimaryRead(PIN_A1333_CS, 0x32, read_angle); 637 | } 638 | return (read_angle & 0x7FFF); 639 | } 640 | 641 | //pipelined read of the encoder angle used for high speed reads, but value is always one read behind 642 | int16_t A1333::readEncoderAnglePipeLineRead(void) 643 | { 644 | 645 | int16_t data = 0; 646 | return data; 647 | } 648 | 649 | 650 | void A1333::diagnostics(char *ptrStr) 651 | { 652 | 653 | } 654 | 655 | #pragma GCC pop_options 656 | 657 | --------------------------------------------------------------------------------