Showing preview only (1,335K chars total). Download the full file or copy to clipboard to get everything.
Repository: Misfittech/nano_stepper
Branch: master
Commit: ddaf3ad388c0
Files: 84
Total size: 1.3 MB
Directory structure:
gitextract_ktl3d9ev/
├── README.md
├── firmware/
│ └── stepper_nano_zero/
│ ├── .cproject
│ ├── .project
│ ├── A1333.cpp
│ ├── A1333.h
│ ├── A4954.cpp
│ ├── A4954.h
│ ├── A5995.cpp
│ ├── A5995.h
│ ├── Adafruit_GFX.cpp
│ ├── Adafruit_GFX.h
│ ├── Adafruit_SSD1306.cpp
│ ├── Adafruit_SSD1306.h
│ ├── Flash.cpp
│ ├── Flash.h
│ ├── angle.h
│ ├── as5047d.cpp
│ ├── as5047d.h
│ ├── board.h
│ ├── calibration.cpp
│ ├── calibration.h
│ ├── command.cpp
│ ├── command.h
│ ├── commands.cpp
│ ├── commands.h
│ ├── eeprom.cpp
│ ├── eeprom.h
│ ├── fet_driver.cpp
│ ├── fet_driver.h
│ ├── ftoa.cpp
│ ├── ftoa.h
│ ├── gfxfont.h
│ ├── glcdfont.c
│ ├── nonvolatile.cpp
│ ├── nonvolatile.h
│ ├── nzs.cpp
│ ├── nzs.h
│ ├── nzs_lcd.cpp
│ ├── nzs_lcd.h
│ ├── planner.cpp
│ ├── planner.h
│ ├── sine.cpp
│ ├── sine.h
│ ├── stepper_controller.cpp
│ ├── stepper_controller.h
│ ├── stepper_nano_zero.ino
│ ├── steppin.cpp
│ ├── steppin.h
│ ├── syslog.cpp
│ ├── syslog.h
│ ├── utils.cpp
│ └── utils.h
└── hardware/
├── NZS_A4954_R2.0.PrjPcb
├── NZS_A4954_R2_0.PcbDoc
├── Nano_stepper.SchDoc
├── Status Report.Txt
├── panel-RoundHoles.TXT
├── panel-SlotHoles.TXT
├── panel-macro.APR_LIB
├── panel.1
├── panel.DRR
├── panel.EXTREP
├── panel.GBL
├── panel.GBO
├── panel.GBP
├── panel.GBS
├── panel.GD1
├── panel.GKO
├── panel.GM1
├── panel.GM13
├── panel.GM15
├── panel.GM2
├── panel.GM3
├── panel.GML
├── panel.GPB
├── panel.GPT
├── panel.GTL
├── panel.GTO
├── panel.GTP
├── panel.GTS
├── panel.LDP
├── panel.REP
├── panel.RUL
└── panel.apr
================================================
FILE CONTENTS
================================================
================================================
FILE: README.md
================================================
# Smart Stepper (also known as the nano zero stepper)
Firmware to turn a stepper motor into servo motor: see http://misfittech.net for hardware!
If you have a MKS Servo42, this firmware is not fully tested. MKS has taken this firmware but has not provided source back and hardware designs back to the community. Test submissions are appreciated. __Test at you own RISK!__
If you want to support the work on the firmware and hardware consider buying hardware from www.misfittech.net or buying me a beer using the donation button.
[](https://www.paypal.com/cgi-bin/webscr?cmd=_s-xclick&hosted_button_id=4JAEK4G24W2U4)
# How to Install
[Hardware install and manual](https://github.com/Misfittech/nano_stepper/raw/master/documentation/Smart%20Stepper%20Manual.pdf)
[Arduino install](http://misfittech.net/blog/arduino-package-install/) for building firmware
[Further Details and to purchase Hardware](http://misfittech.net/smart-steppers/)
# Google groups forum
[Google groups forum](https://groups.google.com/forum/#!forum/smart-stepper)
# Command List
## Smart Stepper Command Line Interface
The smart stepper uses a command line interface where the prompt is “:>”
### help
The help command will return a list of commands that the smart stepper supports.
### getcal
This command will print out the 200 point calibration table. This is useful if you are doing firmware development and do not want to calibrate each time you update firmware. You can take this table and copy it into the nonvolatile.cpp file as shown below
### calibrate
This will run a 200 point calibration of the encoder.
### testcal
This will test the calibration and report the maximum error in degrees.
### microsteps
This command gets/sets the number of microsteps the smart stepper will use for the step command and the step pin. The number of microsteps does not affect the resolution of the controller but rather how fine you can set the position.
### step
This will move the motor one step clockwise, the step size is based on the current microstep setting. To move the motor counterclockwise use “step 1”. To move the motor clockwise 16 steps used “step 0 16” to move motor counterclockwise 16 steps use “step 1 16”
### feedback
This commands disable/enables feedback control loop.
The plan is to discountinue this command in the future and use the “controlmode” command to put controller in open or one of the many closed loop operational modes.
### readpos
Reads the current motor position and reports it as degrees.
### encoderdiag
This command will read and report the AS5047D internal registers for diagnostic purposes.
### spid
This command sets the Kp, Ki, and Kd terms for the simple positional PID controller.
### ppid
This command sets the Kp, Ki, and Kd terms for the positional PID controller.
### vpid
This command sets the Kp, Ki, and Kd terms for the velocity PID controller.
### velocity
This sets the velocity to rotate motor when unit is configured for velocity PID mode of operation.
### boot
This command will put the microprocessor in the boot loader mode. Alternatively this can be done by double pressing the reset button.
### factoryreset
This erases the calibration and other system and motor parameters such that unit is reset to the factory ship state. After this command the unit will need to be calibrated to the motor again.
### dirpin
This command sets which direction the motor will rotate when direction pin is pulled high. The direction pin is only sampled when the step pin has a rising edge.
‘dirpin 0’ will set the motor to rotate clockwise when dir pin is high
‘dirpin 1’ will set the motor to rotate counter-clockwise when dir pin is high
### errorlimit
Gets set the maximum number of degrees of error that is acceptable, any posistioning error about the error limit will assert the error pin, when error pin is set as error output.
For example:
~~~~
:>errorlimit 1.8
~~~~
Will set the error limit to 1.8 degrees.
### ctrlmode
Gets/Sets the feedback controller mode of operation. The command takes an integer from 0 through 4 to set the control mode per table below:
* Controller off - 0 -- this is not currently used
* Open-Loop - 1 -- this is open loop with no feedback
* Simple PID - 2 -- simple positional PID, which is factory default
* Positional PID - 3 -- current based PID mode, requires tuning for your machine
* Velocity PID - 4 -- velocity based PID, requires tuning for your machine and speed range
If you are unsure what you are doing leave unit in the Simple PID mode of operation.
### maxcurrent
This sets the maximum current that will be pushed into the motor. To set the current for maximum of 2.0A you would use command “maxcurrent 2000” as the argument is in milliAmps.
### holdcurrent
For the Simple Positional PID mode the minimal current (ie current with no positional error) is the hold current. You set this current based on the required holding torque you need for your application. The higher the hold current, the hotter and noisier the motor will be but also the larger the holding torque.
For the Positional PID mode the PID tuning params have to be set correctly such that the control loop will dynamically determine the holding torque. This tuning of the PID can be difficult, hence the simple PID mode will work most of the time out of the box by setting maximum current and holding current.
### motorwiring
The firmware always uses a positive angle as a clockwise rotation. A stepper motor however could have wiring done with one coil reversed wired, which will cause motor to normally operate in opposite direction. The Smart Stepper firmware will detect the motor wiring direction, using the encoder, and the firmware will compensate for a reverse wired motor. The reverse or forward wiring of a motor is detected on first power up after factory reset. If the wiring changes after that you can compensate using this command.
HOWEVER it is better to do a factory reset and recalibrate motor if wiring changes.
### stepsperrotation
The Smart Stepper firmware will with first power on after factory reset detect the number of full steps per rotation for the stepper motor and store in flash memory. This command will read this parameter from flash and allow user to change this parameter if motor is changed.
HOWEVER it is better to do a factory reset and recalibrate motor if motorchanges.
### move
The move command will request the motor to move to an absolute angle position. Optionally the user can specify the rotational speed (RPMs) by which the move should happen. For example if the current motor position is at angle 0 and you issue ‘move 3600” the motor will turn 10 rotations clockwise to angle of 3600 degrees. If issue the ‘move 3600’ again nothing will happen as motor is already at angle 3600.
If motor is at angle 0 and user issues the command ‘move 3600 20’ then motor will move to 10 rotations clockwise to angle of 3600 at a rate of 20 RPMs.
### stop
If user issues a move command that takes a long time and wants to stop the move before completion then user can issue the stop command command which will stop a move operation.
### setzero
This command will take the current motor position and set it to absolute angle of zero. Note that if you are in the middle move it will take the position at the time of the command and use it, thus it is recommend a move be stopped or wait for completion before issuing the setzero.
### data
This command will toggle output of binary data.
### looptime
This command will display the time it takes for a single processing loop to execute.
### eepromerror
This command displays the motor error in degrees difference from the stored eeprom value at motor power up.
### eepromloc
Displays the location of the shaft angle in degrees at motor power on.
### eepromwrite
Forces the eeprom to store all current values in ram to eeprom.
### eepromsetloc
Forces the eeprom to write the current shaft angle overwriting the stored location from powerup.
### setpos
Overwrites the current shaft angle in the motion planner.
### reboot
Forces the smart stepper to reboot
### homecurrent
If using built in homing routine (command "home") this will specify the amount of current applied when motor is moving during homing operation when homepin is logic active.__EXPERIMENTAL USE WITH CAUTION__
### homepin
Allows setting of pin for current limited enable for homing. This triggers a current drop during homing movements. Current set using command "homecurrent". This pin is pulled low to activate. __EXPERIMENTAL USE WITH CAUTION__
### homeangledelay
Currently unused.
### home
Tells the motion controller to move motor until the home switch (enable pin) is pulled low. (Only on boards 3/21/2017 or newer) (Must be enabled in firmware).
For example:
~~~~
:>home 360 0.5
~~~~
Will move up to 360 degrees at 0.5 RPM. __EXPERIMENTAL USE WITH CAUTION__
### pinread
Displays the binary states of all pins (Step, Dir, Enable, Error, A3, TX, RX)
### errorpinmode
Sets or displays the error pin mode. Allows someone to swap usage of the error pin as an enable pin on older boards. (Not compiled for use on boards 3/21/2017 or newer since they have separate enable and error pins) (Must be enabled in firmware)
Modes are:
* "0" - Enable mode, active high (digital input).
* "1" - Enable mode, active low (digital input).
* "2" - Error mode, active low (digital output). Active level is reached when there is an angle error.
* "3" - Error mode, bi-directional, (digital input/output open collector). (Not currently used).
### errorpin
Sets or displays the binary state of the enable pin. Acceptable values are 0 or 1
For example:
~~~~
:>errorpin 1
~~~~
Will set the error pin on the terminal block to output a logic high when the error level is reached
### enablepinmode
Sets or displays the enable pin mode. Allows someone to swap usage of the enable pin as an error pin on older boards. (Only on boards 3/21/2017 or newer since they have separate enable and error pins) (Must be enabled in firmware)
Modes are:
* "0" - Enable mode, active high (digital input).
* "1" - Enable mode, active low (digital input).
* "2" - Error mode, active low (digital output). Active level is reached when there is an angle error.
* "3" - Error mode, bi-directional, (digital input/output open collector). (Not currently used).
### geterror
Displays the current motor shaft error in degrees.
### getsteps
Displays the number of steps that have been seen from the DIR pin.
### debug
Sets if syslog debugging will be output on USB serial. Allowed values are 0 for disable, 1 for enable.
## License:
The smart stepper related hardware is released under the Creative Commons Attribution Share-Alike 4.0 License as much of the work is based on Mechaduino project by J. Church.
https://github.com/jcchurch13/Mechaduino-Firmware
The firmware 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.
================================================
FILE: firmware/stepper_nano_zero/.cproject
================================================
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.cross.base.1504376676">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.1504376676" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings>
<externalSetting>
<entry flags="RESOLVED" kind="includePath" name="C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\variants\nano_zero"/>
<entry flags="RESOLVED" kind="includePath" name="C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\cores\arduino"/>
</externalSetting>
</externalSettings>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="${ProjName}" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.cross.base.1504376676" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.1504376676.350447049" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.cross.base.515513271" name="Cross GCC" superClass="cdt.managedbuild.toolchain.gnu.cross.base">
<option id="cdt.managedbuild.option.gnu.cross.prefix.582370251" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"/>
<option id="cdt.managedbuild.option.gnu.cross.path.231717393" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"/>
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.1339435354" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
<builder id="cdt.managedbuild.builder.gnu.cross.241627781" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.builder.gnu.cross">
<outputEntries/>
</builder>
<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.1805317535" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
<option id="gnu.c.compiler.option.include.paths.1641555759" name="Include paths (-I)" superClass="gnu.c.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\variants\nano_zero""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit_SSD1306-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit-GFX-Library-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\pio""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\instance""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\component""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\cores\arduino""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL""/>
<listOptionValue builtIn="false" value=""C:\Users\TSTERN\AppData\Local\Arduino15\packages\arduino\hardware\samd\1.6.6\libraries\SPI""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\CMSIS\include""/>
</option>
<option id="gnu.c.compiler.option.preprocessor.def.symbols.77778103" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
<listOptionValue builtIn="false" value="ARDUINO_SAMD_ZERO"/>
<listOptionValue builtIn="false" value="__SAMD21G18A__"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1328245026" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.1543385682" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
<option id="gnu.cpp.compiler.option.include.paths.784350866" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" useByScannerDiscovery="false" valueType="includePath">
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\cores\arduino""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\variants\nano_zero""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit_SSD1306-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit-GFX-Library-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\pio""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\instance""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\component""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL""/>
<listOptionValue builtIn="false" value=""C:\Users\TSTERN\AppData\Local\Arduino15\packages\arduino\hardware\samd\1.6.6\libraries\SPI""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\CMSIS\include""/>
</option>
<option id="gnu.cpp.compiler.option.preprocessor.def.215333140" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" useByScannerDiscovery="false" valueType="definedSymbols">
<listOptionValue builtIn="false" value="__SAMD21G18A__"/>
<listOptionValue builtIn="false" value="ARDUINO_SAMD_ZERO"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1198492994" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.1644022969" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.1373326859" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
<option id="gnu.cpp.link.option.paths.1624146493" name="Library search path (-L)" superClass="gnu.cpp.link.option.paths"/>
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1257879712" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.archiver.722380566" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
<tool id="cdt.managedbuild.tool.gnu.cross.assembler.7579640" name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
<option id="gnu.both.asm.option.include.paths.2128933212" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\variants\nano_zero""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit_SSD1306-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\Documents\Arduino\libraries\Adafruit-GFX-Library-master""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\pio""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\instance""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL\samd21\include\component""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\misfittech\hardware\samd\1.0.0\cores\arduino""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\Device\ATMEL""/>
<listOptionValue builtIn="false" value=""C:\Users\TSTERN\AppData\Local\Arduino15\packages\arduino\hardware\samd\1.6.6\libraries\SPI""/>
<listOptionValue builtIn="false" value=""C:\Users\tstern\AppData\Local\Arduino15\packages\arduino\tools\CMSIS\4.0.0-atmel\CMSIS\Include""/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.474088881" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
</toolChain>
</folderInfo>
<sourceEntries>
<entry excluding="Adafruit-GFX-Library-master|Adafruit_SSD1306-master" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="ilg.gnuarmeclipse.managedbuild.packs"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="NZS.null.440051529" name="NZS"/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1504376676;cdt.managedbuild.toolchain.gnu.cross.base.1504376676.350447049;cdt.managedbuild.tool.gnu.cross.c.compiler.1805317535;cdt.managedbuild.tool.gnu.c.compiler.input.1328245026">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1504376676;cdt.managedbuild.toolchain.gnu.cross.base.1504376676.350447049;cdt.managedbuild.tool.gnu.cross.cpp.compiler.1543385682;cdt.managedbuild.tool.gnu.cpp.compiler.input.1198492994">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
</cproject>
================================================
FILE: firmware/stepper_nano_zero/.project
================================================
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>NZS</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
<linkedResources>
<link>
<name>arduino</name>
<type>2</type>
<location>C:/Users/tramp_000/AppData/Local/Arduino15/packages/arduino/hardware/samd/1.6.8/cores/arduino</location>
</link>
</linkedResources>
</projectDescription>
================================================
FILE: firmware/stepper_nano_zero/A1333.cpp
================================================
/**********************************************************************
Copyright (C) 2019 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include <Arduino.h>
#include "syslog.h"
#include "A1333.h"
#include "SPI.h"
#include <stdio.h>
#include "board.h"
#define A1333_CMD_NOP (0x0000)
#define A1333_ANG15 (0x3200)
SPISettings settingsA(500000, MSBFIRST, SPI_MODE3); ///400000, MSBFIRST, SPI_MODE1);
boolean A1333::begin(int csPin)
{
digitalWrite(PIN_AS5047D_CS,LOW); //pull CS LOW by default (chip powered off)
digitalWrite(PIN_MOSI,LOW);
digitalWrite(PIN_SCK,LOW);
digitalWrite(PIN_MISO,LOW);
pinMode(PIN_MISO,OUTPUT);
delay(1000);
digitalWrite(PIN_AS5047D_CS,HIGH); //pull CS high
pinMode(PIN_MISO,INPUT);
chipSelectPin=csPin;
LOG("csPin is %d",csPin);
pinMode(chipSelectPin,OUTPUT);
digitalWrite(chipSelectPin,HIGH); //pull CS high by default
delay(1);
SPI.begin(); //AS5047D SPI uses mode=1 (CPOL=0, CPHA=1)
LOG("Begin A1333...");
LOG("Address is 0x%04X",readAddress(A1333_ANG15));
}
//read the encoders
int16_t A1333::readAddress(uint16_t addr)
{
uint16_t data;
//make sure it is a write by setting bit 14
//addr=addr | 0x4000;
SPI.beginTransaction(settingsA);
digitalWrite(chipSelectPin, LOW);
delayMicroseconds(1);
//clock out the address to read
//LOG("address 0x%04X",addr);
SPI.transfer16(addr);
digitalWrite(chipSelectPin, HIGH);
delayMicroseconds(1);
digitalWrite(chipSelectPin, LOW);
//clock out zeros to read in the data from address
data=SPI.transfer16(0x00);
digitalWrite(chipSelectPin, HIGH);
SPI.endTransaction();
return data;
}
//read the encoders
int16_t A1333::readEncoderAngle(void)
{
return readAddress(A1333_ANG15)>>1;
}
int16_t A1333::readEncoderAnglePipeLineRead(void)
{
static bool first=true;
uint16_t addr = A1333_ANG15;
uint16_t addr2;
uint16_t data;
if (first)
{
//make sure it is a write by setting bit 14
//addr2=addr | 0x4000;
SPI.beginTransaction(settingsA);
digitalWrite(chipSelectPin, LOW);
delayMicroseconds(1);
//clock out the address to read
SPI.transfer16(addr);
digitalWrite(chipSelectPin, HIGH);
delayMicroseconds(1);
digitalWrite(chipSelectPin, LOW);
delayMicroseconds(1);
//clock out zeros to read in the data from address
data=SPI.transfer16(addr);
digitalWrite(chipSelectPin, HIGH);
SPI.endTransaction();
first=false;
return data>>1;
}
SPI.beginTransaction(settingsA);
digitalWrite(chipSelectPin, LOW);
delayMicroseconds(1);
//clock out zeros to read in the data from address
data=SPI.transfer16(addr);
digitalWrite(chipSelectPin, HIGH);
SPI.endTransaction();
return data>>1;
}
================================================
FILE: firmware/stepper_nano_zero/A1333.h
================================================
/**********************************************************************
Copyright (C) 2019 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef A1333_H_
#define A1333_H_
#include <Arduino.h>
#define A1333_DEGREES_PER_BIT (360.0/(float)(0x7FFF))
class A1333 {
private:
int chipSelectPin;
public:
boolean begin(int csPin);
int16_t readEncoderAngle(void);
int16_t readAddress(uint16_t addr);
int16_t readEncoderAnglePipeLineRead(void);
void diagnostics(char *ptrStr) {return;};
bool getError(void) {return false;};
};
#endif /* A1333_H_ */
================================================
FILE: firmware/stepper_nano_zero/A4954.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "A4954.h"
#include "wiring_private.h"
#include "syslog.h"
#include "angle.h"
#include "Arduino.h"
#include "sine.h"
static uint8_t pinState=0;
#pragma GCC push_options
#pragma GCC optimize ("-Ofast")
#define DAC_MAX (0x01FFL)
// Wait for synchronization of registers between the clock domains
static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
static void syncTCC(Tcc* TCCx) {
//int32_t t0=1000;
while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK)
{
// t0--;
// if (t0==0)
// {
// break;
// }
// delay(1);
}
}
static inline void bridge1(int state)
{
if (state==0)
{
PORT->Group[g_APinDescription[PIN_A4954_IN1].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN1].ulPin].bit.PMUXEN = 0;
GPIO_OUTPUT(PIN_A4954_IN1);//pinMode(PIN_A4954_IN1,OUTPUT);
GPIO_OUTPUT(PIN_A4954_IN2);//pinMode(PIN_A4954_IN2,OUTPUT);
GPIO_HIGH(PIN_A4954_IN1);// digitalWrite(PIN_A4954_IN1, HIGH);
GPIO_LOW(PIN_A4954_IN2);//digitalWrite(PIN_A4954_IN2, LOW);
//pinPeripheral(PIN_A4954_IN2, PIO_TIMER_ALT);
pinState=(pinState & 0x0C) | 0x1;
}
if (state==1)
{
PORT->Group[g_APinDescription[PIN_A4954_IN2].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN2].ulPin].bit.PMUXEN = 0;
GPIO_OUTPUT(PIN_A4954_IN2);//pinMode(PIN_A4954_IN2,OUTPUT);
GPIO_OUTPUT(PIN_A4954_IN1);pinMode(PIN_A4954_IN1,OUTPUT);
GPIO_LOW(PIN_A4954_IN1);//digitalWrite(PIN_A4954_IN1, LOW);
GPIO_HIGH(PIN_A4954_IN2);//digitalWrite(PIN_A4954_IN2, HIGH);
//pinPeripheral(PIN_A4954_IN1, PIO_TIMER);
pinState=(pinState & 0x0C) | 0x2;
}
if (state==3)
{
GPIO_LOW(PIN_A4954_IN1);
GPIO_LOW(PIN_A4954_IN2);
//digitalWrite(PIN_A4954_IN1, LOW);
//digitalWrite(PIN_A4954_IN2, LOW);
}
}
static inline void bridge2(int state)
{
if (state==0)
{
PORT->Group[g_APinDescription[PIN_A4954_IN3].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN3].ulPin].bit.PMUXEN = 0;
GPIO_OUTPUT(PIN_A4954_IN3); //pinMode(PIN_A4954_IN3,OUTPUT);
GPIO_OUTPUT(PIN_A4954_IN4);//pinMode(PIN_A4954_IN4,OUTPUT);
GPIO_HIGH(PIN_A4954_IN3);//digitalWrite(PIN_A4954_IN3, HIGH);
GPIO_LOW(PIN_A4954_IN4);//digitalWrite(PIN_A4954_IN4, LOW);
//pinPeripheral(PIN_A4954_IN4, PIO_TIMER_ALT);
pinState=(pinState & 0x03) | 0x4;
}
if (state==1)
{
PORT->Group[g_APinDescription[PIN_A4954_IN4].ulPort].PINCFG[g_APinDescription[PIN_A4954_IN4].ulPin].bit.PMUXEN = 0;
GPIO_OUTPUT(PIN_A4954_IN4);//pinMode(PIN_A4954_IN4,OUTPUT);
GPIO_OUTPUT(PIN_A4954_IN3);//pinMode(PIN_A4954_IN3,OUTPUT);
GPIO_LOW(PIN_A4954_IN3);//digitalWrite(PIN_A4954_IN3, LOW);
GPIO_HIGH(PIN_A4954_IN4);//digitalWrite(PIN_A4954_IN4, HIGH);
//pinPeripheral(PIN_A4954_IN3, PIO_TIMER_ALT);
pinState=(pinState & 0x03) | 0x8;
}
if (state==3)
{
GPIO_LOW(PIN_A4954_IN3);
GPIO_LOW(PIN_A4954_IN4);
//digitalWrite(PIN_A4954_IN3, LOW);
//digitalWrite(PIN_A4954_IN4, LOW);
}
}
static void enableTCC0(uint8_t percent)
{
#ifdef MECHADUINO_HARDWARE
return;
#else
Tcc* TCCx = TCC0 ;
uint32_t ulValue=((uint32_t)(100-percent)*480)/100;
//ERROR("Enable TCC0");
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ;
//ERROR("Setting TCC %d %d",ulValue,ulPin);
TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE;
syncTCC(TCCx);
// Set TCx as normal PWM
TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM;
syncTCC(TCCx);
// Set TCx in waveform mode Normal PWM
TCCx->CC[1].reg = (uint32_t)ulValue; //ch5 //IN3
syncTCC(TCCx);
TCCx->CC[2].reg = (uint32_t)ulValue; //ch6 //IN4
syncTCC(TCCx);
TCCx->CC[3].reg = (uint32_t)ulValue; //ch7 //IN2
syncTCC(TCCx);
TCCx->CC[1].reg = (uint32_t)ulValue; //ch1 == ch5 //IN1
syncTCC(TCCx);
// Set PER to maximum counter value (resolution : 0xFF)
TCCx->PER.reg = DAC_MAX;
syncTCC(TCCx);
// Enable TCCx
TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ;
syncTCC(TCCx);
//ERROR("Enable TCC0 DONE");
#endif
}
static void setDAC(uint32_t DAC1, uint32_t DAC2)
{
TCC1->CC[1].reg = (uint32_t)DAC1; //D9 PA07 - VREF12
syncTCC(TCC1);
TCC1->CC[0].reg = (uint32_t)DAC2; //D4 - VREF34
syncTCC(TCC1);
}
static void setupDAC(void)
{
Tcc* TCCx = TCC1 ;
pinPeripheral(PIN_A4954_VREF34, PIO_TIMER_ALT);
pinPeripheral(PIN_A4954_VREF12, PIO_TIMER);
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ;
//ERROR("Setting TCC %d %d",ulValue,ulPin);
TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE;
syncTCC(TCCx);
// Set TCx as normal PWM
TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM;
syncTCC(TCCx);
// Set TCx in waveform mode Normal PWM
TCCx->CC[1].reg = (uint32_t)0;
syncTCC(TCCx);
TCCx->CC[0].reg = (uint32_t)0;
syncTCC(TCCx);
// Set PER to maximum counter value (resolution : 0xFFF = 12 bits)
// =48e6/2^12=11kHz frequency
TCCx->PER.reg = DAC_MAX;
syncTCC(TCCx);
// Enable TCCx
TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ;
syncTCC(TCCx);
}
void A4954::begin()
{
//setup the A4954 pins
digitalWrite(PIN_A4954_IN3,LOW);
pinMode(PIN_A4954_IN3,OUTPUT);
digitalWrite(PIN_A4954_IN4,LOW);
pinMode(PIN_A4954_IN4,OUTPUT);
digitalWrite(PIN_A4954_IN2,LOW);
pinMode(PIN_A4954_IN2,OUTPUT);
digitalWrite(PIN_A4954_IN1,LOW);
pinMode(PIN_A4954_IN1,OUTPUT);
//setup the PWM for current on the A4954, set for low current
digitalWrite(PIN_A4954_VREF12,LOW);
digitalWrite(PIN_A4954_VREF34,LOW);
pinMode(PIN_A4954_VREF34, OUTPUT);
pinMode(PIN_A4954_VREF12, OUTPUT);
enabled=true;
lastStepMicros=0;
forwardRotation=true;
enableTCC0(90);
setupDAC();
//
// int i=0;
// bridge1(0);
// bridge2(0);
//while (1)
// {
// int32_t x;
// WARNING("MA %d",i);
// x=(int32_t)((int64_t)i*(DAC_MAX))/3300;
// setDAC(x,x);
// delay(1000);
// i=i+10;
// if (i>1000)
// {
// i=0;
// }
//
// }
//
// WARNING("Setting DAC for 500mA output");
// setDAC((int32_t)((int64_t)1000*(DAC_MAX))/3300,(int32_t)((int64_t)1000*(DAC_MAX))/3300);
// bridge1(0);
// bridge2(0);
// while(1)
// {
//
// }
return;
}
void A4954::limitCurrent(uint8_t percent)
{
#ifdef MECHADUINO_HARDWARE
return;
#else
//WARNING("current limit %d",percent);
enableTCC0(percent);
if (pinState & 0x01)
{
pinPeripheral(PIN_A4954_IN2, PIO_TIMER_ALT); //TCC0 WO[7]
}
if (pinState & 0x02)
{
pinPeripheral(PIN_A4954_IN1, PIO_TIMER); //TCC0 WO[1]
}
if (pinState & 0x04)
{
pinPeripheral(PIN_A4954_IN4, PIO_TIMER_ALT);
}
if (pinState & 0x08)
{
pinPeripheral(PIN_A4954_IN3, PIO_TIMER_ALT);
}
#endif
}
void A4954::enable(bool enable)
{
enabled=enable;
if (enabled == false)
{
WARNING("A4954 disabled");
setDAC(0,0); //turn current off
bridge1(3); //tri state bridge outputs
bridge2(3); //tri state bridge outputs
}
}
//this is precise move and modulo of A4954_NUM_MICROSTEPS is a full step.
// stepAngle is in A4954_NUM_MICROSTEPS units..
// The A4954 has no idea where the motor is, so the calling function has to
// to tell the A4954 what phase to drive motor coils.
// A4954_NUM_MICROSTEPS is 256 by default so stepAngle of 1024 is 360 degrees
// Note you can only move up to +/-A4954_NUM_MICROSTEPS from where you
// currently are.
int32_t A4954::move(int32_t stepAngle, uint32_t mA)
{
uint16_t angle;
int32_t cos,sin;
int32_t dacSin,dacCos;
//static int i=0;
if (enabled == false)
{
//WARNING("A4954 disabled");
setDAC(0,0); //turn current off
bridge1(3); //tri state bridge outputs
bridge2(3); //tri state bridge outputs
return stepAngle;
}
//WARNING("move %d %d",stepAngle,mA);
//handle roll overs, could do with modulo operator
stepAngle=stepAngle%SINE_STEPS;
//figure out our sine Angle
// note our SINE_STEPS is 4x of microsteps for a reason
//angle=(stepAngle+(SINE_STEPS/8)) % SINE_STEPS;
angle=(stepAngle);
//calculate the sine and cosine of our angle
sin=sine(angle);
cos=cosine(angle);
//if we are reverse swap the sign of one of the angels
if (false == forwardRotation)
{
cos=-cos;
}
//scale sine result by current(mA)
dacSin=((int32_t)mA*(int64_t)abs(sin))/SINE_MAX;
//scale cosine result by current(mA)
dacCos=((int32_t)mA*(int64_t)abs(cos))/SINE_MAX;
// if (i==0)
// {
// WARNING("dacs are %d %d",dacSin,dacCos);
// }
//convert value into DAC scaled to 3300mA max
dacCos=(int32_t)((int64_t)dacCos*(DAC_MAX))/3300;
//convert value into DAC scaled to 3300mA max
dacSin=(int32_t)((int64_t)dacSin*(DAC_MAX))/3300;
//WARNING("dacs are %d %d ",dacSin,dacCos);
setDAC(dacSin,dacCos);
if (sin>0)
{
bridge1(1);
}else
{
bridge1(0);
}
if (cos>0)
{
bridge2(1);
}else
{
bridge2(0);
}
// if (i++>3000)
// {
// i=0;
// }
// YELLOW_LED(led);
// led=(led+1) & 0x01;
lastStepMicros=micros();
return stepAngle;
}
#pragma GCC pop_options
================================================
FILE: firmware/stepper_nano_zero/A4954.h
================================================
/**********************************************************************
Copyright (C) 2019 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef __A4954__H__
#define __A4954__H__
#include <Arduino.h>
#include "board.h"
#include "angle.h"
#include "sine.h"
#define A4954_NUM_MICROSTEPS (256)
#define A4954_MIN_TIME_BETWEEN_STEPS_MICROS (1000)
//prevent someone for making a mistake with the code
#if ((A4954_NUM_MICROSTEPS*4) != SINE_STEPS)
#error "SINE_STEPS must be 4x of Micro steps for the move function"
#endif
/*
* When it comes to the stepper driver if we use angles
* we will always have a rounding error. For example
* a 0-65536(360) angle for 1.8 degree step is 327.68 so
* if you increment 200 of these as 327 you have a 13.6 error
* after one rotation.
* If you use floating point the effect is the same but takes longer.
*
* The only error-less accumulation system is to use native units, ie full
* steps and microsteps.
*
*/
class A4954
{
private:
uint32_t lastStepMicros; // time in microseconds that last step happened
bool forwardRotation=true;
volatile bool enabled=true;
public:
void begin(void);
//moves motor where the modulo of A4954_NUM_MICROSTEPS is a full step.
int32_t move(int32_t stepAngle, uint32_t mA);
uint32_t microsSinceStep(void) {return micros()-lastStepMicros;};
void setRotationDirection(bool forward) {forwardRotation=forward;};
void enable(bool enable);
void limitCurrent(uint8_t percent); //higher more current
};
#endif //__A4954__H__
================================================
FILE: firmware/stepper_nano_zero/A5995.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "A5995.h"
#include "wiring_private.h"
#include "syslog.h"
#include "angle.h"
#include "Arduino.h"
#include "sine.h"
static uint8_t pinState=0;
#pragma GCC push_options
#pragma GCC optimize ("-Ofast")
#define DAC_MAX (0x01FFL)
// Wait for synchronization of registers between the clock domains
static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
static void syncTCC(Tcc* TCCx) {
//int32_t t0=1000;
while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK)
{
// t0--;
// if (t0==0)
// {
// break;
// }
// delay(1);
}
}
static void setDAC(uint32_t DAC1, uint32_t DAC2)
{
TCC1->CC[1].reg = (uint32_t)DAC1; //D9 PA07 - VREF2
syncTCC(TCC1);
TCC1->CC[0].reg = (uint32_t)DAC2; //D4 - VREF1
syncTCC(TCC1);
}
static void setupDAC(void)
{
Tcc* TCCx = TCC1 ;
pinPeripheral(PIN_A5995_VREF1, PIO_TIMER_ALT);
pinPeripheral(PIN_A5995_VREF2, PIO_TIMER);
GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID( GCM_TCC0_TCC1 )) ;
while ( GCLK->STATUS.bit.SYNCBUSY == 1 ) ;
//ERROR("Setting TCC %d %d",ulValue,ulPin);
TCCx->CTRLA.reg &= ~TCC_CTRLA_ENABLE;
syncTCC(TCCx);
// Set TCx as normal PWM
TCCx->WAVE.reg |= TCC_WAVE_WAVEGEN_NPWM;
syncTCC(TCCx);
// Set TCx in waveform mode Normal PWM
TCCx->CC[1].reg = (uint32_t)0;
syncTCC(TCCx);
TCCx->CC[0].reg = (uint32_t)0;
syncTCC(TCCx);
// Set PER to maximum counter value (resolution : 0xFFF = 12 bits)
// =48e6/2^12=11kHz frequency
TCCx->PER.reg = DAC_MAX;
syncTCC(TCCx);
// Enable TCCx
TCCx->CTRLA.reg |= TCC_CTRLA_ENABLE ;
syncTCC(TCCx);
}
void A5995::begin()
{
//setup the A5995 pins
digitalWrite(PIN_A5995_ENABLE1,LOW);
pinMode(PIN_A5995_ENABLE1,OUTPUT);
digitalWrite(PIN_A5995_ENABLE2,LOW);
pinMode(PIN_A5995_ENABLE2,OUTPUT);
digitalWrite(PIN_A5995_MODE1,LOW);
pinMode(PIN_A5995_MODE1,OUTPUT);
digitalWrite(PIN_A5995_MODE2,LOW);
pinMode(PIN_A5995_MODE2,OUTPUT);
digitalWrite(PIN_A5995_PHASE1,LOW);
pinMode(PIN_A5995_PHASE1,OUTPUT);
digitalWrite(PIN_A5995_PHASE2,LOW);
pinMode(PIN_A5995_PHASE2,OUTPUT);
digitalWrite(PIN_A5995_SLEEPn,HIGH);
pinMode(PIN_A5995_SLEEPn,OUTPUT);
//setup the PWM for current on the A4954, set for low current
digitalWrite(PIN_A5995_VREF1,LOW);
digitalWrite(PIN_A5995_VREF2,LOW);
pinMode(PIN_A5995_VREF1, OUTPUT);
pinMode(PIN_A5995_VREF2, OUTPUT);
enabled=true;
lastStepMicros=0;
forwardRotation=true;
setupDAC();
//
// GPIO_HIGH(PIN_A5995_ENABLE1);
// GPIO_HIGH(PIN_A5995_ENABLE2);
// GPIO_LOW(PIN_A5995_MODE1);
// GPIO_LOW(PIN_A5995_MODE2);
// GPIO_HIGH(PIN_A5995_PHASE1);
// GPIO_HIGH(PIN_A5995_PHASE2);
// int i=0;;
// while (1)
// {
// int32_t x;
// WARNING("MA %d",i);
// x=(int32_t)((int64_t)i*(DAC_MAX))/3300;
// setDAC(x,x);
// delay(1000);
// i=i+10;
// if (i>1000)
// {
// i=0;
// }
//
// }
return;
}
void A5995::enable(bool enable)
{
enabled=enable;
if (enabled == false)
{
WARNING("A4954 disabled");
setDAC(0,0); //turn current off
GPIO_LOW(PIN_A5995_ENABLE1);
GPIO_LOW(PIN_A5995_ENABLE2);
GPIO_LOW(PIN_A5995_MODE1);
GPIO_LOW(PIN_A5995_MODE2);
GPIO_LOW(PIN_A5995_PHASE1);
GPIO_LOW(PIN_A5995_PHASE2);
}
}
//this is precise move and modulo of A4954_NUM_MICROSTEPS is a full step.
// stepAngle is in A4954_NUM_MICROSTEPS units..
// The A4954 has no idea where the motor is, so the calling function has to
// to tell the A4954 what phase to drive motor coils.
// A4954_NUM_MICROSTEPS is 256 by default so stepAngle of 1024 is 360 degrees
// Note you can only move up to +/-A4954_NUM_MICROSTEPS from where you
// currently are.
int32_t A5995::move(int32_t stepAngle, uint32_t mA)
{
uint16_t angle;
int32_t cos,sin;
int32_t dacSin,dacCos;
static int32_t lastSin=0,lastCos=0;
static int i=1;
if (enabled == false)
{
WARNING("A4954 disabled");
setDAC(0,0); //turn current off
GPIO_LOW(PIN_A5995_ENABLE1);
GPIO_LOW(PIN_A5995_ENABLE2);
GPIO_LOW(PIN_A5995_MODE1);
GPIO_LOW(PIN_A5995_MODE2);
GPIO_LOW(PIN_A5995_PHASE1);
GPIO_LOW(PIN_A5995_PHASE2);
return stepAngle;
}
//WARNING("move %d %d",stepAngle,mA);
stepAngle=(stepAngle) % SINE_STEPS;
//figure out our sine Angle
// note our SINE_STEPS is 4x of microsteps for a reason
//angle=(stepAngle+(SINE_STEPS/8)) % SINE_STEPS;
angle=stepAngle;
if (i==0)
{
WARNING("angle %d ",angle);
}
//calculate the sine and cosine of our angle
sin=sine(angle);
cos=cosine(angle);
//if we are reverse swap the sign of one of the angels
if (false == forwardRotation)
{
cos=-cos;
}
//scale sine result by current(mA)
dacSin=((int32_t)mA*(int64_t)(sin))/SINE_MAX;
if (i==0)
{
WARNING("dacsine %d ",dacSin);
}
// if ((lastSin-dacSin)>100) //decreasing current
// {
// GPIO_LOW(PIN_A5995_MODE2); //fast decay
// } else
// {
// GPIO_HIGH(PIN_A5995_MODE2); //slow decay
// }
lastSin=dacSin;
//convert value into DAC scaled to 3300mA max
dacSin=(int32_t)((int64_t)abs(dacSin)*(DAC_MAX))/3300;
//scale cosine result by current(mA)
dacCos=((int32_t)mA*(int64_t)(cos))/SINE_MAX;
if (i==0)
{
WARNING("daccos %d ",dacCos);
}
// if ((lastCos-dacCos)>100) //decreasing current
// {
// GPIO_LOW(PIN_A5995_MODE1); //fast decay
// } else
// {
// GPIO_HIGH(PIN_A5995_MODE1); //slow decay
// }
lastCos=dacCos;
//convert value into DAC scaled to 3300mA max
dacCos=(int32_t)((int64_t)abs(dacCos)*(DAC_MAX))/3300;
if (i==0)
{
WARNING("dacs are %d %d",dacSin,dacCos);
}
setDAC(dacSin,dacCos);
GPIO_HIGH(PIN_A5995_ENABLE1);
GPIO_HIGH(PIN_A5995_ENABLE2);
GPIO_LOW(PIN_A5995_MODE1);
GPIO_LOW(PIN_A5995_MODE2);
if (i==0)
{
WARNING("sins are %d %d",sin,cos);
}
if (sin>0)
{
GPIO_HIGH(PIN_A5995_PHASE2);
}else
{
GPIO_LOW(PIN_A5995_PHASE2);
}
if (cos>0)
{
GPIO_HIGH(PIN_A5995_PHASE1);
}else
{
GPIO_LOW(PIN_A5995_PHASE1);
}
// i++;
// if (i>3000) i=0;
// YELLOW_LED(led);
// led=(led+1) & 0x01;
lastStepMicros=micros();
return stepAngle;
}
#pragma GCC pop_options
================================================
FILE: firmware/stepper_nano_zero/A5995.h
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef A5995_H_
#define A5995_H_
#include <Arduino.h>
#include "board.h"
#include "angle.h"
#include "sine.h"
#define A5995_NUM_MICROSTEPS (256)
//prevent someone for making a mistake with the code
#if ((A5995_NUM_MICROSTEPS*4) != SINE_STEPS)
#error "SINE_STEPS must be 4x of Micro steps for the move function"
#endif
/*
* When it comes to the stepper driver if we use angles
* we will always have a rounding error. For example
* a 0-65536(360) angle for 1.8 degree step is 327.68 so
* if you increment 200 of these as 327 you have a 13.6 error
* after one rotation.
* If you use floating point the effect is the same but takes longer.
*
* The only error-less accumulation system is to use native units, ie full
* steps and microsteps.
*
*/
class A5995
{
private:
uint32_t lastStepMicros; // time in microseconds that last step happened
bool forwardRotation=true;
volatile bool enabled=true;
public:
void begin(void);
//moves motor where the modulo of A4954_NUM_MICROSTEPS is a full step.
int32_t move(int32_t stepAngle, uint32_t mA);
uint32_t microsSinceStep(void) {return micros()-lastStepMicros;};
void setRotationDirection(bool forward) {forwardRotation=forward;};
void enable(bool enable);
void limitCurrent(uint8_t percent) {return;}; //Not used
};
#endif /* A5995_H_ */
================================================
FILE: firmware/stepper_nano_zero/Adafruit_GFX.cpp
================================================
/*
This is the core graphics library for all our displays, providing a common
set of graphics primitives (points, lines, circles, etc.). It needs to be
paired with a hardware-specific library for each display device we carry
(to handle the lower-level functions).
Adafruit invests time and resources providing this open source code, please
support Adafruit & open-source hardware by purchasing products from Adafruit!
Copyright (c) 2013 Adafruit Industries. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
- Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
- Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifdef __AVR__
#include <avr/pgmspace.h>
#elif defined(ESP8266)
#include <pgmspace.h>
#endif
#include "Adafruit_GFX.h"
#include "glcdfont.c"
// Many (but maybe not all) non-AVR board installs define macros
// for compatibility with existing PROGMEM-reading AVR code.
// Do our own checks and defines here for good measure...
#ifndef pgm_read_byte
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
#endif
#ifndef pgm_read_word
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
#endif
#ifndef pgm_read_dword
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
#endif
// Pointers are a peculiar case...typically 16-bit on AVR boards,
// 32 bits elsewhere. Try to accommodate both...
#if !defined(__INT_MAX__) || (__INT_MAX__ > 0xFFFF)
#define pgm_read_pointer(addr) ((void *)pgm_read_dword(addr))
#else
#define pgm_read_pointer(addr) ((void *)pgm_read_word(addr))
#endif
#ifndef min
#define min(a,b) (((a) < (b)) ? (a) : (b))
#endif
#ifndef _swap_int16_t
#define _swap_int16_t(a, b) { int16_t t = a; a = b; b = t; }
#endif
Adafruit_GFX::Adafruit_GFX(int16_t w, int16_t h):
WIDTH(w), HEIGHT(h)
{
_width = WIDTH;
_height = HEIGHT;
rotation = 0;
cursor_y = cursor_x = 0;
textsize = 1;
textcolor = textbgcolor = 0xFFFF;
wrap = true;
_cp437 = false;
gfxFont = NULL;
}
// Draw a circle outline
void Adafruit_GFX::drawCircle(int16_t x0, int16_t y0, int16_t r,
uint16_t color) {
int16_t f = 1 - r;
int16_t ddF_x = 1;
int16_t ddF_y = -2 * r;
int16_t x = 0;
int16_t y = r;
drawPixel(x0 , y0+r, color);
drawPixel(x0 , y0-r, color);
drawPixel(x0+r, y0 , color);
drawPixel(x0-r, y0 , color);
while (x<y) {
if (f >= 0) {
y--;
ddF_y += 2;
f += ddF_y;
}
x++;
ddF_x += 2;
f += ddF_x;
drawPixel(x0 + x, y0 + y, color);
drawPixel(x0 - x, y0 + y, color);
drawPixel(x0 + x, y0 - y, color);
drawPixel(x0 - x, y0 - y, color);
drawPixel(x0 + y, y0 + x, color);
drawPixel(x0 - y, y0 + x, color);
drawPixel(x0 + y, y0 - x, color);
drawPixel(x0 - y, y0 - x, color);
}
}
void Adafruit_GFX::drawCircleHelper( int16_t x0, int16_t y0,
int16_t r, uint8_t cornername, uint16_t color) {
int16_t f = 1 - r;
int16_t ddF_x = 1;
int16_t ddF_y = -2 * r;
int16_t x = 0;
int16_t y = r;
while (x<y) {
if (f >= 0) {
y--;
ddF_y += 2;
f += ddF_y;
}
x++;
ddF_x += 2;
f += ddF_x;
if (cornername & 0x4) {
drawPixel(x0 + x, y0 + y, color);
drawPixel(x0 + y, y0 + x, color);
}
if (cornername & 0x2) {
drawPixel(x0 + x, y0 - y, color);
drawPixel(x0 + y, y0 - x, color);
}
if (cornername & 0x8) {
drawPixel(x0 - y, y0 + x, color);
drawPixel(x0 - x, y0 + y, color);
}
if (cornername & 0x1) {
drawPixel(x0 - y, y0 - x, color);
drawPixel(x0 - x, y0 - y, color);
}
}
}
void Adafruit_GFX::fillCircle(int16_t x0, int16_t y0, int16_t r,
uint16_t color) {
drawFastVLine(x0, y0-r, 2*r+1, color);
fillCircleHelper(x0, y0, r, 3, 0, color);
}
// Used to do circles and roundrects
void Adafruit_GFX::fillCircleHelper(int16_t x0, int16_t y0, int16_t r,
uint8_t cornername, int16_t delta, uint16_t color) {
int16_t f = 1 - r;
int16_t ddF_x = 1;
int16_t ddF_y = -2 * r;
int16_t x = 0;
int16_t y = r;
while (x<y) {
if (f >= 0) {
y--;
ddF_y += 2;
f += ddF_y;
}
x++;
ddF_x += 2;
f += ddF_x;
if (cornername & 0x1) {
drawFastVLine(x0+x, y0-y, 2*y+1+delta, color);
drawFastVLine(x0+y, y0-x, 2*x+1+delta, color);
}
if (cornername & 0x2) {
drawFastVLine(x0-x, y0-y, 2*y+1+delta, color);
drawFastVLine(x0-y, y0-x, 2*x+1+delta, color);
}
}
}
// Bresenham's algorithm - thx wikpedia
void Adafruit_GFX::drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
uint16_t color) {
int16_t steep = abs(y1 - y0) > abs(x1 - x0);
if (steep) {
_swap_int16_t(x0, y0);
_swap_int16_t(x1, y1);
}
if (x0 > x1) {
_swap_int16_t(x0, x1);
_swap_int16_t(y0, y1);
}
int16_t dx, dy;
dx = x1 - x0;
dy = abs(y1 - y0);
int16_t err = dx / 2;
int16_t ystep;
if (y0 < y1) {
ystep = 1;
} else {
ystep = -1;
}
for (; x0<=x1; x0++) {
if (steep) {
drawPixel(y0, x0, color);
} else {
drawPixel(x0, y0, color);
}
err -= dy;
if (err < 0) {
y0 += ystep;
err += dx;
}
}
}
// Draw a rectangle
void Adafruit_GFX::drawRect(int16_t x, int16_t y, int16_t w, int16_t h,
uint16_t color) {
drawFastHLine(x, y, w, color);
drawFastHLine(x, y+h-1, w, color);
drawFastVLine(x, y, h, color);
drawFastVLine(x+w-1, y, h, color);
}
void Adafruit_GFX::drawFastVLine(int16_t x, int16_t y,
int16_t h, uint16_t color) {
// Update in subclasses if desired!
drawLine(x, y, x, y+h-1, color);
}
void Adafruit_GFX::drawFastHLine(int16_t x, int16_t y,
int16_t w, uint16_t color) {
// Update in subclasses if desired!
drawLine(x, y, x+w-1, y, color);
}
void Adafruit_GFX::fillRect(int16_t x, int16_t y, int16_t w, int16_t h,
uint16_t color) {
// Update in subclasses if desired!
for (int16_t i=x; i<x+w; i++) {
drawFastVLine(i, y, h, color);
}
}
void Adafruit_GFX::fillScreen(uint16_t color) {
fillRect(0, 0, _width, _height, color);
}
// Draw a rounded rectangle
void Adafruit_GFX::drawRoundRect(int16_t x, int16_t y, int16_t w,
int16_t h, int16_t r, uint16_t color) {
// smarter version
drawFastHLine(x+r , y , w-2*r, color); // Top
drawFastHLine(x+r , y+h-1, w-2*r, color); // Bottom
drawFastVLine(x , y+r , h-2*r, color); // Left
drawFastVLine(x+w-1, y+r , h-2*r, color); // Right
// draw four corners
drawCircleHelper(x+r , y+r , r, 1, color);
drawCircleHelper(x+w-r-1, y+r , r, 2, color);
drawCircleHelper(x+w-r-1, y+h-r-1, r, 4, color);
drawCircleHelper(x+r , y+h-r-1, r, 8, color);
}
// Fill a rounded rectangle
void Adafruit_GFX::fillRoundRect(int16_t x, int16_t y, int16_t w,
int16_t h, int16_t r, uint16_t color) {
// smarter version
fillRect(x+r, y, w-2*r, h, color);
// draw four corners
fillCircleHelper(x+w-r-1, y+r, r, 1, h-2*r-1, color);
fillCircleHelper(x+r , y+r, r, 2, h-2*r-1, color);
}
// Draw a triangle
void Adafruit_GFX::drawTriangle(int16_t x0, int16_t y0,
int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color) {
drawLine(x0, y0, x1, y1, color);
drawLine(x1, y1, x2, y2, color);
drawLine(x2, y2, x0, y0, color);
}
// Fill a triangle
void Adafruit_GFX::fillTriangle(int16_t x0, int16_t y0,
int16_t x1, int16_t y1, int16_t x2, int16_t y2, uint16_t color) {
int16_t a, b, y, last;
// Sort coordinates by Y order (y2 >= y1 >= y0)
if (y0 > y1) {
_swap_int16_t(y0, y1); _swap_int16_t(x0, x1);
}
if (y1 > y2) {
_swap_int16_t(y2, y1); _swap_int16_t(x2, x1);
}
if (y0 > y1) {
_swap_int16_t(y0, y1); _swap_int16_t(x0, x1);
}
if(y0 == y2) { // Handle awkward all-on-same-line case as its own thing
a = b = x0;
if(x1 < a) a = x1;
else if(x1 > b) b = x1;
if(x2 < a) a = x2;
else if(x2 > b) b = x2;
drawFastHLine(a, y0, b-a+1, color);
return;
}
int16_t
dx01 = x1 - x0,
dy01 = y1 - y0,
dx02 = x2 - x0,
dy02 = y2 - y0,
dx12 = x2 - x1,
dy12 = y2 - y1;
int32_t
sa = 0,
sb = 0;
// For upper part of triangle, find scanline crossings for segments
// 0-1 and 0-2. If y1=y2 (flat-bottomed triangle), the scanline y1
// is included here (and second loop will be skipped, avoiding a /0
// error there), otherwise scanline y1 is skipped here and handled
// in the second loop...which also avoids a /0 error here if y0=y1
// (flat-topped triangle).
if(y1 == y2) last = y1; // Include y1 scanline
else last = y1-1; // Skip it
for(y=y0; y<=last; y++) {
a = x0 + sa / dy01;
b = x0 + sb / dy02;
sa += dx01;
sb += dx02;
/* longhand:
a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
*/
if(a > b) _swap_int16_t(a,b);
drawFastHLine(a, y, b-a+1, color);
}
// For lower part of triangle, find scanline crossings for segments
// 0-2 and 1-2. This loop is skipped if y1=y2.
sa = dx12 * (y - y1);
sb = dx02 * (y - y0);
for(; y<=y2; y++) {
a = x1 + sa / dy12;
b = x0 + sb / dy02;
sa += dx12;
sb += dx02;
/* longhand:
a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
*/
if(a > b) _swap_int16_t(a,b);
drawFastHLine(a, y, b-a+1, color);
}
}
// Draw a 1-bit image (bitmap) at the specified (x,y) position from the
// provided bitmap buffer (must be PROGMEM memory) using the specified
// foreground color (unset bits are transparent).
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y,
const uint8_t *bitmap, int16_t w, int16_t h, uint16_t color) {
int16_t i, j, byteWidth = (w + 7) / 8;
uint8_t byte;
for(j=0; j<h; j++) {
for(i=0; i<w; i++) {
if(i & 7) byte <<= 1;
else byte = pgm_read_byte(bitmap + j * byteWidth + i / 8);
if(byte & 0x80) drawPixel(x+i, y+j, color);
}
}
}
// Draw a 1-bit image (bitmap) at the specified (x,y) position from the
// provided bitmap buffer (must be PROGMEM memory) using the specified
// foreground (for set bits) and background (for clear bits) colors.
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y,
const uint8_t *bitmap, int16_t w, int16_t h, uint16_t color, uint16_t bg) {
int16_t i, j, byteWidth = (w + 7) / 8;
uint8_t byte;
for(j=0; j<h; j++) {
for(i=0; i<w; i++ ) {
if(i & 7) byte <<= 1;
else byte = pgm_read_byte(bitmap + j * byteWidth + i / 8);
if(byte & 0x80) drawPixel(x+i, y+j, color);
else drawPixel(x+i, y+j, bg);
}
}
}
// drawBitmap() variant for RAM-resident (not PROGMEM) bitmaps.
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y,
uint8_t *bitmap, int16_t w, int16_t h, uint16_t color) {
int16_t i, j, byteWidth = (w + 7) / 8;
uint8_t byte;
for(j=0; j<h; j++) {
for(i=0; i<w; i++ ) {
if(i & 7) byte <<= 1;
else byte = bitmap[j * byteWidth + i / 8];
if(byte & 0x80) drawPixel(x+i, y+j, color);
}
}
}
// drawBitmap() variant w/background for RAM-resident (not PROGMEM) bitmaps.
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y,
uint8_t *bitmap, int16_t w, int16_t h, uint16_t color, uint16_t bg) {
int16_t i, j, byteWidth = (w + 7) / 8;
uint8_t byte;
for(j=0; j<h; j++) {
for(i=0; i<w; i++ ) {
if(i & 7) byte <<= 1;
else byte = bitmap[j * byteWidth + i / 8];
if(byte & 0x80) drawPixel(x+i, y+j, color);
else drawPixel(x+i, y+j, bg);
}
}
}
//Draw XBitMap Files (*.xbm), exported from GIMP,
//Usage: Export from GIMP to *.xbm, rename *.xbm to *.c and open in editor.
//C Array can be directly used with this function
void Adafruit_GFX::drawXBitmap(int16_t x, int16_t y,
const uint8_t *bitmap, int16_t w, int16_t h, uint16_t color) {
int16_t i, j, byteWidth = (w + 7) / 8;
uint8_t byte;
for(j=0; j<h; j++) {
for(i=0; i<w; i++ ) {
if(i & 7) byte >>= 1;
else byte = pgm_read_byte(bitmap + j * byteWidth + i / 8);
if(byte & 0x01) drawPixel(x+i, y+j, color);
}
}
}
#if ARDUINO >= 100
size_t Adafruit_GFX::write(uint8_t c) {
#else
void Adafruit_GFX::write(uint8_t c) {
#endif
if(!gfxFont) { // 'Classic' built-in font
if(c == '\n') {
cursor_y += textsize*8;
cursor_x = 0;
} else if(c == '\r') {
// skip em
} else {
if(wrap && ((cursor_x + textsize * 6) >= _width)) { // Heading off edge?
cursor_x = 0; // Reset x to zero
cursor_y += textsize * 8; // Advance y one line
}
drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize);
cursor_x += textsize * 6;
}
} else { // Custom font
if(c == '\n') {
cursor_x = 0;
cursor_y += (int16_t)textsize *
(uint8_t)pgm_read_byte(&gfxFont->yAdvance);
} else if(c != '\r') {
uint8_t first = pgm_read_byte(&gfxFont->first);
if((c >= first) && (c <= (uint8_t)pgm_read_byte(&gfxFont->last))) {
uint8_t c2 = c - pgm_read_byte(&gfxFont->first);
GFXglyph *glyph = &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c2]);
uint8_t w = pgm_read_byte(&glyph->width),
h = pgm_read_byte(&glyph->height);
if((w > 0) && (h > 0)) { // Is there an associated bitmap?
int16_t xo = (int8_t)pgm_read_byte(&glyph->xOffset); // sic
if(wrap && ((cursor_x + textsize * (xo + w)) >= _width)) {
// Drawing character would go off right edge; wrap to new line
cursor_x = 0;
cursor_y += (int16_t)textsize *
(uint8_t)pgm_read_byte(&gfxFont->yAdvance);
}
drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize);
}
cursor_x += pgm_read_byte(&glyph->xAdvance) * (int16_t)textsize;
}
}
}
#if ARDUINO >= 100
return 1;
#endif
}
// Draw a character
void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,
uint16_t color, uint16_t bg, uint8_t size) {
if(!gfxFont) { // 'Classic' built-in font
if((x >= _width) || // Clip right
(y >= _height) || // Clip bottom
((x + 6 * size - 1) < 0) || // Clip left
((y + 8 * size - 1) < 0)) // Clip top
return;
if(!_cp437 && (c >= 176)) c++; // Handle 'classic' charset behavior
for(int8_t i=0; i<6; i++ ) {
uint8_t line;
if(i < 5) line = pgm_read_byte(font+(c*5)+i);
else line = 0x0;
for(int8_t j=0; j<8; j++, line >>= 1) {
if(line & 0x1) {
if(size == 1) drawPixel(x+i, y+j, color);
else fillRect(x+(i*size), y+(j*size), size, size, color);
} else if(bg != color) {
if(size == 1) drawPixel(x+i, y+j, bg);
else fillRect(x+i*size, y+j*size, size, size, bg);
}
}
}
} else { // Custom font
// Character is assumed previously filtered by write() to eliminate
// newlines, returns, non-printable characters, etc. Calling drawChar()
// directly with 'bad' characters of font may cause mayhem!
c -= pgm_read_byte(&gfxFont->first);
GFXglyph *glyph = &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c]);
uint8_t *bitmap = (uint8_t *)pgm_read_pointer(&gfxFont->bitmap);
uint16_t bo = pgm_read_word(&glyph->bitmapOffset);
uint8_t w = pgm_read_byte(&glyph->width),
h = pgm_read_byte(&glyph->height),
xa = pgm_read_byte(&glyph->xAdvance);
int8_t xo = pgm_read_byte(&glyph->xOffset),
yo = pgm_read_byte(&glyph->yOffset);
uint8_t xx, yy, bits, bit = 0;
int16_t xo16, yo16;
if(size > 1) {
xo16 = xo;
yo16 = yo;
}
// Todo: Add character clipping here
// NOTE: THERE IS NO 'BACKGROUND' COLOR OPTION ON CUSTOM FONTS.
// THIS IS ON PURPOSE AND BY DESIGN. The background color feature
// has typically been used with the 'classic' font to overwrite old
// screen contents with new data. This ONLY works because the
// characters are a uniform size; it's not a sensible thing to do with
// proportionally-spaced fonts with glyphs of varying sizes (and that
// may overlap). To replace previously-drawn text when using a custom
// font, use the getTextBounds() function to determine the smallest
// rectangle encompassing a string, erase the area with fillRect(),
// then draw new text. This WILL infortunately 'blink' the text, but
// is unavoidable. Drawing 'background' pixels will NOT fix this,
// only creates a new set of problems. Have an idea to work around
// this (a canvas object type for MCUs that can afford the RAM and
// displays supporting setAddrWindow() and pushColors()), but haven't
// implemented this yet.
for(yy=0; yy<h; yy++) {
for(xx=0; xx<w; xx++) {
if(!(bit++ & 7)) {
bits = pgm_read_byte(&bitmap[bo++]);
}
if(bits & 0x80) {
if(size == 1) {
drawPixel(x+xo+xx, y+yo+yy, color);
} else {
fillRect(x+(xo16+xx)*size, y+(yo16+yy)*size, size, size, color);
}
}
bits <<= 1;
}
}
} // End classic vs custom font
}
void Adafruit_GFX::setCursor(int16_t x, int16_t y) {
cursor_x = x;
cursor_y = y;
}
int16_t Adafruit_GFX::getCursorX(void) const {
return cursor_x;
}
int16_t Adafruit_GFX::getCursorY(void) const {
return cursor_y;
}
void Adafruit_GFX::setTextSize(uint8_t s) {
textsize = (s > 0) ? s : 1;
}
void Adafruit_GFX::setTextColor(uint16_t c) {
// For 'transparent' background, we'll set the bg
// to the same as fg instead of using a flag
textcolor = textbgcolor = c;
}
void Adafruit_GFX::setTextColor(uint16_t c, uint16_t b) {
textcolor = c;
textbgcolor = b;
}
void Adafruit_GFX::setTextWrap(boolean w) {
wrap = w;
}
uint8_t Adafruit_GFX::getRotation(void) const {
return rotation;
}
void Adafruit_GFX::setRotation(uint8_t x) {
rotation = (x & 3);
switch(rotation) {
case 0:
case 2:
_width = WIDTH;
_height = HEIGHT;
break;
case 1:
case 3:
_width = HEIGHT;
_height = WIDTH;
break;
}
}
// Enable (or disable) Code Page 437-compatible charset.
// There was an error in glcdfont.c for the longest time -- one character
// (#176, the 'light shade' block) was missing -- this threw off the index
// of every character that followed it. But a TON of code has been written
// with the erroneous character indices. By default, the library uses the
// original 'wrong' behavior and old sketches will still work. Pass 'true'
// to this function to use correct CP437 character values in your code.
void Adafruit_GFX::cp437(boolean x) {
_cp437 = x;
}
void Adafruit_GFX::setFont(const GFXfont *f) {
if(f) { // Font struct pointer passed in?
if(!gfxFont) { // And no current font struct?
// Switching from classic to new font behavior.
// Move cursor pos down 6 pixels so it's on baseline.
cursor_y += 6;
}
} else if(gfxFont) { // NULL passed. Current font struct defined?
// Switching from new to classic font behavior.
// Move cursor pos up 6 pixels so it's at top-left of char.
cursor_y -= 6;
}
gfxFont = (GFXfont *)f;
}
// Pass string and a cursor position, returns UL corner and W,H.
void Adafruit_GFX::getTextBounds(char *str, int16_t x, int16_t y,
int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h) {
uint8_t c; // Current character
*x1 = x;
*y1 = y;
*w = *h = 0;
if(gfxFont) {
GFXglyph *glyph;
uint8_t first = pgm_read_byte(&gfxFont->first),
last = pgm_read_byte(&gfxFont->last),
gw, gh, xa;
int8_t xo, yo;
int16_t minx = _width, miny = _height, maxx = -1, maxy = -1,
gx1, gy1, gx2, gy2, ts = (int16_t)textsize,
ya = ts * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
while((c = *str++)) {
if(c != '\n') { // Not a newline
if(c != '\r') { // Not a carriage return, is normal char
if((c >= first) && (c <= last)) { // Char present in current font
c -= first;
glyph = &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c]);
gw = pgm_read_byte(&glyph->width);
gh = pgm_read_byte(&glyph->height);
xa = pgm_read_byte(&glyph->xAdvance);
xo = pgm_read_byte(&glyph->xOffset);
yo = pgm_read_byte(&glyph->yOffset);
if(wrap && ((x + (((int16_t)xo + gw) * ts)) >= _width)) {
// Line wrap
x = 0; // Reset x to 0
y += ya; // Advance y by 1 line
}
gx1 = x + xo * ts;
gy1 = y + yo * ts;
gx2 = gx1 + gw * ts - 1;
gy2 = gy1 + gh * ts - 1;
if(gx1 < minx) minx = gx1;
if(gy1 < miny) miny = gy1;
if(gx2 > maxx) maxx = gx2;
if(gy2 > maxy) maxy = gy2;
x += xa * ts;
}
} // Carriage return = do nothing
} else { // Newline
x = 0; // Reset x
y += ya; // Advance y by 1 line
}
}
// End of string
*x1 = minx;
*y1 = miny;
if(maxx >= minx) *w = maxx - minx + 1;
if(maxy >= miny) *h = maxy - miny + 1;
} else { // Default font
uint16_t lineWidth = 0, maxWidth = 0; // Width of current, all lines
while((c = *str++)) {
if(c != '\n') { // Not a newline
if(c != '\r') { // Not a carriage return, is normal char
if(wrap && ((x + textsize * 6) >= _width)) {
x = 0; // Reset x to 0
y += textsize * 8; // Advance y by 1 line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Save widest line
lineWidth = textsize * 6; // First char on new line
} else { // No line wrap, just keep incrementing X
lineWidth += textsize * 6; // Includes interchar x gap
}
} // Carriage return = do nothing
} else { // Newline
x = 0; // Reset x to 0
y += textsize * 8; // Advance y by 1 line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Save widest line
lineWidth = 0; // Reset lineWidth for new line
}
}
// End of string
if(lineWidth) y += textsize * 8; // Add height of last (or only) line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Is the last or only line the widest?
*w = maxWidth - 1; // Don't include last interchar x gap
*h = y - *y1;
} // End classic vs custom font
}
// Same as above, but for PROGMEM strings
void Adafruit_GFX::getTextBounds(const __FlashStringHelper *str,
int16_t x, int16_t y, int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h) {
uint8_t *s = (uint8_t *)str, c;
*x1 = x;
*y1 = y;
*w = *h = 0;
if(gfxFont) {
GFXglyph *glyph;
uint8_t first = pgm_read_byte(&gfxFont->first),
last = pgm_read_byte(&gfxFont->last),
gw, gh, xa;
int8_t xo, yo;
int16_t minx = _width, miny = _height, maxx = -1, maxy = -1,
gx1, gy1, gx2, gy2, ts = (int16_t)textsize,
ya = ts * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
while((c = pgm_read_byte(s++))) {
if(c != '\n') { // Not a newline
if(c != '\r') { // Not a carriage return, is normal char
if((c >= first) && (c <= last)) { // Char present in current font
c -= first;
glyph = &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c]);
gw = pgm_read_byte(&glyph->width);
gh = pgm_read_byte(&glyph->height);
xa = pgm_read_byte(&glyph->xAdvance);
xo = pgm_read_byte(&glyph->xOffset);
yo = pgm_read_byte(&glyph->yOffset);
if(wrap && ((x + (((int16_t)xo + gw) * ts)) >= _width)) {
// Line wrap
x = 0; // Reset x to 0
y += ya; // Advance y by 1 line
}
gx1 = x + xo * ts;
gy1 = y + yo * ts;
gx2 = gx1 + gw * ts - 1;
gy2 = gy1 + gh * ts - 1;
if(gx1 < minx) minx = gx1;
if(gy1 < miny) miny = gy1;
if(gx2 > maxx) maxx = gx2;
if(gy2 > maxy) maxy = gy2;
x += xa * ts;
}
} // Carriage return = do nothing
} else { // Newline
x = 0; // Reset x
y += ya; // Advance y by 1 line
}
}
// End of string
*x1 = minx;
*y1 = miny;
if(maxx >= minx) *w = maxx - minx + 1;
if(maxy >= miny) *h = maxy - miny + 1;
} else { // Default font
uint16_t lineWidth = 0, maxWidth = 0; // Width of current, all lines
while((c = pgm_read_byte(s++))) {
if(c != '\n') { // Not a newline
if(c != '\r') { // Not a carriage return, is normal char
if(wrap && ((x + textsize * 6) >= _width)) {
x = 0; // Reset x to 0
y += textsize * 8; // Advance y by 1 line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Save widest line
lineWidth = textsize * 6; // First char on new line
} else { // No line wrap, just keep incrementing X
lineWidth += textsize * 6; // Includes interchar x gap
}
} // Carriage return = do nothing
} else { // Newline
x = 0; // Reset x to 0
y += textsize * 8; // Advance y by 1 line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Save widest line
lineWidth = 0; // Reset lineWidth for new line
}
}
// End of string
if(lineWidth) y += textsize * 8; // Add height of last (or only) line
if(lineWidth > maxWidth) maxWidth = lineWidth; // Is the last or only line the widest?
*w = maxWidth - 1; // Don't include last interchar x gap
*h = y - *y1;
} // End classic vs custom font
}
// Return the size of the display (per current rotation)
int16_t Adafruit_GFX::width(void) const {
return _width;
}
int16_t Adafruit_GFX::height(void) const {
return _height;
}
void Adafruit_GFX::invertDisplay(boolean i) {
// Do nothing, must be subclassed if supported by hardware
}
/***************************************************************************/
// code for the GFX button UI element
Adafruit_GFX_Button::Adafruit_GFX_Button(void) {
_gfx = 0;
}
void Adafruit_GFX_Button::initButton(
Adafruit_GFX *gfx, int16_t x, int16_t y, uint8_t w, uint8_t h,
uint16_t outline, uint16_t fill, uint16_t textcolor,
char *label, uint8_t textsize)
{
_x = x;
_y = y;
_w = w;
_h = h;
_outlinecolor = outline;
_fillcolor = fill;
_textcolor = textcolor;
_textsize = textsize;
_gfx = gfx;
strncpy(_label, label, 9);
_label[9] = 0;
}
void Adafruit_GFX_Button::drawButton(boolean inverted) {
uint16_t fill, outline, text;
if(!inverted) {
fill = _fillcolor;
outline = _outlinecolor;
text = _textcolor;
} else {
fill = _textcolor;
outline = _outlinecolor;
text = _fillcolor;
}
_gfx->fillRoundRect(_x - (_w/2), _y - (_h/2), _w, _h, min(_w,_h)/4, fill);
_gfx->drawRoundRect(_x - (_w/2), _y - (_h/2), _w, _h, min(_w,_h)/4, outline);
_gfx->setCursor(_x - strlen(_label)*3*_textsize, _y-4*_textsize);
_gfx->setTextColor(text);
_gfx->setTextSize(_textsize);
_gfx->print(_label);
}
boolean Adafruit_GFX_Button::contains(int16_t x, int16_t y) {
if ((x < (_x - _w/2)) || (x > (_x + _w/2))) return false;
if ((y < (_y - _h/2)) || (y > (_y + _h/2))) return false;
return true;
}
void Adafruit_GFX_Button::press(boolean p) {
laststate = currstate;
currstate = p;
}
boolean Adafruit_GFX_Button::isPressed() { return currstate; }
boolean Adafruit_GFX_Button::justPressed() { return (currstate && !laststate); }
boolean Adafruit_GFX_Button::justReleased() { return (!currstate && laststate); }
// -------------------------------------------------------------------------
// GFXcanvas1 and GFXcanvas16 (currently a WIP, don't get too comfy with the
// implementation) provide 1- and 16-bit offscreen canvases, the address of
// which can be passed to drawBitmap() or pushColors() (the latter appears
// to only be in Adafruit_TFTLCD at this time). This is here mostly to
// help with the recently-added proportionally-spaced fonts; adds a way to
// refresh a section of the screen without a massive flickering clear-and-
// redraw...but maybe you'll find other uses too. VERY RAM-intensive, since
// the buffer is in MCU memory and not the display driver...GXFcanvas1 might
// be minimally useful on an Uno-class board, but this and GFXcanvas16 are
// much more likely to require at least a Mega or various recent ARM-type
// boards (recomment, as the text+bitmap draw can be pokey). GFXcanvas1
// requires 1 bit per pixel (rounded up to nearest byte per scanline),
// GFXcanvas16 requires 2 bytes per pixel (no scanline pad).
// NOT EXTENSIVELY TESTED YET. MAY CONTAIN WORST BUGS KNOWN TO HUMANKIND.
GFXcanvas1::GFXcanvas1(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {
uint16_t bytes = ((w + 7) / 8) * h;
if((buffer = (uint8_t *)malloc(bytes))) {
memset(buffer, 0, bytes);
}
}
GFXcanvas1::~GFXcanvas1(void) {
if(buffer) free(buffer);
}
uint8_t* GFXcanvas1::getBuffer(void) {
return buffer;
}
void GFXcanvas1::drawPixel(int16_t x, int16_t y, uint16_t color) {
// Bitmask tables of 0x80>>X and ~(0x80>>X), because X>>Y is slow on AVR
static const uint8_t PROGMEM
GFXsetBit[] = { 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01 },
GFXclrBit[] = { 0x7F, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB, 0xFD, 0xFE };
if(buffer) {
if((x < 0) || (y < 0) || (x >= _width) || (y >= _height)) return;
int16_t t;
switch(rotation) {
case 1:
t = x;
x = WIDTH - 1 - y;
y = t;
break;
case 2:
x = WIDTH - 1 - x;
y = HEIGHT - 1 - y;
break;
case 3:
t = x;
x = y;
y = HEIGHT - 1 - t;
break;
}
uint8_t *ptr = &buffer[(x / 8) + y * ((WIDTH + 7) / 8)];
if(color) *ptr |= pgm_read_byte(&GFXsetBit[x & 7]);
else *ptr &= pgm_read_byte(&GFXclrBit[x & 7]);
}
}
void GFXcanvas1::fillScreen(uint16_t color) {
if(buffer) {
uint16_t bytes = ((WIDTH + 7) / 8) * HEIGHT;
memset(buffer, color ? 0xFF : 0x00, bytes);
}
}
GFXcanvas16::GFXcanvas16(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {
uint16_t bytes = w * h * 2;
if((buffer = (uint16_t *)malloc(bytes))) {
memset(buffer, 0, bytes);
}
}
GFXcanvas16::~GFXcanvas16(void) {
if(buffer) free(buffer);
}
uint16_t* GFXcanvas16::getBuffer(void) {
return buffer;
}
void GFXcanvas16::drawPixel(int16_t x, int16_t y, uint16_t color) {
if(buffer) {
if((x < 0) || (y < 0) || (x >= _width) || (y >= _height)) return;
int16_t t;
switch(rotation) {
case 1:
t = x;
x = WIDTH - 1 - y;
y = t;
break;
case 2:
x = WIDTH - 1 - x;
y = HEIGHT - 1 - y;
break;
case 3:
t = x;
x = y;
y = HEIGHT - 1 - t;
break;
}
buffer[x + y * WIDTH] = color;
}
}
void GFXcanvas16::fillScreen(uint16_t color) {
if(buffer) {
uint8_t hi = color >> 8, lo = color & 0xFF;
if(hi == lo) {
memset(buffer, lo, WIDTH * HEIGHT * 2);
} else {
uint16_t i, pixels = WIDTH * HEIGHT;
for(i=0; i<pixels; i++) buffer[i] = color;
}
}
}
================================================
FILE: firmware/stepper_nano_zero/Adafruit_GFX.h
================================================
#ifndef _ADAFRUIT_GFX_H
#define _ADAFRUIT_GFX_H
#if ARDUINO >= 100
#include "Arduino.h"
#include "Print.h"
#else
#include "WProgram.h"
#endif
#include "gfxfont.h"
class Adafruit_GFX : public Print {
public:
Adafruit_GFX(int16_t w, int16_t h); // Constructor
// This MUST be defined by the subclass:
virtual void drawPixel(int16_t x, int16_t y, uint16_t color) = 0;
// These MAY be overridden by the subclass to provide device-specific
// optimized code. Otherwise 'generic' versions are used.
virtual void
drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, uint16_t color),
drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color),
drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color),
drawRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color),
fillRect(int16_t x, int16_t y, int16_t w, int16_t h, uint16_t color),
fillScreen(uint16_t color),
invertDisplay(boolean i);
// These exist only with Adafruit_GFX (no subclass overrides)
void
drawCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color),
drawCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername,
uint16_t color),
fillCircle(int16_t x0, int16_t y0, int16_t r, uint16_t color),
fillCircleHelper(int16_t x0, int16_t y0, int16_t r, uint8_t cornername,
int16_t delta, uint16_t color),
drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
int16_t x2, int16_t y2, uint16_t color),
fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
int16_t x2, int16_t y2, uint16_t color),
drawRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h,
int16_t radius, uint16_t color),
fillRoundRect(int16_t x0, int16_t y0, int16_t w, int16_t h,
int16_t radius, uint16_t color),
drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap,
int16_t w, int16_t h, uint16_t color),
drawBitmap(int16_t x, int16_t y, const uint8_t *bitmap,
int16_t w, int16_t h, uint16_t color, uint16_t bg),
drawBitmap(int16_t x, int16_t y, uint8_t *bitmap,
int16_t w, int16_t h, uint16_t color),
drawBitmap(int16_t x, int16_t y, uint8_t *bitmap,
int16_t w, int16_t h, uint16_t color, uint16_t bg),
drawXBitmap(int16_t x, int16_t y, const uint8_t *bitmap,
int16_t w, int16_t h, uint16_t color),
drawChar(int16_t x, int16_t y, unsigned char c, uint16_t color,
uint16_t bg, uint8_t size),
setCursor(int16_t x, int16_t y),
setTextColor(uint16_t c),
setTextColor(uint16_t c, uint16_t bg),
setTextSize(uint8_t s),
setTextWrap(boolean w),
setRotation(uint8_t r),
cp437(boolean x=true),
setFont(const GFXfont *f = NULL),
getTextBounds(char *string, int16_t x, int16_t y,
int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h),
getTextBounds(const __FlashStringHelper *s, int16_t x, int16_t y,
int16_t *x1, int16_t *y1, uint16_t *w, uint16_t *h);
#if ARDUINO >= 100
virtual size_t write(uint8_t);
#else
virtual void write(uint8_t);
#endif
int16_t height(void) const;
int16_t width(void) const;
uint8_t getRotation(void) const;
// get current cursor position (get rotation safe maximum values, using: width() for x, height() for y)
int16_t getCursorX(void) const;
int16_t getCursorY(void) const;
protected:
const int16_t
WIDTH, HEIGHT; // This is the 'raw' display w/h - never changes
int16_t
_width, _height, // Display w/h as modified by current rotation
cursor_x, cursor_y;
uint16_t
textcolor, textbgcolor;
uint8_t
textsize,
rotation;
boolean
wrap, // If set, 'wrap' text at right edge of display
_cp437; // If set, use correct CP437 charset (default is off)
GFXfont
*gfxFont;
};
class Adafruit_GFX_Button {
public:
Adafruit_GFX_Button(void);
void initButton(Adafruit_GFX *gfx, int16_t x, int16_t y,
uint8_t w, uint8_t h, uint16_t outline, uint16_t fill,
uint16_t textcolor, char *label, uint8_t textsize);
void drawButton(boolean inverted = false);
boolean contains(int16_t x, int16_t y);
void press(boolean p);
boolean isPressed();
boolean justPressed();
boolean justReleased();
private:
Adafruit_GFX *_gfx;
int16_t _x, _y;
uint16_t _w, _h;
uint8_t _textsize;
uint16_t _outlinecolor, _fillcolor, _textcolor;
char _label[10];
boolean currstate, laststate;
};
class GFXcanvas1 : public Adafruit_GFX {
public:
GFXcanvas1(uint16_t w, uint16_t h);
~GFXcanvas1(void);
void drawPixel(int16_t x, int16_t y, uint16_t color),
fillScreen(uint16_t color);
uint8_t *getBuffer(void);
private:
uint8_t *buffer;
};
class GFXcanvas16 : public Adafruit_GFX {
GFXcanvas16(uint16_t w, uint16_t h);
~GFXcanvas16(void);
void drawPixel(int16_t x, int16_t y, uint16_t color),
fillScreen(uint16_t color);
uint16_t *getBuffer(void);
private:
uint16_t *buffer;
};
#endif // _ADAFRUIT_GFX_H
================================================
FILE: firmware/stepper_nano_zero/Adafruit_SSD1306.cpp
================================================
/*********************************************************************
This is a library for our Monochrome OLEDs based on SSD1306 drivers
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/category/63_98
These displays use SPI to communicate, 4 or 5 pins are required to
interface
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
Written by Limor Fried/Ladyada for Adafruit Industries.
BSD license, check license.txt for more information
All text above, and the splash screen below must be included in any redistribution
*********************************************************************/
#ifdef __AVR__
#include <avr/pgmspace.h>
#elif defined(ESP8266)
#include <pgmspace.h>
#else
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
#endif
#if !defined(__ARM_ARCH) && !defined(ENERGIA) && !defined(ESP8266)
#include <util/delay.h>
#endif
#include <stdlib.h>
#include <Wire.h>
#include <SPI.h>
#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"
#include "syslog.h"
// the memory buffer for the LCD
static uint8_t buffer[SSD1306_LCDHEIGHT * SSD1306_LCDWIDTH / 8] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x80, 0x80, 0xC0, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xF8, 0xE0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80,
0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0xFF,
#if (SSD1306_LCDHEIGHT * SSD1306_LCDWIDTH > 96*16)
0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x80, 0x80, 0x00, 0x00,
0x80, 0xFF, 0xFF, 0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x80,
0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x00, 0x00, 0x8C, 0x8E, 0x84, 0x00, 0x00, 0x80, 0xF8,
0xF8, 0xF8, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xE0, 0xE0, 0xC0, 0x80,
0x00, 0xE0, 0xFC, 0xFE, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFF, 0xC7, 0x01, 0x01,
0x01, 0x01, 0x83, 0xFF, 0xFF, 0x00, 0x00, 0x7C, 0xFE, 0xC7, 0x01, 0x01, 0x01, 0x01, 0x83, 0xFF,
0xFF, 0xFF, 0x00, 0x38, 0xFE, 0xC7, 0x83, 0x01, 0x01, 0x01, 0x83, 0xC7, 0xFF, 0xFF, 0x00, 0x00,
0x01, 0xFF, 0xFF, 0x01, 0x01, 0x00, 0xFF, 0xFF, 0x07, 0x01, 0x01, 0x01, 0x00, 0x00, 0x7F, 0xFF,
0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x01, 0xFF,
0xFF, 0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x03, 0x0F, 0x3F, 0x7F, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xE7, 0xC7, 0xC7, 0x8F,
0x8F, 0x9F, 0xBF, 0xFF, 0xFF, 0xC3, 0xC0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0xFC, 0xFC,
0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xF8, 0xF8, 0xF0, 0xF0, 0xE0, 0xC0, 0x00, 0x01, 0x03, 0x03, 0x03,
0x03, 0x03, 0x01, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x01, 0x01,
0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x01, 0x01, 0x03, 0x03, 0x00, 0x00,
0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x03, 0x03, 0x03, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x01, 0x00, 0x00, 0x00, 0x03,
0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
#if (SSD1306_LCDHEIGHT == 64)
0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF9, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3F, 0x1F, 0x0F,
0x87, 0xC7, 0xF7, 0xFF, 0xFF, 0x1F, 0x1F, 0x3D, 0xFC, 0xF8, 0xF8, 0xF8, 0xF8, 0x7C, 0x7D, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0x0F, 0x07, 0x00, 0x30, 0x30, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0xC0, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0xC0, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x7F, 0x3F, 0x1F,
0x0F, 0x07, 0x1F, 0x7F, 0xFF, 0xFF, 0xF8, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xF8, 0xE0,
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00,
0x00, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x0E, 0xFC, 0xF8, 0x00, 0x00, 0xF0, 0xF8, 0x1C, 0x0E,
0x06, 0x06, 0x06, 0x0C, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0x00, 0x00, 0xFC,
0xFE, 0xFC, 0x00, 0x18, 0x3C, 0x7E, 0x66, 0xE6, 0xCE, 0x84, 0x00, 0x00, 0x06, 0xFF, 0xFF, 0x06,
0x06, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x06, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0xC0, 0xF8,
0xFC, 0x4E, 0x46, 0x46, 0x46, 0x4E, 0x7C, 0x78, 0x40, 0x18, 0x3C, 0x76, 0xE6, 0xCE, 0xCC, 0x80,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x01, 0x07, 0x0F, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F, 0x3F, 0x1F, 0x0F, 0x03,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00,
0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00, 0x03, 0x07, 0x0E, 0x0C,
0x18, 0x18, 0x0C, 0x06, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x01, 0x0F, 0x0E, 0x0C, 0x18, 0x0C, 0x0F,
0x07, 0x01, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00,
0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x07,
0x07, 0x0C, 0x0C, 0x18, 0x1C, 0x0C, 0x06, 0x06, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
#endif
#endif
};
#define ssd1306_swap(a, b) { int16_t t = a; a = b; b = t; }
// the most basic function, set a single pixel
void Adafruit_SSD1306::drawPixel(int16_t x, int16_t y, uint16_t color) {
if ((x < 0) || (x >= width()) || (y < 0) || (y >= height()))
return;
// check rotation, move pixel around if necessary
switch (getRotation()) {
case 1:
ssd1306_swap(x, y);
x = WIDTH - x - 1;
break;
case 2:
x = WIDTH - x - 1;
y = HEIGHT - y - 1;
break;
case 3:
ssd1306_swap(x, y);
y = HEIGHT - y - 1;
break;
}
// x is which column
switch (color)
{
case WHITE: buffer[x+ (y/8)*SSD1306_LCDWIDTH] |= (1 << (y&7)); break;
case BLACK: buffer[x+ (y/8)*SSD1306_LCDWIDTH] &= ~(1 << (y&7)); break;
case INVERSE: buffer[x+ (y/8)*SSD1306_LCDWIDTH] ^= (1 << (y&7)); break;
}
}
Adafruit_SSD1306::Adafruit_SSD1306(int8_t SID, int8_t SCLK, int8_t DC, int8_t RST, int8_t CS) : Adafruit_GFX(SSD1306_LCDWIDTH, SSD1306_LCDHEIGHT) {
cs = CS;
rst = RST;
dc = DC;
sclk = SCLK;
sid = SID;
hwSPI = false;
}
// constructor for hardware SPI - we indicate DataCommand, ChipSelect, Reset
Adafruit_SSD1306::Adafruit_SSD1306(int8_t DC, int8_t RST, int8_t CS) : Adafruit_GFX(SSD1306_LCDWIDTH, SSD1306_LCDHEIGHT) {
dc = DC;
rst = RST;
cs = CS;
hwSPI = true;
}
// initializer for I2C - we only indicate the reset pin!
Adafruit_SSD1306::Adafruit_SSD1306(int8_t reset) :
Adafruit_GFX(SSD1306_LCDWIDTH, SSD1306_LCDHEIGHT) {
sclk = dc = cs = sid = -1;
rst = reset;
}
bool Adafruit_SSD1306::begin(uint8_t vccstate, uint8_t i2caddr, bool reset) {
_vccstate = vccstate;
_i2caddr = i2caddr;
// set pin directions
if (sid != -1){
pinMode(dc, OUTPUT);
pinMode(cs, OUTPUT);
#ifdef HAVE_PORTREG
csport = portOutputRegister(digitalPinToPort(cs));
cspinmask = digitalPinToBitMask(cs);
dcport = portOutputRegister(digitalPinToPort(dc));
dcpinmask = digitalPinToBitMask(dc);
#endif
if (!hwSPI){
// set pins for software-SPI
pinMode(sid, OUTPUT);
pinMode(sclk, OUTPUT);
#ifdef HAVE_PORTREG
clkport = portOutputRegister(digitalPinToPort(sclk));
clkpinmask = digitalPinToBitMask(sclk);
mosiport = portOutputRegister(digitalPinToPort(sid));
mosipinmask = digitalPinToBitMask(sid);
#endif
}
if (hwSPI){
SPI.begin();
#ifdef SPI_HAS_TRANSACTION
SPI.beginTransaction(SPISettings(8000000, MSBFIRST, SPI_MODE0));
#else
SPI.setClockDivider (4);
#endif
}
}
else
{
// I2C Init
Wire.begin();
#ifdef __SAM3X8E__
// Force 400 KHz I2C, rawr! (Uses pins 20, 21 for SDA, SCL)
TWI1->TWI_CWGR = 0;
TWI1->TWI_CWGR = ((VARIANT_MCK / (2 * 400000)) - 4) * 0x101;
#endif
}
if ((reset) && (rst >= 0)) {
// Setup reset pin direction (used by both SPI and I2C)
pinMode(rst, OUTPUT);
digitalWrite(rst, HIGH);
// VDD (3.3V) goes high at start, lets just chill for a ms
delay(1);
// bring reset low
digitalWrite(rst, LOW);
// wait 10ms
delay(10);
// bring out of reset
digitalWrite(rst, HIGH);
// turn on VCC (9V?)
}
uint8_t ret;
// Init sequence
LOG("Sending LCD display off");
ret=ssd1306_command(SSD1306_DISPLAYOFF); // 0xAE
LOG("Display off returned %d",ret);
if (ret != 0)
{
return false;
}
ssd1306_command(SSD1306_SETDISPLAYCLOCKDIV); // 0xD5
ssd1306_command(0x80); // the suggested ratio 0x80
ssd1306_command(SSD1306_SETMULTIPLEX); // 0xA8
ssd1306_command(SSD1306_LCDHEIGHT - 1);
ssd1306_command(SSD1306_SETDISPLAYOFFSET); // 0xD3
ssd1306_command(0x0); // no offset
ssd1306_command(SSD1306_SETSTARTLINE | 0x0); // line #0
ssd1306_command(SSD1306_CHARGEPUMP); // 0x8D
if (vccstate == SSD1306_EXTERNALVCC)
{ ssd1306_command(0x10); }
else
{ ssd1306_command(0x14); }
ssd1306_command(SSD1306_MEMORYMODE); // 0x20
ssd1306_command(0x00); // 0x0 act like ks0108
ssd1306_command(SSD1306_SEGREMAP | 0x1);
ssd1306_command(SSD1306_COMSCANDEC);
#if defined SSD1306_128_32
ssd1306_command(SSD1306_SETCOMPINS); // 0xDA
ssd1306_command(0x02);
ssd1306_command(SSD1306_SETCONTRAST); // 0x81
ssd1306_command(0x8F);
#elif defined SSD1306_128_64
ssd1306_command(SSD1306_SETCOMPINS); // 0xDA
ssd1306_command(0x12);
ssd1306_command(SSD1306_SETCONTRAST); // 0x81
if (vccstate == SSD1306_EXTERNALVCC)
{ ssd1306_command(0x9F); }
else
{ ssd1306_command(0xCF); }
#elif defined SSD1306_96_16
ssd1306_command(SSD1306_SETCOMPINS); // 0xDA
ssd1306_command(0x2); //ada x12
ssd1306_command(SSD1306_SETCONTRAST); // 0x81
if (vccstate == SSD1306_EXTERNALVCC)
{ ssd1306_command(0x10); }
else
{ ssd1306_command(0xAF); }
#endif
ssd1306_command(SSD1306_SETPRECHARGE); // 0xd9
if (vccstate == SSD1306_EXTERNALVCC)
{ ssd1306_command(0x22); }
else
{ ssd1306_command(0xF1); }
ssd1306_command(SSD1306_SETVCOMDETECT); // 0xDB
ssd1306_command(0x40);
ssd1306_command(SSD1306_DISPLAYALLON_RESUME); // 0xA4
ssd1306_command(SSD1306_NORMALDISPLAY); // 0xA6
ssd1306_command(SSD1306_DEACTIVATE_SCROLL);
ssd1306_command(SSD1306_DISPLAYON);//--turn on oled panel
return true;
}
void Adafruit_SSD1306::invertDisplay(uint8_t i) {
if (i) {
ssd1306_command(SSD1306_INVERTDISPLAY);
} else {
ssd1306_command(SSD1306_NORMALDISPLAY);
}
}
// Errors:
// 0 : Success
// 1 : Data too long
// 2 : NACK on transmit of address
// 3 : NACK on transmit of data
// 4 : Other error
uint8_t Adafruit_SSD1306::ssd1306_command(uint8_t c) {
if (sid != -1)
{
// SPI
#ifdef HAVE_PORTREG
*csport |= cspinmask;
*dcport &= ~dcpinmask;
*csport &= ~cspinmask;
#else
digitalWrite(cs, HIGH);
digitalWrite(dc, LOW);
digitalWrite(cs, LOW);
#endif
fastSPIwrite(c);
#ifdef HAVE_PORTREG
*csport |= cspinmask;
#else
digitalWrite(cs, HIGH);
#endif
}
else
{
// I2C
uint8_t control = 0x00; // Co = 0, D/C = 0
//LOG("wire begin");
Wire.beginTransmission(_i2caddr);
//LOG("wire write");
Wire.write(control);
//LOG("wire write c");
Wire.write(c);
//LOG("wire end");
Wire.endTransmission();
//LOG("wire done");
}
}
// startscrollright
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F)
void Adafruit_SSD1306::startscrollright(uint8_t start, uint8_t stop){
ssd1306_command(SSD1306_RIGHT_HORIZONTAL_SCROLL);
ssd1306_command(0X00);
ssd1306_command(start);
ssd1306_command(0X00);
ssd1306_command(stop);
ssd1306_command(0X00);
ssd1306_command(0XFF);
ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}
// startscrollleft
// Activate a right handed scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F)
void Adafruit_SSD1306::startscrollleft(uint8_t start, uint8_t stop){
ssd1306_command(SSD1306_LEFT_HORIZONTAL_SCROLL);
ssd1306_command(0X00);
ssd1306_command(start);
ssd1306_command(0X00);
ssd1306_command(stop);
ssd1306_command(0X00);
ssd1306_command(0XFF);
ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}
// startscrolldiagright
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F)
void Adafruit_SSD1306::startscrolldiagright(uint8_t start, uint8_t stop){
ssd1306_command(SSD1306_SET_VERTICAL_SCROLL_AREA);
ssd1306_command(0X00);
ssd1306_command(SSD1306_LCDHEIGHT);
ssd1306_command(SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL);
ssd1306_command(0X00);
ssd1306_command(start);
ssd1306_command(0X00);
ssd1306_command(stop);
ssd1306_command(0X01);
ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}
// startscrolldiagleft
// Activate a diagonal scroll for rows start through stop
// Hint, the display is 16 rows tall. To scroll the whole display, run:
// display.scrollright(0x00, 0x0F)
void Adafruit_SSD1306::startscrolldiagleft(uint8_t start, uint8_t stop){
ssd1306_command(SSD1306_SET_VERTICAL_SCROLL_AREA);
ssd1306_command(0X00);
ssd1306_command(SSD1306_LCDHEIGHT);
ssd1306_command(SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL);
ssd1306_command(0X00);
ssd1306_command(start);
ssd1306_command(0X00);
ssd1306_command(stop);
ssd1306_command(0X01);
ssd1306_command(SSD1306_ACTIVATE_SCROLL);
}
void Adafruit_SSD1306::stopscroll(void){
ssd1306_command(SSD1306_DEACTIVATE_SCROLL);
}
// Dim the display
// dim = true: display is dimmed
// dim = false: display is normal
void Adafruit_SSD1306::dim(boolean dim) {
uint8_t contrast;
if (dim) {
contrast = 0; // Dimmed display
} else {
if (_vccstate == SSD1306_EXTERNALVCC) {
contrast = 0x9F;
} else {
contrast = 0xCF;
}
}
// the range of contrast to too small to be really useful
// it is useful to dim the display
ssd1306_command(SSD1306_SETCONTRAST);
ssd1306_command(contrast);
}
void Adafruit_SSD1306::display(void) {
ssd1306_command(SSD1306_COLUMNADDR);
ssd1306_command(0); // Column start address (0 = reset)
ssd1306_command(SSD1306_LCDWIDTH-1); // Column end address (127 = reset)
ssd1306_command(SSD1306_PAGEADDR);
ssd1306_command(0); // Page start address (0 = reset)
#if SSD1306_LCDHEIGHT == 64
ssd1306_command(7); // Page end address
#endif
#if SSD1306_LCDHEIGHT == 32
ssd1306_command(3); // Page end address
#endif
#if SSD1306_LCDHEIGHT == 16
ssd1306_command(1); // Page end address
#endif
if (sid != -1)
{
// SPI
#ifdef HAVE_PORTREG
*csport |= cspinmask;
*dcport |= dcpinmask;
*csport &= ~cspinmask;
#else
digitalWrite(cs, HIGH);
digitalWrite(dc, HIGH);
digitalWrite(cs, LOW);
#endif
for (uint16_t i=0; i<(SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8); i++) {
fastSPIwrite(buffer[i]);
}
#ifdef HAVE_PORTREG
*csport |= cspinmask;
#else
digitalWrite(cs, HIGH);
#endif
}
else
{
// save I2C bitrate
#ifdef TWBR
uint8_t twbrbackup = TWBR;
TWBR = 12; // upgrade to 400KHz!
#endif
//Serial.println(TWBR, DEC);
//Serial.println(TWSR & 0x3, DEC);
// I2C
for (uint16_t i=0; i<(SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8); i++) {
// send a bunch of data in one xmission
Wire.beginTransmission(_i2caddr);
WIRE_WRITE(0x40);
for (uint8_t x=0; x<16; x++) {
WIRE_WRITE(buffer[i]);
i++;
}
i--;
Wire.endTransmission();
}
#ifdef TWBR
TWBR = twbrbackup;
#endif
}
}
// clear everything
void Adafruit_SSD1306::clearDisplay(void) {
memset(buffer, 0, (SSD1306_LCDWIDTH*SSD1306_LCDHEIGHT/8));
}
inline void Adafruit_SSD1306::fastSPIwrite(uint8_t d) {
if(hwSPI) {
(void)SPI.transfer(d);
} else {
for(uint8_t bit = 0x80; bit; bit >>= 1) {
#ifdef HAVE_PORTREG
*clkport &= ~clkpinmask;
if(d & bit) *mosiport |= mosipinmask;
else *mosiport &= ~mosipinmask;
*clkport |= clkpinmask;
#else
digitalWrite(sclk, LOW);
if(d & bit) digitalWrite(sid, HIGH);
else digitalWrite(sid, LOW);
digitalWrite(sclk, HIGH);
#endif
}
}
}
void Adafruit_SSD1306::drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color) {
boolean bSwap = false;
switch(rotation) {
case 0:
// 0 degree rotation, do nothing
break;
case 1:
// 90 degree rotation, swap x & y for rotation, then invert x
bSwap = true;
ssd1306_swap(x, y);
x = WIDTH - x - 1;
break;
case 2:
// 180 degree rotation, invert x and y - then shift y around for height.
x = WIDTH - x - 1;
y = HEIGHT - y - 1;
x -= (w-1);
break;
case 3:
// 270 degree rotation, swap x & y for rotation, then invert y and adjust y for w (not to become h)
bSwap = true;
ssd1306_swap(x, y);
y = HEIGHT - y - 1;
y -= (w-1);
break;
}
if(bSwap) {
drawFastVLineInternal(x, y, w, color);
} else {
drawFastHLineInternal(x, y, w, color);
}
}
void Adafruit_SSD1306::drawFastHLineInternal(int16_t x, int16_t y, int16_t w, uint16_t color) {
// Do bounds/limit checks
if(y < 0 || y >= HEIGHT) { return; }
// make sure we don't try to draw below 0
if(x < 0) {
w += x;
x = 0;
}
// make sure we don't go off the edge of the display
if( (x + w) > WIDTH) {
w = (WIDTH - x);
}
// if our width is now negative, punt
if(w <= 0) { return; }
// set up the pointer for movement through the buffer
register uint8_t *pBuf = buffer;
// adjust the buffer pointer for the current row
pBuf += ((y/8) * SSD1306_LCDWIDTH);
// and offset x columns in
pBuf += x;
register uint8_t mask = 1 << (y&7);
switch (color)
{
case WHITE: while(w--) { *pBuf++ |= mask; }; break;
case BLACK: mask = ~mask; while(w--) { *pBuf++ &= mask; }; break;
case INVERSE: while(w--) { *pBuf++ ^= mask; }; break;
}
}
void Adafruit_SSD1306::drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color) {
bool bSwap = false;
switch(rotation) {
case 0:
break;
case 1:
// 90 degree rotation, swap x & y for rotation, then invert x and adjust x for h (now to become w)
bSwap = true;
ssd1306_swap(x, y);
x = WIDTH - x - 1;
x -= (h-1);
break;
case 2:
// 180 degree rotation, invert x and y - then shift y around for height.
x = WIDTH - x - 1;
y = HEIGHT - y - 1;
y -= (h-1);
break;
case 3:
// 270 degree rotation, swap x & y for rotation, then invert y
bSwap = true;
ssd1306_swap(x, y);
y = HEIGHT - y - 1;
break;
}
if(bSwap) {
drawFastHLineInternal(x, y, h, color);
} else {
drawFastVLineInternal(x, y, h, color);
}
}
void Adafruit_SSD1306::drawFastVLineInternal(int16_t x, int16_t __y, int16_t __h, uint16_t color) {
// do nothing if we're off the left or right side of the screen
if(x < 0 || x >= WIDTH) { return; }
// make sure we don't try to draw below 0
if(__y < 0) {
// __y is negative, this will subtract enough from __h to account for __y being 0
__h += __y;
__y = 0;
}
// make sure we don't go past the height of the display
if( (__y + __h) > HEIGHT) {
__h = (HEIGHT - __y);
}
// if our height is now negative, punt
if(__h <= 0) {
return;
}
// this display doesn't need ints for coordinates, use local byte registers for faster juggling
register uint8_t y = __y;
register uint8_t h = __h;
// set up the pointer for fast movement through the buffer
register uint8_t *pBuf = buffer;
// adjust the buffer pointer for the current row
pBuf += ((y/8) * SSD1306_LCDWIDTH);
// and offset x columns in
pBuf += x;
// do the first partial byte, if necessary - this requires some masking
register uint8_t mod = (y&7);
if(mod) {
// mask off the high n bits we want to set
mod = 8-mod;
// note - lookup table results in a nearly 10% performance improvement in fill* functions
// register uint8_t mask = ~(0xFF >> (mod));
static uint8_t premask[8] = {0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xFE };
register uint8_t mask = premask[mod];
// adjust the mask if we're not going to reach the end of this byte
if( h < mod) {
mask &= (0XFF >> (mod-h));
}
switch (color)
{
case WHITE: *pBuf |= mask; break;
case BLACK: *pBuf &= ~mask; break;
case INVERSE: *pBuf ^= mask; break;
}
// fast exit if we're done here!
if(h<mod) { return; }
h -= mod;
pBuf += SSD1306_LCDWIDTH;
}
// write solid bytes while we can - effectively doing 8 rows at a time
if(h >= 8) {
if (color == INVERSE) { // separate copy of the code so we don't impact performance of the black/white write version with an extra comparison per loop
do {
*pBuf=~(*pBuf);
// adjust the buffer forward 8 rows worth of data
pBuf += SSD1306_LCDWIDTH;
// adjust h & y (there's got to be a faster way for me to do this, but this should still help a fair bit for now)
h -= 8;
} while(h >= 8);
}
else {
// store a local value to work with
register uint8_t val = (color == WHITE) ? 255 : 0;
do {
// write our value in
*pBuf = val;
// adjust the buffer forward 8 rows worth of data
pBuf += SSD1306_LCDWIDTH;
// adjust h & y (there's got to be a faster way for me to do this, but this should still help a fair bit for now)
h -= 8;
} while(h >= 8);
}
}
// now do the final partial byte, if necessary
if(h) {
mod = h & 7;
// this time we want to mask the low bits of the byte, vs the high bits we did above
// register uint8_t mask = (1 << mod) - 1;
// note - lookup table results in a nearly 10% performance improvement in fill* functions
static uint8_t postmask[8] = {0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F };
register uint8_t mask = postmask[mod];
switch (color)
{
case WHITE: *pBuf |= mask; break;
case BLACK: *pBuf &= ~mask; break;
case INVERSE: *pBuf ^= mask; break;
}
}
}
================================================
FILE: firmware/stepper_nano_zero/Adafruit_SSD1306.h
================================================
/*********************************************************************
This is a library for our Monochrome OLEDs based on SSD1306 drivers
Pick one up today in the adafruit shop!
------> http://www.adafruit.com/category/63_98
These displays use SPI to communicate, 4 or 5 pins are required to
interface
Adafruit invests time and resources providing this open source code,
please support Adafruit and open-source hardware by purchasing
products from Adafruit!
Written by Limor Fried/Ladyada for Adafruit Industries.
BSD license, check license.txt for more information
All text above, and the splash screen must be included in any redistribution
*********************************************************************/
#ifndef _Adafruit_SSD1306_H_
#define _Adafruit_SSD1306_H_
#if ARDUINO >= 100
#include "Arduino.h"
#define WIRE_WRITE Wire.write
#else
#include "WProgram.h"
#define WIRE_WRITE Wire.send
#endif
#if defined(__SAM3X8E__)
typedef volatile RwReg PortReg;
typedef uint32_t PortMask;
#define HAVE_PORTREG
#elif defined(ARDUINO_ARCH_SAMD)
// not supported
#elif defined(ESP8266) || defined(ARDUINO_STM32_FEATHER)
typedef volatile uint32_t PortReg;
typedef uint32_t PortMask;
#else
typedef volatile uint8_t PortReg;
typedef uint8_t PortMask;
#define HAVE_PORTREG
#endif
#include <SPI.h>
#include "Adafruit_GFX.h"
#define BLACK 0
#define WHITE 1
#define INVERSE 2
#define SSD1306_I2C_ADDRESS 0x3C // 011110+SA0+RW - 0x3C or 0x3D
// Address for 128x32 is 0x3C
// Address for 128x64 is 0x3D (default) or 0x3C (if SA0 is grounded)
/*=========================================================================
SSD1306 Displays
-----------------------------------------------------------------------
The driver is used in multiple displays (128x64, 128x32, etc.).
Select the appropriate display below to create an appropriately
sized framebuffer, etc.
SSD1306_128_64 128x64 pixel display
SSD1306_128_32 128x32 pixel display
SSD1306_96_16
-----------------------------------------------------------------------*/
#define SSD1306_128_64
// #define SSD1306_128_32
// #define SSD1306_96_16
/*=========================================================================*/
#if defined SSD1306_128_64 && defined SSD1306_128_32
#error "Only one SSD1306 display can be specified at once in SSD1306.h"
#endif
#if !defined SSD1306_128_64 && !defined SSD1306_128_32 && !defined SSD1306_96_16
#error "At least one SSD1306 display must be specified in SSD1306.h"
#endif
#if defined SSD1306_128_64
#define SSD1306_LCDWIDTH 128
#define SSD1306_LCDHEIGHT 64
#endif
#if defined SSD1306_128_32
#define SSD1306_LCDWIDTH 128
#define SSD1306_LCDHEIGHT 32
#endif
#if defined SSD1306_96_16
#define SSD1306_LCDWIDTH 96
#define SSD1306_LCDHEIGHT 16
#endif
#define SSD1306_SETCONTRAST 0x81
#define SSD1306_DISPLAYALLON_RESUME 0xA4
#define SSD1306_DISPLAYALLON 0xA5
#define SSD1306_NORMALDISPLAY 0xA6
#define SSD1306_INVERTDISPLAY 0xA7
#define SSD1306_DISPLAYOFF 0xAE
#define SSD1306_DISPLAYON 0xAF
#define SSD1306_SETDISPLAYOFFSET 0xD3
#define SSD1306_SETCOMPINS 0xDA
#define SSD1306_SETVCOMDETECT 0xDB
#define SSD1306_SETDISPLAYCLOCKDIV 0xD5
#define SSD1306_SETPRECHARGE 0xD9
#define SSD1306_SETMULTIPLEX 0xA8
#define SSD1306_SETLOWCOLUMN 0x00
#define SSD1306_SETHIGHCOLUMN 0x10
#define SSD1306_SETSTARTLINE 0x40
#define SSD1306_MEMORYMODE 0x20
#define SSD1306_COLUMNADDR 0x21
#define SSD1306_PAGEADDR 0x22
#define SSD1306_COMSCANINC 0xC0
#define SSD1306_COMSCANDEC 0xC8
#define SSD1306_SEGREMAP 0xA0
#define SSD1306_CHARGEPUMP 0x8D
#define SSD1306_EXTERNALVCC 0x1
#define SSD1306_SWITCHCAPVCC 0x2
// Scrolling #defines
#define SSD1306_ACTIVATE_SCROLL 0x2F
#define SSD1306_DEACTIVATE_SCROLL 0x2E
#define SSD1306_SET_VERTICAL_SCROLL_AREA 0xA3
#define SSD1306_RIGHT_HORIZONTAL_SCROLL 0x26
#define SSD1306_LEFT_HORIZONTAL_SCROLL 0x27
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL 0x2A
class Adafruit_SSD1306 : public Adafruit_GFX {
public:
Adafruit_SSD1306(int8_t SID, int8_t SCLK, int8_t DC, int8_t RST, int8_t CS);
Adafruit_SSD1306(int8_t DC, int8_t RST, int8_t CS);
Adafruit_SSD1306(int8_t RST = -1);
bool begin(uint8_t switchvcc = SSD1306_SWITCHCAPVCC, uint8_t i2caddr = SSD1306_I2C_ADDRESS, bool reset=true);
uint8_t ssd1306_command(uint8_t c);
void clearDisplay(void);
void invertDisplay(uint8_t i);
void display();
void startscrollright(uint8_t start, uint8_t stop);
void startscrollleft(uint8_t start, uint8_t stop);
void startscrolldiagright(uint8_t start, uint8_t stop);
void startscrolldiagleft(uint8_t start, uint8_t stop);
void stopscroll(void);
void dim(boolean dim);
void drawPixel(int16_t x, int16_t y, uint16_t color);
virtual void drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color);
virtual void drawFastHLine(int16_t x, int16_t y, int16_t w, uint16_t color);
private:
int8_t _i2caddr, _vccstate, sid, sclk, dc, rst, cs;
void fastSPIwrite(uint8_t c);
boolean hwSPI;
#ifdef HAVE_PORTREG
PortReg *mosiport, *clkport, *csport, *dcport;
PortMask mosipinmask, clkpinmask, cspinmask, dcpinmask;
#endif
inline void drawFastVLineInternal(int16_t x, int16_t y, int16_t h, uint16_t color) __attribute__((always_inline));
inline void drawFastHLineInternal(int16_t x, int16_t y, int16_t w, uint16_t color) __attribute__((always_inline));
};
#endif /* _Adafruit_SSD1306_H_ */
================================================
FILE: firmware/stepper_nano_zero/Flash.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "Flash.h"
#include "syslog.h"
bool flashInit(void){
if (NVMCTRL->PARAM.bit.PSZ != 3)
{
ERROR("FLASH PAGE SIZE is not 64 bytes");
return false;
}
return true;
}
static void erase(const volatile void *flash_ptr)
{
NVMCTRL->ADDR.reg = ((uint32_t)flash_ptr) / 2;
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER;
while (!NVMCTRL->INTFLAG.bit.READY) { }
}
bool flashErase(const volatile void *flash_ptr, uint32_t size)
{
const uint8_t *ptr = (const uint8_t *)flash_ptr;
while (size > FLASH_ROW_SIZE) {
erase(ptr);
ptr += FLASH_ROW_SIZE;
size -= FLASH_ROW_SIZE;
}
if (size>0)
{
erase(ptr);
}
return true; //TODO should verify the erase
}
static inline uint32_t read_unaligned_uint32(const void *data)
{
union {
uint32_t u32;
uint8_t u8[4];
} res;
const uint8_t *d = (const uint8_t *)data;
res.u8[0] = d[0];
res.u8[1] = d[1];
res.u8[2] = d[2];
res.u8[3] = d[3];
return res.u32;
}
void flashWrite(const volatile void *flash_ptr,const void *data, uint32_t size)
{
uint32_t *ptrPage;
uint8_t *destPtr;
uint8_t *srcPtr;
uint32_t bytesInBlock;
__attribute__((__aligned__(4))) uint8_t buffer[FLASH_ROW_SIZE];
uint32_t offset;
destPtr=(uint8_t *)flash_ptr;
srcPtr=(uint8_t *)data;
//LOG("flash write called");
while(size>0)
{
uint32_t i,j;
//calculate the maximum number of bytes we can write in page
offset=((uint32_t)destPtr)%(FLASH_ROW_SIZE); //offset into page
bytesInBlock=FLASH_ROW_SIZE-offset; //this is how many bytes we need to overwrite in this page
//LOG("offset %d, bytesInBlock %d size %d", offset, bytesInBlock,size);
//get pointer to start of page
ptrPage=(uint32_t *) ((((uint32_t)destPtr)/(FLASH_ROW_SIZE)) * FLASH_ROW_SIZE);
//LOG("pointer to page %d(0x%08x) %d",(uint32_t)ptrPage,(uint32_t)ptrPage,destPtr);
//fill page buffer with data from flash
memcpy(buffer,ptrPage,FLASH_ROW_SIZE);
//now fill buffer with new data that needs changing
i=bytesInBlock;
if (size<i)
{
i=size;
}
//LOG("changing %d bytes",i);
memcpy(&buffer[offset],srcPtr,i);
//erase page
flashErase(ptrPage,FLASH_ROW_SIZE);
//write new data to flash
flashWritePage(ptrPage,buffer,FLASH_ROW_SIZE);
uint32_t *ptr=(uint32_t *)buffer;
for (j=0; j<FLASH_ROW_SIZE/4; j++)
{
if (*ptrPage != *ptr)
{
ERROR("write failed on byte %d %x %x",j,*ptrPage, *ptr);
}
ptrPage++;
ptr++;
}
size=size-i; //decrease number of bytes to write
srcPtr+=i; //increase pointer to next bytes to read
destPtr+=i; //increment destination pointer
}
}
void flashWritePage(const volatile void *flash_ptr, const void *data, uint32_t size)
{
// Calculate data boundaries
size = (size + 3) / 4; //convert bytes to words with rounding
volatile uint32_t *dst_addr = (volatile uint32_t *)flash_ptr;
const uint8_t *src_addr = (uint8_t *)data;
if (0 != ((uint32_t)flash_ptr)%(FLASH_PAGE_SIZE))
{
ERROR("Flash page write must be on boundry");
return;
}
// Disable automatic page write
NVMCTRL->CTRLB.bit.MANW = 1;
// Do writes in pages
while (size)
{
// Execute "PBC" Page Buffer Clear
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_PBC;
while (NVMCTRL->INTFLAG.bit.READY == 0) { }
// Fill page buffer
uint32_t i;
for (i=0; i<(FLASH_PAGE_SIZE/4) && size; i++) //we write 4 bytes at a time
{
*dst_addr = read_unaligned_uint32(src_addr);
src_addr += 4;
dst_addr++;
size--; //size is set to number of 32bit words in first line above
}
// Execute "WP" Write Page
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP;
while (NVMCTRL->INTFLAG.bit.READY == 0) { }
}
}
================================================
FILE: firmware/stepper_nano_zero/Flash.h
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef __FLASH__H__
#define __FLASH__H__
#include <Arduino.h>
#include "syslog.h"
#define FLASH_PAGE_SIZE_NZS (64) //bytes
#define FLASH_ROW_SIZE (FLASH_PAGE_SIZE_NZS*4) //defined in the datasheet as 4x page size
#define FLASH_ERASE_VALUE (0xFF) //value of flash after an erase
#define FLASH_ALLOCATE(name, size) \
__attribute__((__aligned__(FLASH_ROW_SIZE))) \
const uint8_t name[(size+(FLASH_ROW_SIZE-1))/FLASH_ROW_SIZE*FLASH_ROW_SIZE] = { };
bool flashInit(void); //this checks that our assumptions are true
bool flashErase(const volatile void *flash_ptr, uint32_t size);
void flashWrite(const volatile void *flash_ptr,const void *data,uint32_t size);
void flashWritePage(const volatile void *flash_ptr, const void *data, uint32_t size);
//you can read by dereferencing pointer but we will add a read
static inline int32_t flashRead(const volatile void *flash_ptr, void *data, uint32_t size)
{
memcpy(data, (const void *)flash_ptr, size);
}
#endif //__FLASH__H__
================================================
FILE: firmware/stepper_nano_zero/angle.h
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef ANGLE_H_
#define ANGLE_H_
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#define ANGLE_STEPS (0x010000UL)
#define ANGLE_MAX ((uint16_t)0x0FFFF)
#define ANGLE_FROM_DEGREES(x) ((int32_t) ( ((float)ANGLE_STEPS*(float)(x)+180.0)/360.0 ) )
#define ANGLE_T0_DEGREES(x) ( (float) ((float(x)*360.0)/((float)ANGLE_STEPS) ))
class Angle
{
private:
uint16_t angle;
public:
Angle(void) {angle=0;}
Angle(int32_t x) {angle=(uint16_t)x;}
Angle(const Angle &x) {angle=x.angle;}
int16_t operator-( const Angle &a2)
{
int32_t x,y,dx;
x=(int32_t)angle;
y=(int32_t)a2.angle;
dx=x-y;
if (abs(x-y)>ANGLE_STEPS/2)
{
//we have a wrap condition
if (x>y)
{
dx=x-(y+ANGLE_STEPS);
}else if (x<y)
{
dx=(ANGLE_STEPS+x)-y;
}
}
return (int16_t)dx;
}
//
// int16_t operator-( const int32_t y)
// {
// int32_t x,y,dx;
// x=(int32_t)angle;
// while(y>ANGLE_MAX)
// {
// y=y-ANGLE_STEPS;
// }
// while(y<-ANGLE_MAX)
// {
// y=y+ANGLE_STEPS;
// }
//
// dx=x-y;
// if (abs(x-y)>ANGLE_STEPS/2)
// {
// //we have a wrap condition
// if (x>y)
// {
// dx=x-(y+ANGLE_STEPS);
// }else if (x<y)
// {
// dx=(ANGLE_STEPS+x)-y;
// }
// }
// return (int16_t)dx;
// }
Angle operator+(const Angle &y)
{
uint16_t a;
a=angle+ (uint16_t)y.angle;
return Angle(a);
}
Angle operator+(const long int x)
{
int32_t a;
a=(int32_t)angle+ x;
while (a>=ANGLE_STEPS)
{
a=a-ANGLE_STEPS;
}
while (a<0)
{
a=a+ANGLE_STEPS;
}
return Angle((uint16_t)a);
}
Angle operator+(const unsigned long int x)
{
uint32_t a;
a=(uint32_t)angle+ x;
while (a>=ANGLE_STEPS)
{
a=a-ANGLE_STEPS;
}
return Angle((uint16_t)a);
}
operator uint16_t() const {return angle;}
operator uint32_t() const {return (uint32_t)angle;}
operator int32_t() const {return (int32_t)angle;}
};
#endif /* ANGLE_H_ */
================================================
FILE: firmware/stepper_nano_zero/as5047d.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include <Arduino.h>
#include "syslog.h"
#include "as5047d.h"
#include "SPI.h"
#include <stdio.h>
#include "board.h"
#define AS5047D_CMD_NOP (0x0000)
#define AS5047D_CMD_ERRFL (0x0001)
#define AS5047D_CMD_PROG (0x0003)
#define AS5047D_CMD_DIAAGC (0x3FFC)
#define AS5047D_CMD_MAG (0x3FFD)
#define AS5047D_CMD_ANGLEUNC (0x3FFE)
#define AS5047D_CMD_ANGLECOM (0x3FFF)
#define AS5048A_CMD_NOP (0x0000)
#define AS5048A_CMD_ERRFL (0x0001)
#define AS5048A_CMD_PROG (0x0003)
#define AS5048A_CMD_DIAAGC (0x3FFD)
#define AS5048A_CMD_MAG (0x3FFE)
#define AS5048A_CMD_ANGLE (0x3FFF)
#pragma GCC push_options
#pragma GCC optimize ("-Ofast")
static int getBit(int16_t data, int bit)
{
return (data>>bit) & 0x01;
}
static int getParity(uint16_t data)
{
int i,bits;
data=data & 0x7FFF; //mask out upper bit
//count number of bits, brute force
bits=0;
for(i=0; i<16; i++)
{
if (0 != (data & ((0x0001)<<i)))
{
bits++;
}
}
return (bits & 0x01); //return 1 if odd
}
boolean AS5047D::begin(int csPin)
{
#ifdef PIN_AS5047D_PWR
digitalWrite(PIN_AS5047D_PWR,HIGH);
#endif
digitalWrite(PIN_AS5047D_CS,LOW); //pull CS LOW by default (chip powered off)
digitalWrite(PIN_MOSI,LOW);
digitalWrite(PIN_SCK,LOW);
digitalWrite(PIN_MISO,LOW);
pinMode(PIN_MISO,OUTPUT);
delay(1000);
digitalWrite(PIN_AS5047D_CS,HIGH); //pull CS high
#ifdef PIN_AS5047D_PWR
digitalWrite(PIN_AS5047D_PWR,LOW);
#endif
pinMode(PIN_MISO,INPUT);
error=false;
SPISettings settingsA(5000000, MSBFIRST, SPI_MODE1); ///400000, MSBFIRST, SPI_MODE1);
chipSelectPin=csPin;
LOG("csPin is %d",csPin);
pinMode(chipSelectPin,OUTPUT);
digitalWrite(chipSelectPin,HIGH); //pull CS high by default
delay(1);
SPI.begin(); //AS5047D SPI uses mode=1 (CPOL=0, CPHA=1)
LOG("Begin AS5047D...");
SPI.beginTransaction(settingsA);
SPI.transfer16(AS5047D_CMD_NOP);
delay(10);
//wait for the LF bit to be set
uint16_t data=0,t0=100;
while (getBit(data,8)==0 && t0>0)
{
delay(1);
t0--;
if (t0==0)
{
ERROR("LF bit not set");
error=true;
break;
//return false;
}
LOG("AS5047D diag data is 0x%04X",data);
data=readAddress(AS5047D_CMD_DIAAGC);
}
if (error)
{
error=false;
uint16_t data=0,t0=100;
while (getBit(data,8)==0 && t0>0)
{
delay(1);
t0--;
if (t0==0)
{
ERROR("AS5048A OCF bit not set");
error=true;
return false;
}
data=readAddress(AS5048A_CMD_DIAAGC);
LOG("AS5048A diag data is 0x%04X",data);
}
as5047d=false;
}
#ifdef NZS_AS5047_PIPELINE
//read encoder a few times to flush the pipeline
readEncoderAnglePipeLineRead();
readEncoderAnglePipeLineRead();
#endif
return true;
}
//read the encoders
int16_t AS5047D::readAddress(uint16_t addr)
{
uint16_t data;
error=false;
//make sure it is a read by setting bit 14
addr=addr | 0x4000;
//add the parity to the command
if (1 == getParity(addr))
{
addr=(addr & 0x7FFF) | 0x8000; //add parity bit to make command even number of bits
}
digitalWrite(chipSelectPin, LOW);
delayMicroseconds(1);
//clock out the address to read
SPI.transfer16(addr);
digitalWrite(chipSelectPin, HIGH);
delayMicroseconds(1);
digitalWrite(chipSelectPin, LOW);
//clock out zeros to read in the data from address
data=SPI.transfer16(0x00);
digitalWrite(chipSelectPin, HIGH);
if (data & (1<<14))
{
//if bit 14 is set then we have an error
ERROR("read command 0x%04X failed",addr);
error=true;
return -1;
}
if (data>>15 != getParity(data))
{
//parity did not match
ERROR("read command parity error 0x%04X ",addr);
error=true;
return -2;
}
data=data & 0x3FFF; //mask off the error and parity bits
return data;
}
//read the encoders
int16_t AS5047D::readEncoderAngle(void)
{
if (as5047d)
{
return readAddress(AS5047D_CMD_ANGLECOM);
}
return readAddress(AS5048A_CMD_ANGLE);
}
//pipelined read of the encoder angle used for high speed reads, but value is always one read behind
int16_t AS5047D::readEncoderAnglePipeLineRead(void)
{
int16_t data;
int error, t0=10;
GPIO_LOW(chipSelectPin);//(chipSelectPin, LOW);
//delayMicroseconds(1);
do {
// doing two 8 bit transfers is faster than one 16 bit
data =(uint16_t)SPI.transfer(0xFF)<<8 | ((uint16_t)SPI.transfer(0xFF) & 0x0FF);
t0--;
if (t0<=0)
{
ERROR("AS5047D problem");
break;
}
//data=SPI.transfer16(0xFFFF); //to speed things up we know the parity and address for the read
}while(data & (1<<14)); //while error bit is set
data=data & 0x3FFF; //mask off the error and parity bits
GPIO_HIGH(chipSelectPin);
//digitalWrite(chipSelectPin, HIGH);
//TODO we really should check for errors and return a negative result or something
return data;
}
void AS5047D::diagnostics(char *ptrStr)
{
int16_t data;
int m,d;
if (as5047d)
{
data=readAddress(AS5047D_CMD_DIAAGC);
if (NULL == ptrStr)
{
LOG("DIAAGC: 0x%04X", data);
LOG("MAGL: %d", getBit(data,11));
LOG("MAGH: %d", getBit(data,10));
LOG("COF: %d", getBit(data,9));
LOG("LFGL: %d", getBit(data,8));
LOG("AGC: %d", data & 0x0FF);
data=readAddress(AS5047D_CMD_MAG);
LOG("CMAG: 0x%04X(%d)",data,data);
data=readAddress(AS5047D_CMD_ANGLEUNC);
m=(int)((float)data*AS5047D_DEGREES_PER_BIT);
d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100);
LOG("CORDICANG: 0x%04X(%d) %d.%02d deg(est)",data,data,m,d);
data=readAddress(AS5047D_CMD_ANGLECOM);
m=(int)((float)data*AS5047D_DEGREES_PER_BIT);
d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100);
LOG("DAECANG: 0x%04X(%d) %d.%02d deg(est)",data,data,m,d);
}else
{
sprintf(ptrStr,"DIAAGC: 0x%04X\n\r", data);
sprintf(ptrStr,"%sMAGL: %d\n\r", ptrStr,getBit(data,11));
sprintf(ptrStr,"%sMAGH: %d\n\r", ptrStr,getBit(data,10));
sprintf(ptrStr,"%sCOF: %d\n\r", ptrStr, getBit(data,9));
sprintf(ptrStr,"%sLFGL: %d\n\r", ptrStr, getBit(data,8));
sprintf(ptrStr,"%sAGC: %d\n\r", ptrStr,data & 0x0FF);
data=readAddress(AS5047D_CMD_MAG);
sprintf(ptrStr,"%sCMAG: 0x%04X(%d)\n\r", ptrStr,data,data);
data=readAddress(AS5047D_CMD_ANGLEUNC);
m=(int)((float)data*AS5047D_DEGREES_PER_BIT);
d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100);
sprintf(ptrStr,"%sCORDICANG: 0x%04X(%d) %d.%02d deg(est)\n\r", ptrStr,data,data,m,d);
data=readAddress(AS5047D_CMD_ANGLECOM);
m=(int)((float)data*AS5047D_DEGREES_PER_BIT);
d=(int)((float)data*AS5047D_DEGREES_PER_BIT*100 -m*100);
sprintf(ptrStr,"%sDAECANG: 0x%04X(%d) %d.%02d deg(est)\n\r", ptrStr,data,data,m,d);
}
} else
{
data=readAddress(AS5048A_CMD_DIAAGC);
sprintf(ptrStr,"AS5048A DIAAGC: 0x%04X\n\r", data);
data=readAddress(AS5048A_CMD_MAG);
sprintf(ptrStr,"%sMagnitude: %d\n\r", ptrStr,data);
data=readAddress(AS5048A_CMD_ANGLE);
sprintf(ptrStr,"%sAngle: %d\n\r", ptrStr,data);
}
}
#pragma GCC pop_options
================================================
FILE: firmware/stepper_nano_zero/as5047d.h
================================================
/**********************************************************************
Copyright (C) 2019 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef __AS5047D_H__
#define __AS5047D_H__
#include <Arduino.h>
#define AS5047D_DEGREES_PER_BIT (360.0/(float)(0x3FFF))
class AS5047D {
private:
int chipSelectPin;
int16_t readAddress(uint16_t addr);
bool error=false;
bool as5047d=true;
public:
boolean begin(int csPin);
int16_t readEncoderAngle(void);
void diagnostics(char *ptrStr);
int16_t readEncoderAnglePipeLineRead(void);
bool getError(void) {return error;};
};
#endif //__AS5047D_H__
================================================
FILE: firmware/stepper_nano_zero/board.h
================================================
/**********************************************************************
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef __BOARD_H__
#define __BOARD_H__
#include <Arduino.h>
//uncomment this if you are using the Mechaduino hardware
//#define MECHADUINO_HARDWARE
//uncomment the follow lines if using the NEMA 23 10A hardware
//#define NEMA_23_10A_HW
//uncomment the following if the board uses the A5995 driver (NEMA 23 3.2A boards)
//#define A5995_DRIVER
//The March 21 2017 NEMA 17 Smart Stepper has changed some pin outs
// A1 was changed to read motor voltage, hence SW4 is now using D4
// comment out this next line if using the older hardware
#define NEMA17_SMART_STEPPER_3_21_2017
#if defined(MECHADUINO_HARDWARE) && defined(NEMA17_SMART_STEPPER_3_21_2017)
#error "Cannot have both MECHADUINO_HARDWARE and NEMA17_SMART_STEPPER_3_21_2017 defined in board.h"
#endif
//The MKS Servo42 uses the A1333_Encoder
// Please uncomment this line and make sure the NEMA17_SMART_STEPPER_3_21_2017 is
// uncommented for the Servo42
//#define A1333_ENCODER
#ifdef A5995_DRIVER
#ifdef NEMA17_SMART_STEPPER_3_21_2017
#error "Only NEMA17_SMART_STEPPER_3_21_2017 or A5595_DRIVER may be defined"
#endif
#endif
#define NZS_FAST_CAL // define this to use 32k of flash for fast calibration table
#define NZS_FAST_SINE //uses 2048 extra bytes to implement faster sine tables
#define NZS_AS5047_PIPELINE //does a pipeline read of encoder, which is slightly faster
#define NZS_CONTROL_LOOP_HZ (6000) //update rate of control loop
#define NZS_LCD_ABSOULTE_ANGLE //define this to show angle from zero in positive and negative direction
// for example 2 rotations from start will be angle of 720 degrees
//#define ENABLE_PHASE_PREDICTION //this enables prediction of phase at high velocity to increase motor speed
//as of FW0.11 it is considered development only
#define VERSION "FW: 0.40" //this is what prints on LCD during splash screen
//Define this to allow command out serial port, else hardware serial is debug log
//#define CMD_SERIAL_PORT
#define SERIAL_BAUD (115200) //baud rate for the serial ports
//This section is for using the step and dir pins as serial port
// when the enable pin is inactive.
#ifndef MECHADUINO_HARDWARE
#define USE_STEP_DIR_SERIAL
#define STEP_DIR_BAUD (19200) //this is the baud rate we will use
#endif
// These are used as an attempt to use TC4 to count steps
// currently this is not working.
//#define USE_NEW_STEP //define this to use new step method
#define USE_TC_STEP //use timer counter for step pin
#ifndef F_CPU
#define F_CPU (48000000UL)
#endif
/* TODO are flaged with TODO
* TODO - add detection of magnet to make sure PCB is on motor
*/
/* change log
* 0.02 added fixes for 0.9 degree motor
* 0.03 added code for using error pin as an enable pin, enable feedback by default
* 0.04
* 0.05 added different modes added support for mechaduino
* 0.06 added time out pipeline read, add some error logging on encoder failure for mechaduino
* 0.07 many changes including
* - fixed error on display when doing a move 99999
* - added velocity and position PID modes
* - fixed LCD menu and put LCD code in own file
* - include LCD source files from adafruit as that ssd1306 need lcd resoultion fix
* - added motor parameters to NVM such step size and rotation are only check on first boot
* - added test on power up to see if motor power is applied.
* - added factory reset command
* - pPID is not stable in my testing.
* 0.08
* - moved enable pin processing out of interrupt context
* - added mode for inverted logic on the enable pin
* - added pin definitions for NEMA23 10A hardware
* - Changed enable such that it does not move motor but just sets current posistion
* 0.09
* - enabled auto detection of LCD
* - cleaned up the commands, made motorparams and systemparams individual commands
* - added the option to the move command to move at a constant RPM
* - Added the setzero command to zero the relative location of motor
* - Added the stop command to stop the planner based moves.
* 0.10
* -Fixed bug in switching control mode to 3
* 0.11
* - Fixed bug where output current was half of what it should have been (sine.h)
* - Added #define for phase predictive advancement
* - Changed calibration to be done with one coil/phase on
* - Added smoothing for calibration
* - Continue to work on the Fet Driver code.
* 0.12
* - Continue to work on the FET driver code
* - fixed a constant issue with the DAC for the A4954 driver
* - added command for setting the operational mode of the enable pin
* - added the start of the A5995 driver.
* 0.13
* - Added delay in for the 0.9 degree motor calibration and testing
* - changed calibration to move 1/2 step at time as it was causing problems on A5995 due to current ramp down
* 0.14 - Added in data logging
* - Averaged the encoder when the motor is stationary to reduce noise/vibrations
* 0.15 - Fixed some fet driver code
* - Added support for the NEMA17 smart stepper
* - Fixed RPM display bug on the LCD
* 0.16 - Added support for enable and error pins on the 3-21-2017 hardware
*
* 0.17 - Added the ability for the command line to go over the hardwired serial port
* - Fixed a bug where step and direction pin were setup as pulled down pins
* which could cause false stepping in nosiey environments
* 0.18 - Added support for EEPROM writting of last location during brown out - currently brown out is too fast to write
* - Added commands to support reading and restoring location from eeprom
* - Check for pull up on SDA/SCL before doing a I2C read as that SERCOM driver has not time outs and locks.
* - Added faster detection of USB not being plugged in, reduces power up time with no USB
* 0.19 - removed debug information in the ssd1306 driver which caused LCD not always to be found
* 0.20 - Fixed bug in calibration, thanks to Oliver E.
* 0.21 - Fixed issues compiling for mechaduino, including disabling LCD for MEchaduino
* 0.22 - Added home command;
* 0.23 -- added motor voltage sense to remove stepping on power up
* 0.24 - Disabled the home command which used the enable pin if you do not have enable pin
* 0.25 - Added pin read command
* 0.26 - changed the step/dir pins to be input_pullups
* 0.27 - added the option to make the step/dir uart when enable is low.
* - fixed enable to line to disable the A4954 driver
* 0.28 - Enabled some homing options (still under development)
* 0.29 - fixed rounding bug in ANGLE_T0_DEGREES
* 0.30 - Added support for the AS5048A encoder
* 0.31 - Added reading enable pin on during main loop
* 0.32 - Fixed issue where steps were not being counted correctly
* 0.33 - changed sPID parameters back to 0.9 0.0001 0.01
* 0.34 - Added board type to the splash screen
* 0.35 - fixed usign TC4 (USE_TC_STEP) for counting steps. We can measure steps
* - at over 125khz, however the dir pin has ~8us setup time due to interrupt latency.
* - Added debug command to allow debug messages out the USB serial port
* 0.36 - eeprom set location math was wrong.
* 0.37 - fixed bug where the motor would pause periodically do the the TC4 counter.
* 0.38 - fixed bug in the velocity feedback mode.
* 0.39 - changed step count to TCC2, improved the dir pin setup/hold times
* - added support for the MKS Servo42 (A1333 encoder)
* 0.40 - fixed compiling errors for Mechaduino. Added sanity checks for different hardware boards (AK)
*/
/*
* Typedefs that are used across multiple files/modules
*/
typedef enum {
CW_ROTATION=0,
CCW_ROTATION=1,
} RotationDir_t;
typedef enum {
ERROR_PIN_MODE_ENABLE=0, //error pin works like enable on step sticks
ERROR_PIN_MODE_ACTIVE_LOW_ENABLE=1, //error pin works like enable on step sticks
ERROR_PIN_MODE_ERROR=2, //error pin is low when there is angle error
ERROR_PIN_MODE_BIDIR=3, //error pin is bidirection open collector
} ErrorPinMode_t;
typedef enum {
CTRL_OFF =0, //controller is disabled
CTRL_OPEN=1, //controller is in open loop mode
CTRL_SIMPLE = 2, //simple error controller
CTRL_POS_PID =3, //PID Position controller
CTRL_POS_VELOCITY_PID =4, //PID Velocity controller
} feedbackCtrl_t;
// ******** EVENT SYS USAGAE ************
// Channel 0 - Step pin event
// ******** TIMER USAGE A4954 versions ************
//TCC1 is used for DAC PWM to the A4954
//TCC0 can be used as PWM for the input pins on the A4954
//TCC2 is used for the step count
//D0 step input could use TCC1 or TCC0 if not used
//TC3 is used for planner tick
//TC5 is use for timing the control loop
// ******** TIMER USAGE NEMA23 10A versions ************
//TCC0 PWM for the FET IN pins
//D10 step input could use TC3 or TCC0 if not used
//TC3 is used for planner tick
//TC4 is used for step count
//TC5 is use for timing the control loop
//mechaduio and Arduino Zero has defined serial ports differently than NZS
#ifdef MECHADUINO_HARDWARE
#warning "Compiling source for Mechaduino NOT NZS"
#define DISABLE_LCD
#undef Serial5
#define Serial5 Serial
#else
#define SerialUSB Serial
#endif
#define PIN_TXD (30)
#define PIN_RXD (31)
#define PIN_STEP_INPUT (0)
#define PIN_DIR_INPUT (1)
#define PIN_MOSI (23)
#define PIN_SCK (24)
#define PIN_MISO (22)
#ifdef MECHADUINO_HARDWARE
#ifdef USE_STEP_DIR_SERIAL
#error "Step/Dir UART not supported on Mechaduino yet"
#endif
#define PIN_ERROR (19) //analogInputToDigitalPin(PIN_A5))
#else //not Mechaduino hardware
#ifdef NEMA17_SMART_STEPPER_3_21_2017
#define PIN_SW1 (19)//analogInputToDigitalPin(PIN_A5))
#define PIN_SW3 (14)//analogInputToDigitalPin(PIN_A0))
#ifdef A1333_ENCODER //the MKS Servo42 uses A1 for this switch
#define PIN_SW4 (15)//analogInputToDigitalPin(PIN_A1))
#else
#define PIN_SW4 (2)//D2
#endif
#define PIN_ENABLE (10)
#define PIN_ERROR (3)
#define PIN_VMOTOR (A1) //analog pin for the motor
#else
#define PIN_SW1 (19)//analogInputToDigitalPin(PIN_A5))
#define PIN_SW3 (14)//analogInputToDigitalPin(PIN_A0))
#define PIN_SW4 (15)//analogInputToDigitalPin(PIN_A1))
#define PIN_ERROR (10)
#endif
#endif
#ifdef A5995_DRIVER
#define PIN_ENABLE (3)
#endif
#define PIN_SCL (21)
#define PIN_SDA (20)
#define PIN_USB_PWR (38) // this pin is high when usb is connected
#define PIN_AS5047D_CS (16)//analogInputToDigitalPin(PIN_A2))
#ifndef MECHADUINO_HARDWARE
#define PIN_AS5047D_PWR (11) //pull low to power on AS5047D
#endif
//these pins use the TIMER in the A4954 driver
// changing the pin definitions here may require changes in the A4954.cpp file
#define PIN_FET_IN1 (5) //PA15 TC3/WO[1] TCC0/WO[5]1
#define PIN_FET_IN2 (6) //PA20 TC7/W0[0] TCC0/WO[6]2
#define PIN_FET_IN3 (7) //PA21 TC7/WO[1] TCC0/WO[7]3
#define PIN_FET_IN4 (2) //PA14 TC3/W0[0] TCC0/WO[4] 0
#define PIN_FET_VREF1 (4)
#define PIN_FET_VREF2 (3)
#define PIN_FET_ENABLE (12)
//current sense pin from each H-bridge
#define ISENSE_FET_A (17) //analogInputToDigitalPin(PIN_A3)
#define ISENSE_FET_B (8)
//Comparators analog inputs
//#define COMP_FET_A (18)//analogInputToDigitalPin(PIN_A4))
//#define COMP_FET_B (9)
//these are the pins used on the A5995 driver
#define PIN_A5995_ENABLE1 (2) //PA14
#define PIN_A5995_ENABLE2 (18) //PA05 analogInputToDigitalPin(PIN_A4))
#define PIN_A5995_MODE1 (8) //PA06 TCC1 WO[0]
#define PIN_A5995_MODE2 (7) //PA21 TCC0 WO[4] //3
#define PIN_A5995_PHASE1 (6) //PA20 TCC0 WO[6] //2
#define PIN_A5995_PHASE2 (5) //PA15 TCC0 W0[5] //1
#define PIN_A5995_VREF1 (4) //PA08
#define PIN_A5995_VREF2 (9) //PA07
#define PIN_A5995_SLEEPn (25) //RXLED
#ifndef MECHADUINO_HARDWARE
#define PIN_YELLOW_LED (8)
#endif
#ifdef NEMA_23_10A_HW
#undef PIN_YELLOW_LED
#define PIN_YELLOW_LED (26) //TXLED (PA27)
#endif //NEMA_23_10A_HW
#define PIN_RED_LED (13)
#define PIN_A4954_IN3 (5)
#define PIN_A4954_IN4 (6)
#define PIN_A4954_IN2 (7)
#ifdef MECHADUINO_HARDWARE
#define PIN_A4954_IN1 (8)
#else
#define PIN_A4954_IN1 (18) //analogInputToDigitalPin(PIN_A4))
#endif
#define PIN_A4954_VREF34 (4)
#define PIN_A4954_VREF12 (9)
//Here are some useful macros
#define DIVIDE_WITH_ROUND(x,y) (((x)+(y)/2)/(y))
#define GPIO_LOW(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].OUTCLR.reg = (1ul << g_APinDescription[(pin)].ulPin);}
#define GPIO_HIGH(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].OUTSET.reg = (1ul << g_APinDescription[(pin)].ulPin);}
#define GPIO_OUTPUT(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].PINCFG[g_APinDescription[(pin)].ulPin].reg &=~(uint8_t)(PORT_PINCFG_INEN) ; PORT->Group[g_APinDescription[(pin)].ulPort].DIRSET.reg = (uint32_t)(1<<g_APinDescription[(pin)].ulPin) ;}
#define PIN_GPIO_OUTPUT(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].PINCFG[g_APinDescription[(pin)].ulPin].reg &=~(uint8_t)(PORT_PINCFG_INEN | PORT_PINCFG_PMUXEN) ; PORT->Group[g_APinDescription[(pin)].ulPort].DIRSET.reg = (uint32_t)(1<<g_APinDescription[(pin)].ulPin) ;}
#define PIN_GPIO(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].PINCFG[g_APinDescription[(pin)].ulPin].reg &=~(uint8_t)(PORT_PINCFG_INEN | PORT_PINCFG_PMUXEN);}
#define GPIO_READ(ulPin) {(PORT->Group[g_APinDescription[ulPin].ulPort].IN.reg & (1ul << g_APinDescription[ulPin].ulPin)) != 0}
#define PIN_PERIPH(pin) {PORT->Group[g_APinDescription[(pin)].ulPort].PINCFG[g_APinDescription[(pin)].ulPin].reg |= PORT_PINCFG_PMUXEN;}
//sets up the pins for the board
static void boardSetupPins(void)
{
//setup switch pins
#ifdef PIN_SW1
pinMode(PIN_SW1, INPUT_PULLUP);
pinMode(PIN_SW3, INPUT_PULLUP);
pinMode(PIN_SW4, INPUT_PULLUP);
#endif
pinMode(PIN_STEP_INPUT, INPUT_PULLUP);
pinMode(PIN_DIR_INPUT, INPUT_PULLUP);
#ifdef PIN_ENABLE
pinMode(PIN_ENABLE, INPUT_PULLUP); //default error pin as enable pin with pull up
#endif
pinMode(PIN_ERROR, INPUT_PULLUP); //default error pin as enable pin with pull up
pinMode(PIN_AS5047D_CS,OUTPUT);
digitalWrite(PIN_AS5047D_CS,LOW); //pull CS LOW by default (chip powered off)
//turn the AS5047D off by default
#ifdef PIN_AS5047D_PWR
pinMode(PIN_AS5047D_PWR,OUTPUT);
digitalWrite(PIN_AS5047D_PWR,HIGH);
#endif
pinMode(PIN_MOSI,OUTPUT);
digitalWrite(PIN_MOSI,LOW);
pinMode(PIN_SCK,OUTPUT);
digitalWrite(PIN_SCK,LOW);
pinMode(PIN_MISO,INPUT);
//setup the A4954 pins
digitalWrite(PIN_A4954_IN3,LOW);
pinMode(PIN_A4954_IN3,OUTPUT);
digitalWrite(PIN_A4954_IN4,LOW);
pinMode(PIN_A4954_IN4,OUTPUT);
digitalWrite(PIN_A4954_IN2,LOW);
pinMode(PIN_A4954_IN2,OUTPUT);
digitalWrite(PIN_A4954_IN1,LOW);
pinMode(PIN_A4954_IN1,OUTPUT);
//setup the PWM for current on the A4954, set for low current
digitalWrite(PIN_A4954_VREF12,LOW);
digitalWrite(PIN_A4954_VREF34,LOW);
pinMode(PIN_A4954_VREF34, OUTPUT);
pinMode(PIN_A4954_VREF12, OUTPUT);
pinMode(PIN_RED_LED,OUTPUT);
#ifdef PIN_YELLOW_LED
pinMode(PIN_YELLOW_LED,OUTPUT);
digitalWrite(PIN_YELLOW_LED,HIGH);
#endif
}
#ifdef NEMA17_SMART_STEPPER_3_21_2017
static float GetMotorVoltage(void)
{
uint32_t x;
float f;
//the motor voltage is 1/101 of the adc
x=analogRead(PIN_VMOTOR); //this should be a 10bit value mapped to 3.3V
f=(float)x*3.3/1024.0*101.0;
return f;
}
#endif
static void inline YELLOW_LED(bool state)
{
#ifdef PIN_YELLOW_LED
digitalWrite(PIN_YELLOW_LED,!state);
#endif
}
static void inline RED_LED(bool state)
{
digitalWrite(PIN_RED_LED,state);
}
#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))
#define ABS(a) (((a)>(0))?(a):(-(a)))
#define DIV(x,y) (((y)>(0))?((x)/(y)):(4294967295))
#define SIGN(x) (((x) > 0) - ((x) < 0))
#define NVIC_IS_IRQ_ENABLED(x) (NVIC->ISER[0] & (1 << ((uint32_t)(x) & 0x1F)))!=0
static inline uint8_t getPinMux(uint16_t ulPin)
{
uint8_t temp;
if ((ulPin & 0x01)==0)
{
temp = (PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg) & PORT_PMUX_PMUXE( 0xF ) ;
}else
{
temp = (PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg)>>4 & 0xF;
}
return temp;
}
static inline uint8_t getPinCfg(uint16_t ulPin)
{
uint8_t temp;
temp = PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg;
return temp;
}
static inline void setPinCfg(uint16_t ulPin, uint8_t val)
{
PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg=val;
}
static inline void setPinMux(uint16_t ulPin, uint8_t val)
{
uint8_t temp;
temp = (PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg);
if ((ulPin & 0x01)==0)
{
//if an even pin
temp = (temp & 0xF0) | (val & 0x0F);
}else
{
temp = (temp & 0x0F) | ((val<<4) & 0x0F);
}
PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg=temp;
PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg |= PORT_PINCFG_PMUXEN ; // Enable port mux
}
static inline void SET_PIN_PERHERIAL(uint16_t ulPin,EPioType ulPeripheral)
{
if ( g_APinDescription[ulPin].ulPin & 1 ) // is pin odd?
{
uint32_t temp ;
// Get whole current setup for both odd and even pins and remove odd one
temp = (PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg) & PORT_PMUX_PMUXE( 0xF ) ;
// Set new muxing
PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg = temp|PORT_PMUX_PMUXO( ulPeripheral ) ;
// Enable port mux
PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg |= PORT_PINCFG_PMUXEN ;
}
else // even pin
{
uint32_t temp ;
temp = (PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg) & PORT_PMUX_PMUXO( 0xF ) ;
PORT->Group[g_APinDescription[ulPin].ulPort].PMUX[g_APinDescription[ulPin].ulPin >> 1].reg = temp|PORT_PMUX_PMUXE( ulPeripheral ) ;
PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg |= PORT_PINCFG_PMUXEN ; // Enable port mux
}
}
//the Arduino delay function requires interrupts to work.
// if interrupts are disabled use the delayMicroseconds which is a spin loop
static inline void DelayMs(uint32_t ms)
{
uint32_t prim;
/* Read PRIMASK register, check interrupt status before you disable them */
/* Returns 0 if they are enabled, or non-zero if disabled */
prim = __get_PRIMASK();
if (prim==0)
{
delay(ms);
}else
{
while(ms)
{
delayMicroseconds(1000);
ms--;
}
}
}
#endif//__BOARD_H__
================================================
FILE: firmware/stepper_nano_zero/calibration.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "calibration.h"
#include "Flash.h"
#include "nonvolatile.h"
#include "board.h" //for divide with rounding macro
#include "utils.h"
static uint16_t getTableIndex(uint16_t value)
{
int32_t x;
x=((int32_t)value*CALIBRATION_TABLE_SIZE)/CALIBRATION_STEPS; //the divide is a floor not a round which is what we want
return (uint16_t)x;
}
static uint16_t interp(Angle x1, Angle y1, Angle x2, Angle y2, Angle x)
{
int32_t dx,dy,dx2,y;
dx=x2-x1;
dy=y2-y1;
dx2=x-x1;
y=(int32_t)y1+DIVIDE_WITH_ROUND((dx2*dy),dx);
if (y<0)
{
y=y+CALIBRATION_STEPS;
}
return (uint16_t)y;
}
static void printData(int32_t *data, int32_t n)
{
int32_t i;
Serial.print("\n\r");
for (i=0; i<n; i++)
{
Serial.print(data[i]);
if (i!=(n-1))
{
Serial.print(",");
}
}
Serial.print("\n\r");
}
bool CalibrationTable::updateTableValue(int32_t index, int32_t value)
{
table[index].value=value;
table[index].error=CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE; //or error is roughly like variance, so set it to span to next calibration value.
return true;
}
void CalibrationTable::printCalTable(void)
{
int i;
Serial.print("\n\r");
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
Serial.print((uint16_t)table[i].value);
Serial.print(",");
}
Serial.print("\n\r");
}
Angle CalibrationTable::fastReverseLookup(Angle encoderAngle)
{
#ifdef NZS_FAST_CAL
//assume calibration is good
if (fastCalVaild)
{
uint16_t x;
x=((uint16_t)encoderAngle)/4; //we only have 16384 values in table
return (Angle)NVM->FastCal.angle[x];
}else
{
return reverseLookup(encoderAngle);
}
#else
return reverseLookup(encoderAngle)
#endif
}
Angle CalibrationTable::reverseLookup(Angle encoderAngle)
{
int32_t i=0;
int32_t a1,a2;
int32_t x;
int16_t y;
int32_t min,max;
min=(uint16_t)table[0].value;
max=min;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
x=(uint16_t)table[i].value;
if (x<min)
{
min=x;
}
if (x>max)
{
max=x;
}
}
x=(uint16_t)encoderAngle;
if (x<min)
{
x=x+CALIBRATION_STEPS;
}
i=0;
while (i<CALIBRATION_TABLE_SIZE)
{
a1=(uint16_t)table[i].value;
//handle index wrap around
if (i==(CALIBRATION_TABLE_SIZE-1))
{
a2=(uint16_t)table[0].value;
}else
{
a2=(uint16_t)table[i+1].value;
}
//wrap
if (abs(a1-a2)>CALIBRATION_STEPS/2)
{
if (a1<a2)
{
a1=a1+CALIBRATION_STEPS;
}else
{
a2=a2+CALIBRATION_STEPS;
}
//LOG("xxxx %d %d %d",a1,a2,x);
}
//finding matching location
if ( (x>=a1 && x<=a2) ||
(x>=a2 && x<=a1) )
{
//LOG("%d", i);
// inerpolate results and return
//LOG("%d %d %d",a1,a2,x);
//LOG("%d,%d",(i*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE,((i+2)*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE);
y=interp(a1, DIVIDE_WITH_ROUND((i*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), a2, DIVIDE_WITH_ROUND( ((i+1)*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), x);
return y;
}
i++;
}
ERROR("WE did some thing wrong");
}
void CalibrationTable::smoothTable(void)
{
uint16_t b[]={1,2,4,5,4,2,1};
uint16_t sum_b=19; //sum of b filter
int32_t data[CALIBRATION_TABLE_SIZE];
int32_t table2[CALIBRATION_TABLE_SIZE];
int32_t i;
int32_t offset=0;
int32_t startNum;
//first lets handle the wrap around in the table
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (i>0 && offset==0)
{
if(((uint16_t)table[i-1].value-(uint16_t)table[i].value) <-32768)
{
offset=-65536;
}
if (((uint16_t)table[i-1].value-(uint16_t)table[i].value) > 32768)
{
offset=65536;
}
}
table2[i]=(int32_t)((uint16_t)table[i].value)+offset;
}
//Serial.print("after wrap\n\r");
//printData(table2,CALIBRATION_TABLE_SIZE);
//remove the starting offset and compensate table for index
startNum=table2[0];
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table2[i]=table2[i]-startNum - (i*65536)/CALIBRATION_TABLE_SIZE;
}
//Serial.print("after phase comp\n\r");
//printData(table2,CALIBRATION_TABLE_SIZE);
//filter the data
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
int j,ix,ib;;
int32_t sum=0;
ib=0;
for (j=i-3; j<i+4; j++)
{
ix=j;
if (ix<0)
{
ix=ix+CALIBRATION_TABLE_SIZE;
}
if (ix>=CALIBRATION_TABLE_SIZE)
{
ix=ix-CALIBRATION_TABLE_SIZE;
}
if (i==0)
{
LOG("index %d",ix);
}
sum=sum+table2[ix]*b[ib];
ib++;
}
sum=DIVIDE_WITH_ROUND(sum,sum_b);
data[i]=sum;
}
//Serial.print("after filter\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//add in offset and the phase compenstation
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
data[i]=data[i]+startNum + (i*65536)/CALIBRATION_TABLE_SIZE;
}
//Serial.print("after phase comp added\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//remove the uint16_t wrap
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (data[i]>=65536)
{
data[i]=data[i]-65536;
}
}
//Serial.print("after wrap added\n\r");
//printData(data,CALIBRATION_TABLE_SIZE);
//save new table
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table[i].value=data[i];
}
}
void CalibrationTable::saveToFlash(void)
{
FlashCalData_t data;
int i;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
{
data.table[i]=(uint16_t)table[i].value;
}
data.status=true;
LOG("Writting Calbiration to Flash");
nvmWriteCalTable(&data,sizeof(data));
memset(&data,0,sizeof(data));
memcpy(&data, &NVM->CalibrationTable,sizeof(data));
createFastCal();
LOG("after writting status is %d",data.status);
loadFromFlash();
}
void CalibrationTable::loadFromFlash(void)
{
FlashCalData_t data;
int i;
LOG("Reading Calbiration to Flash");
memcpy(&data, &NVM->CalibrationTable,sizeof(data));
for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
{
table[i].value=Angle(data.table[i]);
table[i].error=CALIBRATION_MIN_ERROR;
}
data.status=true;
}
bool CalibrationTable::flashGood(void)
{
LOG("calibration table status is: %d",NVM->CalibrationTable.status);
return NVM->CalibrationTable.status;
}
void CalibrationTable::createFastCal(void)
{
#ifdef NZS_FAST_CAL
int32_t i;
uint16_t cs=0;
uint16_t data[256];
int32_t j;
j=0;
cs=0;
LOG("setting fast calibration");
for (i=0; i<16384; i++)
{
uint16_t x;
x=reverseLookup(i*4);
data[j]=x;
j++;
if (j>=256)
{
flashWrite(&NVM->FastCal.angle[i-255],data,256*sizeof(uint16_t));
//LOG("Wrote fastcal at index %d-%d", i-255, i);
j=0;
}
cs+=x;
}
//update the checksum
flashWrite(&NVM->FastCal.checkSum,&cs,sizeof(uint16_t));
fastCalVaild=true;
//this is a quick test
/*
for (i=0; i<16384; i++)
{
LOG("fast Cal %d,%d,%d",i,NVM->FastCal.angle[i],(uint32_t)reverseLookup(i*4));
}
*/
#endif
}
void CalibrationTable::updateFastCal(void)
{
#ifdef NZS_FAST_CAL
int32_t i;
uint16_t cs=0;
uint16_t data[256];
int32_t j;
bool NonZero=false;
for (i=0; i<16384; i++)
{
cs+=NVM->FastCal.angle[i];
if (cs != 0)
{
NonZero=true;
}
}
if (cs!=NVM->FastCal.checkSum || NonZero==false)
{
createFastCal();
}
else
{
LOG("fast cal is valid");
fastCalVaild=true;
}
#endif
}
void CalibrationTable::init(void)
{
int i;
if (true == flashGood())
{
loadFromFlash();
updateFastCal();
}else
{
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
table[i].value=0;
table[i].error=CALIBRATION_ERROR_NOT_SET;
}
}
return;
}
#if 0
//This code was removed because with micro stepping we can not assume
// our actualAngle is correct.
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue);
{
static int32_t lastAngle=-1;
static uint16_t lastEncoderValue=0;
if (last != -1)
{
int32_t dist;
//hopefull we can use the current point and last point to interpolate and set a value or two in table.
dist=abs((int32_t)actualAngle-(int32_t)lastAngle); //distance between the two angles
//since our angles wrap the shortest distance will be one less than 32768
if (dist>CALIBRATION_STEPS/2)
{
dist=dist-CALIBRATION_STEPS;
}
//if our distance is larger than size between calibration points in table we will ignore this sample
if (dist>CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE)
{
//spans two or more table calibration points for this implementation we will not use
lastIndex=(int32_t)index;
lastValue=value;
return;
}
//now lets see if the values are above and below a table calibration point
dist= abs(getTableIndex(lastAngle)-getTableIndex(actualAngle));
if (dist != 0) //if the two indexs into table are not the same it spans a calibration point in table.
{
//the two span a set calibation table point.
uint16_t newValue;
newValue=interp(lastAngle, lastEncoderValue, actualAngle, encoderValue, getTableIndex(actualAngle)*(CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE))
//this new value is our best guess as to the correct calibration value.
updateTableValue(getTableIndex(actualAngle),newValue);
} else
{
//we should calibate the table value for the point the closest
}
}
lastAngle=(int32_t)actualAngle;
lastEncoderValue=encoderValue;
}
#endif
//when we are microstepping and are in between steps the probability the stepper motor did not move
// is high. That is the actualAngle will be correct but the encoderValue will be behind due to not having enough torque to move motor.
// Therefore we only want to update the calibration on whole steps where we have highest probability of things being correct.
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue)
{
int32_t dist, index;
Angle tableAngle;
index = getTableIndex((uint32_t)actualAngle+CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE/2); //add half of distance to next entry to round to closest table index
tableAngle=(index*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE; //calculate the angle for this index
dist=tableAngle-actualAngle; //distance to calibration table angle
//LOG("Dist is %d",dist);
if (abs(dist)<CALIBRATION_MIN_ERROR) //if we are with in our minimal error we can calibrate
{
updateTableValue(index,(int32_t)encoderValue);
}
}
bool CalibrationTable::calValid(void)
{
uint32_t i;
for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
{
if (table[i].error == CALIBRATION_ERROR_NOT_SET)
{
return false;
}
}
if (false == flashGood())
{
saveToFlash();
}
return true;
}
//We want to linearly interpolate between calibration table angle
int CalibrationTable::getValue(Angle actualAngle, CalData_t *ptrData)
{
int32_t indexLow,indexHigh;
int32_t angleLow,angleHigh;
uint16_t value;
int32_t y1,y2;
int16_t err;
indexLow=getTableIndex((uint16_t)actualAngle);
// LOG("index %d, actual %u",indexLow, (uint16_t)actualAngle);
indexHigh=indexLow+1;
angleLow=(indexLow*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;
angleHigh=(indexHigh*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;
if (indexHigh>=CALIBRATION_TABLE_SIZE)
{
indexHigh -= CALIBRATION_TABLE_SIZE;
}
//LOG("AngleLow %d, AngleHigh %d",angleLow,angleHigh);
//LOG("TableLow %u, TableHigh %d",(uint16_t)table[indexLow].value,(uint16_t)table[indexHigh].value);
y1=table[indexLow].value;
y2=table[indexHigh].value;
//handle the wrap condition
if (abs(y2-y1)>CALIBRATION_STEPS/2)
{
if (y2<y1)
{
y2=y2+CALIBRATION_STEPS;
}else
{
y1=y1+CALIBRATION_STEPS;
}
}
value=interp(angleLow, y1, angleHigh, y2,actualAngle);
//handle the wrap condition
if (value>=CALIBRATION_STEPS)
{
value=value-CALIBRATION_STEPS;
}
err=table[indexLow].error;
if (table[indexHigh].error > err)
{
err=table[indexHigh].error;
}
if (table[indexLow].error == CALIBRATION_ERROR_NOT_SET ||
table[indexHigh].error == CALIBRATION_ERROR_NOT_SET)
{
err=CALIBRATION_ERROR_NOT_SET;
}
ptrData->value=value;
ptrData->error=err;
return 0;
}
Angle CalibrationTable::getCal(Angle actualAngle)
{
CalData_t data;
getValue(actualAngle, &data);
return data.value;
}
================================================
FILE: firmware/stepper_nano_zero/calibration.h
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#ifndef __CALIBRAITON_H__
#define __CALIBRAITON_H__
#include <Arduino.h>
#include "syslog.h"
#include "angle.h"
//this file implements a table that is linearly interpolated circular calibration table
// it is assumed the data wraps around, ie you interpolated 65536==0
//we want this to be "whole" steps, for 1.8 degree motors this should be 200.
// 200 will work for 0.9 degree too, but could be 400. However 400 is not good for 1.8 degree motors
#define CALIBRATION_TABLE_SIZE (200)
#define CALIBRATION_STEPS ((uint32_t)ANGLE_STEPS) // this is one rotation ie 0-65535 aka 65536 steps is 0-360 degrees
#define CALIBRATION_ERROR_NOT_SET (-1) //indicated that the calibration value is not set.
#define CALIBRATION_UPDATE_RATE (32) //number of samples to keep 1 pole running average
#define CALIBRATION_MIN_ERROR (4) //the minimal expected error on our calibration 4 ~=+/0.2 degrees
typedef struct {
uint16_t table[CALIBRATION_TABLE_SIZE];
bool status;
} FlashCalData_t;
typedef struct {
Angle value; //cal value
int16_t error; //error assuming it is constantly updated
} CalData_t;
class CalibrationTable
{
private:
CalData_t table[CALIBRATION_TABLE_SIZE];
bool fastCalVaild=false;
void loadFromFlash(void);
bool flashGood(void); //returns true if the flash copy of calibration is valid
void updateFastCal(void);
void createFastCal(void);
public:
void init(void);
void saveToFlash(void); //saves the calibration to flash
bool updateTableValue(int32_t index, int32_t value);
void updateTable(Angle actualAngle, Angle encoderValue);
int getValue(Angle actualAngle, CalData_t *ptrData);
Angle getCal(Angle actualAngle);
bool calValid(void);
Angle reverseLookup(Angle encoderAngle); //this turns encoder angle into real angle
void printCalTable(void);
void smoothTable(void);
Angle fastReverseLookup(Angle encoderAngle);
};
#endif //__CALIBRAITON_H__
================================================
FILE: firmware/stepper_nano_zero/command.cpp
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
implied, expressed, or statutory.
Written by Trampas Stern for MisfitTech.
Misfit Tech invests time and resources providing this open source code,
please support MisfitTech and open-source hardware by purchasing
products from MisfitTech, www.misifittech.net!
*********************************************************************/
#include "command.h"
#include <string.h>
#define ASCII_BACKSPACE 0x08
#define ASCII_ESC 0x1B
#define ASCII_UP_ARROW 0x9b
//const char CMD_ANSI_UP[]= {ASCII_ESC,'[','A',0};
int strcicmp(char const *a, char const *b)
{
for (;; a++, b++) {
int d = tolower(*a) - tolower(*b);
if (d != 0 || !*a)
return d;
}
}
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) )
{
ptrUart->kbhit=kbhit;
ptrUart->getch=getch;
ptrUart->putch=putch;
ptrUart->puts=puts;
ptrUart->histIndex=0;
ptrUart->buffIndex=0;
return 0;
}
#ifdef PGM_P //check and see if the PGM_P is defined for the AVR
int CommandPrintf(sCmdUart *ptrUart, const char *fmt, ...)
{
int ret=0;
char vastr[MAX_STRING]={0};
//char str[MAX_STRING]={0};
char *ptr;
va_list ap;
//LOG("Command printf");
memset(vastr,0,MAX_STRING);
va_start(ap,fmt);
ret=vsprintf(vastr,(const char *)fmt,ap);
//ret=sprintf(vastr,"%s\r\n",str);
//LOG("%s",vastr);
if (ptrUart->puts!=NULL)
{
return ptrUart->puts((uint8_t *)vastr, (uint8_t)ret);
}
if (ptrUart->putch!=NULL)
{
ptr=vastr;
while(*ptr)
{
ptrUart->putch(*ptr++);
}
return ret;
}
return 0;
}
#else
int CommandPrintf(sCmdUart *ptrUart, char *fmt, ...)
{
int ret=0;
char vastr[MAX_STRING]={0};
char *ptr;
va_list ap;
memset(vastr,0,MAX_STRING);
va_start(ap,fmt);
ret=vsprintf(vastr,(char *)fmt,ap);
if (ptrUart->puts!=NULL)
{
return ptrUart->puts((uint8_t *)vastr, (uint8_t)ret);
}
if (ptrUart->putch!=NULL)
{
ptr=vastr;
while(*ptr)
{
ptrUart->putch(*ptr++);
}
return ret;
}
return 0;
}
#endif
// the delimiter is command/parameter delimiter
// by default a ' '0x20 is used but for the TDR with GUI a ':' was preferred, not sure why
// set to ' '/0x20 if you want normal command parsing, like DOS
unsigned int CommandParse(sCmdUart *ptrUart,sCommand *ptrCmds, char *str, char delimitor )
{
char *ptr;
char *ptr2;
unsigned int i;
//char cmd[MAX_STRING];
char buff[MAX_CMD_LENGTH];
char argv[MAX_ARGS][MAX_ARG_LENGTH];
char *ptrArgv[MAX_ARGS];
unsigned int numArgs;
int emptyArg=0;
sCommand cmd_list;
while (*str==0x20 || *str=='\n' || *str=='\r' || *str=='\t') str++;
//first we need find command and arguments
ptr=strchr(str,delimitor); //find first char
//LOG("2parsing %s",str);
if (ptr==0)
{
//we have two options, frist whole thing is command
//second bad command
if(strlen(str)>0)
ptr=str+strlen(str);
else
return 0; //bad command
}
//copy string to command buffer.
i=0;
ptr2=str;
while(ptr!=0 && ptr!=ptr2 && i<(MAX_CMD_LENGTH-1))
{
//if (*ptr2!='\n' && *ptr2!='\r') //do not include newlines
{
buff[i++]=*ptr2;
}
ptr2++;
}
buff[i]=0;
//now buff contains the command let's get the args
numArgs=0;
while(*ptr!=0 && (*ptr==' ' || *ptr==delimitor))
ptr++; //increment pointer past ' '
if (*ptr!=0)
{
if (*ptr==34) // " char
{
ptr++;
ptr2=strchr(ptr,34); //find match
} else if (*ptr==39) // 'char
{
ptr++;
ptr2=strchr(ptr,39); //find match
} else
{
ptr2=strchr(ptr,delimitor);
}
if (ptr2==0)
{
//we have two options, frist whole thing is command
//second bad command
//LOG("strlen ptr is %d",strlen(ptr));
if(strlen(ptr)>0)
ptr2=ptr+strlen(ptr);
}
emptyArg=0;
while((ptr2!=0 && numArgs<MAX_ARGS) || emptyArg==1)
{
int j;
emptyArg=0;
j=0;
//LOG("arg %s",ptr);
while (ptr2!=ptr && j<(MAX_ARG_LENGTH-1) && ptr2!=0)
{
argv[numArgs][j++]=*ptr++;
}
argv[numArgs][j++]=0;
numArgs++;
ptr2=0;
if (*ptr!=0)
{
if (*ptr==34 || *ptr==39) ptr++;
if (*ptr==delimitor && strlen(ptr)==1)
{
//LOG("Empty arg");
emptyArg=1;
}
while(*ptr!=0 && (*ptr==' ' || *ptr==delimitor))//p || *ptr==34 || *ptr==39))
ptr++; //increment pointer past ' '
if (*ptr==34) // " char
{
ptr++;
ptr2=strchr(ptr,34); //find match
} else if (*ptr==39) // 'char
{
ptr++;
ptr2=strchr(ptr,39); //find match
} else
{
ptr2=strchr(ptr,delimitor);
}
if (ptr2==0)
{
//we have two options, frist whole thing is command
//second bad command
if(strlen(ptr)>0)
ptr2=ptr+strlen(ptr);
}
}
}
}
for(i=0; i<MAX_ARGS; i++)
{
ptrArgv[i]=argv[i];
}
//now let's parse the command
i=0;
memcpy(&cmd_list, &ptrCmds[i], sizeof(sCommand));
//LOG("command is %s %d",buff,numArgs);
while(cmd_list.function!=0)
{
/*char str[20];
strcpy_P(str,cmd_list.name);
LOG("checkign '%s' to '%s'",buff,str);
LOG("comapre is %d",strcmp_P(buff,cmd_list.name));
*/
//memcpy_P(&p, cmd_list.name, sizeof(PGM_P));
#ifdef PGM_P //check and see if the PGM_P is defined for the AVR
if (strlen(buff)==strlen_P(cmd_list.name))
{
if (strcicmp(buff,cmd_list.name)==0) //ignore device ID
#else
if (strlen(buff)==strlen(cmd_list.name))
{
if (strcicmp(buff,cmd_list.name)==0) //ignore device ID
#endif
{
//LOG("calling function");
//return 1;
return (*cmd_list.function)(ptrUart,numArgs,ptrArgv);
}
}
i=i+1;
memcpy(&cmd_list, &ptrCmds[i], sizeof(sCommand));
}
CommandPrintf(ptrUart,PSTR("Unknown command (try 'help')\n\r"));
return -1;
}
//This function will process commands from the UART
int CommandProcess(sCmdUart *ptrUart,sCommand *ptrCmds, char delimitor, char *cmdPrompt)
{
if(ptrUart->kbhit())
{
ptrUart->data=ptrUart->getch();
//echo the data
ptrUart->putch(ptrUart->data);
//if the data is the CR we need to process buffer
if (ptrUart->data==0x0D)
{
ptrUart->putch(0x0A);
if (strlen(ptrUart->buffer)>0)
{
if (ptrUart->lastChar!=ASCII_UP_ARROW)
{
strcpy(ptrUart->bufferHist[ptrUart->histIndex],ptrUart->buffer);
ptrUart->histIndex=(ptrUart->histIndex+1) % CMD_HISTORY;
}
CommandParse(ptrUart,ptrCmds,ptrUart->buffer,delimitor);
}
CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt);
ptrUart->buffIndex=0;
ptrUart->buffer[ptrUart->buffIndex]=0;
}
if (ptrUart->data==ASCII_BACKSPACE) //backspace
{
if (ptrUart->buffIndex>0)
{
ptrUart->buffIndex--;
ptrUart->buffer[ptrUart->buffIndex]='\0';
//Echo the backspace
ptrUart->putch(' ');
ptrUart->putch(ASCII_BACKSPACE);
}
}else if (ptrUart->data != 0x0A && ptrUart->data !=0x0D && ptrUart->data<127)
{
ptrUart->buffer[ptrUart->buffIndex++]=ptrUart->data;
ptrUart->buffer[ptrUart->buffIndex]=0;
}
if (ptrUart->buffIndex>=(MAX_CMD_LENGTH-1))
{
CommandPrintf(ptrUart,PSTR("\n\rERROR: Command buffer overflow\n\r"));\
ERROR("Command buffer overflow");
ptrUart->buffIndex=0;
ptrUart->buffer[0]=0;
CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt);
}
}
if (strstr(ptrUart->buffer,ANSI_UP)) //up arrow
{
uint8_t i;
CommandPrintf(ptrUart,PSTR("\n\r%s"),cmdPrompt);
i=CMD_HISTORY-1;
if (ptrUart->histIndex>0)
{
i=ptrUart->histIndex-1;
}
if (strlen(ptrUart->bufferHist[i])>0)
{
strcpy(ptrUart->buffer,ptrUart->bufferHist[i]);
ptrUart->buffIndex=strlen(ptrUart->buffer);
CommandPrintf(ptrUart,PSTR("%s"),ptrUart->buffer);
}else
{
ptrUart->buffIndex=0;
ptrUart->buffer[0]=0;
}
ptrUart->data=ASCII_UP_ARROW;
}
ptrUart->lastChar=ptrUart->data;
return 0;
}
================================================
FILE: firmware/stepper_nano_zero/command.h
================================================
/**********************************************************************
Copyright (C) 2018 MisfitTech LLC, All rights reserved.
MisfitTech uses a dual license model that allows the software to be used under
a standard GPL open source license, or a commercial license. The standard GPL
license requires that all software statically linked with MisfitTec Code is
also distributed under the same GPL V2 license terms. Details of both license
options follow:
- Open source licensing -
MisfitTech is a free download and may be used, modified, evaluated and
distributed without charge provided the user adheres to version two of the GNU
General Public License (GPL) and does not remove the copyright notice or this
text. The GPL V2 text is available on the gnu.org web site
- Commercial licensing -
Businesses and individuals that for commercial or other reasons cannot comply
with the terms of the GPL V2 license must obtain a low cost commercial license
before incorporating MisfitTech code into proprietary software for distribution in
any form. Commercial licenses can be purchased from www.misfittech.net
and do not require any source files to be changed.
This code is distributed in the hope that it will be useful. You cannot
use MisfitTech's code unless you agree that you use the software 'as is'.
MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
P
gitextract_ktl3d9ev/
├── README.md
├── firmware/
│ └── stepper_nano_zero/
│ ├── .cproject
│ ├── .project
│ ├── A1333.cpp
│ ├── A1333.h
│ ├── A4954.cpp
│ ├── A4954.h
│ ├── A5995.cpp
│ ├── A5995.h
│ ├── Adafruit_GFX.cpp
│ ├── Adafruit_GFX.h
│ ├── Adafruit_SSD1306.cpp
│ ├── Adafruit_SSD1306.h
│ ├── Flash.cpp
│ ├── Flash.h
│ ├── angle.h
│ ├── as5047d.cpp
│ ├── as5047d.h
│ ├── board.h
│ ├── calibration.cpp
│ ├── calibration.h
│ ├── command.cpp
│ ├── command.h
│ ├── commands.cpp
│ ├── commands.h
│ ├── eeprom.cpp
│ ├── eeprom.h
│ ├── fet_driver.cpp
│ ├── fet_driver.h
│ ├── ftoa.cpp
│ ├── ftoa.h
│ ├── gfxfont.h
│ ├── glcdfont.c
│ ├── nonvolatile.cpp
│ ├── nonvolatile.h
│ ├── nzs.cpp
│ ├── nzs.h
│ ├── nzs_lcd.cpp
│ ├── nzs_lcd.h
│ ├── planner.cpp
│ ├── planner.h
│ ├── sine.cpp
│ ├── sine.h
│ ├── stepper_controller.cpp
│ ├── stepper_controller.h
│ ├── stepper_nano_zero.ino
│ ├── steppin.cpp
│ ├── steppin.h
│ ├── syslog.cpp
│ ├── syslog.h
│ ├── utils.cpp
│ └── utils.h
└── hardware/
├── NZS_A4954_R2.0.PrjPcb
├── NZS_A4954_R2_0.PcbDoc
├── Nano_stepper.SchDoc
├── Status Report.Txt
├── panel-RoundHoles.TXT
├── panel-SlotHoles.TXT
├── panel-macro.APR_LIB
├── panel.1
├── panel.DRR
├── panel.EXTREP
├── panel.GBL
├── panel.GBO
├── panel.GBP
├── panel.GBS
├── panel.GD1
├── panel.GKO
├── panel.GM1
├── panel.GM13
├── panel.GM15
├── panel.GM2
├── panel.GM3
├── panel.GML
├── panel.GPB
├── panel.GPT
├── panel.GTL
├── panel.GTO
├── panel.GTP
├── panel.GTS
├── panel.LDP
├── panel.REP
├── panel.RUL
└── panel.apr
SYMBOL INDEX (226 symbols across 40 files)
FILE: firmware/stepper_nano_zero/A1333.cpp
function boolean (line 51) | boolean A1333::begin(int csPin)
FILE: firmware/stepper_nano_zero/A1333.h
function class (line 45) | class A1333 {
FILE: firmware/stepper_nano_zero/A4954.cpp
function syncTCC (line 56) | static void syncTCC(Tcc* TCCx) {
function bridge1 (line 70) | static inline void bridge1(int state)
function bridge2 (line 101) | static inline void bridge2(int state)
function enableTCC0 (line 132) | static void enableTCC0(uint8_t percent)
function setDAC (line 180) | static void setDAC(uint32_t DAC1, uint32_t DAC2)
function setupDAC (line 190) | static void setupDAC(void)
FILE: firmware/stepper_nano_zero/A4954.h
function class (line 68) | class A4954
FILE: firmware/stepper_nano_zero/A5995.cpp
function syncTCC (line 58) | static void syncTCC(Tcc* TCCx) {
function setDAC (line 73) | static void setDAC(uint32_t DAC1, uint32_t DAC2)
function setupDAC (line 82) | static void setupDAC(void)
FILE: firmware/stepper_nano_zero/A5995.h
function class (line 70) | class A5995
FILE: firmware/stepper_nano_zero/Adafruit_GFX.cpp
function boolean (line 932) | boolean Adafruit_GFX_Button::contains(int16_t x, int16_t y) {
function boolean (line 943) | boolean Adafruit_GFX_Button::isPressed() { return currstate; }
function boolean (line 944) | boolean Adafruit_GFX_Button::justPressed() { return (currstate && !lasts...
function boolean (line 945) | boolean Adafruit_GFX_Button::justReleased() { return (!currstate && last...
FILE: firmware/stepper_nano_zero/Adafruit_GFX.h
function class (line 13) | class Adafruit_GFX : public Print {
function class (line 107) | class Adafruit_GFX_Button {
function class (line 133) | class GFXcanvas1 : public Adafruit_GFX {
function class (line 145) | class GFXcanvas16 : public Adafruit_GFX {
FILE: firmware/stepper_nano_zero/Adafruit_SSD1306.h
type RwReg (line 30) | typedef volatile RwReg PortReg;
type PortMask (line 31) | typedef uint32_t PortMask;
type PortReg (line 36) | typedef volatile uint32_t PortReg;
type PortMask (line 37) | typedef uint32_t PortMask;
type PortReg (line 39) | typedef volatile uint8_t PortReg;
type PortMask (line 40) | typedef uint8_t PortMask;
function class (line 140) | class Adafruit_SSD1306 : public Adafruit_GFX {
FILE: firmware/stepper_nano_zero/Flash.cpp
function flashInit (line 41) | bool flashInit(void){
function erase (line 51) | static void erase(const volatile void *flash_ptr)
function flashErase (line 58) | bool flashErase(const volatile void *flash_ptr, uint32_t size)
function read_unaligned_uint32 (line 73) | static inline uint32_t read_unaligned_uint32(const void *data)
function flashWrite (line 88) | void flashWrite(const volatile void *flash_ptr,const void *data, uint32_...
function flashWritePage (line 152) | void flashWritePage(const volatile void *flash_ptr, const void *data, ui...
FILE: firmware/stepper_nano_zero/Flash.h
function flashRead (line 60) | static inline int32_t flashRead(const volatile void *flash_ptr, void *da...
FILE: firmware/stepper_nano_zero/angle.h
function class (line 50) | class Angle
FILE: firmware/stepper_nano_zero/as5047d.cpp
function getBit (line 64) | static int getBit(int16_t data, int bit)
function getParity (line 69) | static int getParity(uint16_t data)
function boolean (line 86) | boolean AS5047D::begin(int csPin)
FILE: firmware/stepper_nano_zero/as5047d.h
function class (line 44) | class AS5047D {
FILE: firmware/stepper_nano_zero/board.h
type RotationDir_t (line 206) | typedef enum {
type ErrorPinMode_t (line 211) | typedef enum {
type feedbackCtrl_t (line 219) | typedef enum {
function boardSetupPins (line 381) | static void boardSetupPins(void)
function GetMotorVoltage (line 441) | static float GetMotorVoltage(void)
function YELLOW_LED (line 452) | static void inline YELLOW_LED(bool state)
function RED_LED (line 459) | static void inline RED_LED(bool state)
function getPinMux (line 472) | static inline uint8_t getPinMux(uint16_t ulPin)
function getPinCfg (line 486) | static inline uint8_t getPinCfg(uint16_t ulPin)
function setPinCfg (line 494) | static inline void setPinCfg(uint16_t ulPin, uint8_t val)
function setPinMux (line 501) | static inline void setPinMux(uint16_t ulPin, uint8_t val)
function SET_PIN_PERHERIAL (line 517) | static inline void SET_PIN_PERHERIAL(uint16_t ulPin,EPioType ulPeripheral)
function DelayMs (line 543) | static inline void DelayMs(uint32_t ms)
FILE: firmware/stepper_nano_zero/calibration.cpp
function getTableIndex (line 45) | static uint16_t getTableIndex(uint16_t value)
function interp (line 53) | static uint16_t interp(Angle x1, Angle y1, Angle x2, Angle y2, Angle x)
function printData (line 67) | static void printData(int32_t *data, int32_t n)
function Angle (line 101) | Angle CalibrationTable::fastReverseLookup(Angle encoderAngle)
function Angle (line 120) | Angle CalibrationTable::reverseLookup(Angle encoderAngle)
function Angle (line 592) | Angle CalibrationTable::getCal(Angle actualAngle)
FILE: firmware/stepper_nano_zero/calibration.h
type FlashCalData_t (line 60) | typedef struct {
type CalData_t (line 69) | typedef struct {
function class (line 74) | class CalibrationTable
FILE: firmware/stepper_nano_zero/command.cpp
function strcicmp (line 48) | int strcicmp(char const *a, char const *b)
function CommandInit (line 57) | int CommandInit(sCmdUart *ptrUart, uint8_t (*kbhit)(void), uint8_t (*get...
function CommandPrintf (line 70) | int CommandPrintf(sCmdUart *ptrUart, const char *fmt, ...)
function CommandPrintf (line 104) | int CommandPrintf(sCmdUart *ptrUart, char *fmt, ...)
function CommandParse (line 138) | unsigned int CommandParse(sCmdUart *ptrUart,sCommand *ptrCmds, char *str...
FILE: firmware/stepper_nano_zero/command.h
type sCmdUart (line 125) | typedef struct {
type sCommand (line 148) | typedef struct
type sCommand (line 161) | typedef struct
FILE: firmware/stepper_nano_zero/commands.cpp
function isPowerOfTwo (line 58) | static int isPowerOfTwo (unsigned int x)
function debug_cmd (line 186) | static int debug_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function getsteps_cmd (line 196) | static int getsteps_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function geterror_cmd (line 204) | static int geterror_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function errorpin_cmd (line 215) | static int errorpin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function pinread_cmd (line 234) | static int pinread_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function errorPinISR (line 273) | static void errorPinISR(void)
function home_cmd (line 281) | static int home_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function reboot_cmd (line 318) | static int reboot_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function setpos_cmd (line 324) | static int setpos_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function eepromwrite_cmd (line 338) | static int eepromwrite_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function eepromerror_cmd (line 343) | static int eepromerror_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function eepromsetloc_cmd (line 379) | static int eepromsetloc_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function eepromloc_cmd (line 393) | static int eepromloc_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function looptime_cmd (line 407) | static int looptime_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function setzero_cmd (line 414) | static int setzero_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function stop_cmd (line 421) | static int stop_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function data_cmd (line 427) | static int data_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function stepsperrotation_cmd (line 442) | static int stepsperrotation_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function motorwiring_cmd (line 482) | static int motorwiring_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function homeangledelay_cmd (line 523) | static int homeangledelay_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function homepin_cmd (line 548) | static int homepin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function homecurrent_cmd (line 583) | static int homecurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function holdcurrent_cmd (line 639) | static int holdcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function maxcurrent_cmd (line 677) | static int maxcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function ctrlmode_cmd (line 716) | static int ctrlmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function errorlimit_cmd (line 787) | static int errorlimit_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function dirpin_cmd (line 828) | static int dirpin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function errorpinmode_cmd (line 879) | static int errorpinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function enablepinmode_cmd (line 942) | static int enablepinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function factoryreset_cmd (line 1004) | static int factoryreset_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function velocity_cmd (line 1009) | static int velocity_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function move_cmd (line 1041) | static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function boot_cmd (line 1108) | static int boot_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function vpid_cmd (line 1230) | static int vpid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function ppid_cmd (line 1275) | static int ppid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function spid_cmd (line 1319) | static int spid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function encoderdiag_cmd (line 1362) | static int encoderdiag_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function readpos_cmd (line 1370) | static int readpos_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function feedback_cmd (line 1381) | static int feedback_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function step_cmd (line 1392) | static int step_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function microsteps_cmd (line 1418) | static int microsteps_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function help_cmd (line 1455) | static int help_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function getcal_cmd (line 1478) | static int getcal_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function calibrate_cmd (line 1484) | static int calibrate_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function testcal_cmd (line 1491) | static int testcal_cmd(sCmdUart *ptrUart,int argc, char * argv[])
function kbhit (line 1506) | uint8_t kbhit(void)
function getChar (line 1511) | uint8_t getChar(void)
function putch (line 1515) | uint8_t putch(char data)
function kbhit_hw (line 1521) | uint8_t kbhit_hw(void)
function getChar_hw (line 1526) | uint8_t getChar_hw(void)
function putch_hw (line 1530) | uint8_t putch_hw(char data)
function kbhit_step_dir (line 1536) | uint8_t kbhit_step_dir(void)
function getChar_step_dir (line 1541) | uint8_t getChar_step_dir(void)
function putch_step_dir (line 1545) | uint8_t putch_step_dir(char data)
function commandsInit (line 1552) | void commandsInit(void)
function commandsProcess (line 1571) | int commandsProcess(void)
FILE: firmware/stepper_nano_zero/eeprom.cpp
function checksum (line 70) | static uint16_t checksum(uint8_t *ptrData, uint32_t nBytes)
function isPageGood (line 85) | static bool isPageGood(uint32_t page)
function printEEPROM (line 103) | static void printEEPROM(uint32_t page)
function findLastGoodPage (line 116) | static uint32_t findLastGoodPage(void)
function eepromGetNextWritPage (line 154) | static uint32_t eepromGetNextWritPage(void)
function eepromError_t (line 221) | eepromError_t eepromInit(void)
function eepromWriteCache (line 244) | int eepromWriteCache(uint8_t *ptrData, uint32_t size)
function eepromRead (line 263) | int eepromRead(uint8_t *ptrData, uint32_t size) //returns number of byte...
function eepromError_t (line 282) | eepromError_t eepromFlush(void) //flush the cache to flash memory
FILE: firmware/stepper_nano_zero/eeprom.h
type eepromError_t (line 50) | typedef enum {
FILE: firmware/stepper_nano_zero/fet_driver.cpp
function syncDAC (line 57) | static void syncDAC() {
function syncTCC (line 148) | static void syncTCC(Tcc* TCCx) {
function coilA (line 165) | static inline void coilA(CoilState_t state)
function coilB (line 193) | static inline void coilB(CoilState_t state)
function enableTCC0 (line 325) | static void enableTCC0(void)
function setDAC (line 352) | static void setDAC(uint32_t DAC1, uint32_t DAC2)
function setupDAC (line 362) | static void setupDAC(void)
function syncADC (line 464) | static void syncADC() {
function ADCRead (line 483) | static uint32_t ADCRead(uint32_t ulPin)
function fastADCRead (line 567) | int32_t fastADCRead(uint32_t ulPin)
function GetMeanAdc (line 590) | int32_t GetMeanAdc(uint16_t pin, uint16_t samples)
function ADCStart (line 604) | static uint32_t ADCStart(uint32_t ulPin)
function ADC_Handler (line 769) | void ADC_Handler(void)
FILE: firmware/stepper_nano_zero/fet_driver.h
function class (line 74) | class FetDriver
FILE: firmware/stepper_nano_zero/ftoa.cpp
function ftoa (line 56) | int ftoa (float x, char *str, char prec, char format)
FILE: firmware/stepper_nano_zero/gfxfont.h
type GFXglyph (line 10) | typedef struct { // Data stored PER GLYPH
type GFXfont (line 17) | typedef struct { // Data stored for FONT AS A WHOLE:
FILE: firmware/stepper_nano_zero/nonvolatile.cpp
function nvmWriteCalTable (line 68) | bool nvmWriteCalTable(void *ptrData, uint32_t size)
function nvmWrite_sPID (line 75) | bool nvmWrite_sPID(float Kp, float Ki, float Kd)
function nvmWrite_vPID (line 88) | bool nvmWrite_vPID(float Kp, float Ki, float Kd)
function nvmWrite_pPID (line 101) | bool nvmWrite_pPID(float Kp, float Ki, float Kd)
function nvmWriteSystemParms (line 114) | bool nvmWriteSystemParms(SystemParams_t &systemParams)
function nvmWriteMotorParms (line 122) | bool nvmWriteMotorParms(MotorParams_t &motorParams)
function nvmErase (line 130) | bool nvmErase(void)
FILE: firmware/stepper_nano_zero/nonvolatile.h
type PIDparams_t (line 45) | typedef struct {
type MotorParams_t (line 52) | typedef struct {
type SystemParams_t (line 62) | typedef struct {
type FastCal_t (line 75) | typedef struct {
type nvm_t (line 81) | typedef struct {
FILE: firmware/stepper_nano_zero/nzs.cpp
function menuCalibrate (line 49) | int menuCalibrate(int argc, char *argv[])
function menuTestCal (line 54) | int menuTestCal(int argc, char *argv[])
function motorSteps (line 92) | int motorSteps(int argc, char *argv[])
function motorCurrent (line 158) | int motorCurrent(int argc, char *argv[])
function motorHoldCurrent (line 183) | int motorHoldCurrent(int argc, char *argv[])
function microsteps (line 219) | int microsteps(int argc, char *argv[])
function controlLoop (line 259) | int controlLoop(int argc, char *argv[])
function errorPin (line 289) | int errorPin(int argc, char *argv[])
function enablePin (line 316) | int enablePin(int argc, char *argv[])
function dirPin (line 342) | int dirPin(int argc, char *argv[])
function enableInput (line 392) | static void enableInput(void)
function TC5_Handler (line 499) | void TC5_Handler()
function validateAndInitNVMParams (line 543) | void validateAndInitNVMParams(void)
function SYSCTRL_Handler (line 583) | void SYSCTRL_Handler(void)
function syncBOD33 (line 594) | static void syncBOD33(void) {
function configure_bod (line 605) | static void configure_bod(void)
function printLocation (line 794) | void printLocation(void)
FILE: firmware/stepper_nano_zero/nzs.h
type eepromData_t (line 36) | typedef struct
function class (line 43) | class NZS //nano Zero Stepper
FILE: firmware/stepper_nano_zero/nzs_lcd.h
type options_t (line 42) | typedef struct {
type menuItem_t (line 46) | typedef struct {
function class (line 59) | class LCD
FILE: firmware/stepper_nano_zero/planner.cpp
function enterTC3CriticalSection (line 37) | static bool enterTC3CriticalSection()
function exitTC3CriticalSection (line 44) | static void exitTC3CriticalSection(bool prevState)
function TC3_Init (line 55) | void TC3_Init(void)
function TC3_Handler (line 97) | void TC3_Handler(void)
FILE: firmware/stepper_nano_zero/planner.h
type PlannerMode (line 39) | typedef enum {
function class (line 45) | class Planner
FILE: firmware/stepper_nano_zero/sine.cpp
function sine (line 141) | int16_t sine(uint16_t angle)
function cosine (line 167) | int16_t cosine(uint16_t angle)
FILE: firmware/stepper_nano_zero/stepper_controller.cpp
function setupTCInterrupts (line 38) | void setupTCInterrupts() {
function enableTCInterrupts (line 79) | static void enableTCInterrupts() {
function disableTCInterrupts (line 88) | static void disableTCInterrupts() {
function enterCriticalSection (line 95) | static bool enterCriticalSection()
function exitCriticalSection (line 102) | static void exitCriticalSection(bool prevState)
function Angle (line 380) | Angle StepperCtrl::maxCalibrationError(void)
function stepCtrlError_t (line 572) | stepCtrlError_t StepperCtrl::begin(void)
function Angle (line 701) | Angle StepperCtrl::sampleAngle(void)
function Angle (line 724) | Angle StepperCtrl::sampleMeanEncoder(int32_t numSamples)
function Angle (line 920) | Angle StepperCtrl::getEncoderAngle(void)
FILE: firmware/stepper_nano_zero/stepper_controller.h
type stepCtrlError_t (line 41) | typedef enum {
type PID_t (line 49) | typedef struct {
type __attribute__ (line 56) | typedef __attribute__((aligned(4)))
type Control_t (line 65) | typedef struct {
function class (line 77) | class StepperCtrl
FILE: firmware/stepper_nano_zero/steppin.cpp
function checkDir (line 40) | void checkDir(void)
function getSteps (line 69) | int64_t getSteps(void)
function stepInput (line 105) | static void stepInput(void)
function enableEIC (line 130) | void enableEIC(void)
function setupStepEvent (line 148) | void setupStepEvent(void)
function stepPinSetup (line 294) | void stepPinSetup(void)
FILE: firmware/stepper_nano_zero/syslog.cpp
function SysLogDisable (line 58) | int SysLogDisable(void)
function SysLogEnable (line 64) | int SysLogEnable(void)
function SysLogIsEnabled (line 70) | int SysLogIsEnabled(void)
function SysLogDebug (line 74) | void SysLogDebug(bool x)
function SysLogPuts (line 79) | void SysLogPuts(const char *ptrStr)
function SysLogInit (line 105) | void SysLogInit(Stream *ptrSerialObj, eLogLevel LevelToWrite)
function SysLog (line 120) | void SysLog(eLogLevel priorty, const char *fmt, ...)
FILE: firmware/stepper_nano_zero/syslog.h
type eLogLevel (line 67) | typedef enum _eLogLevel {
FILE: firmware/stepper_nano_zero/utils.cpp
function CubicInterpolate (line 28) | double CubicInterpolate(
Condensed preview — 84 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,419K chars).
[
{
"path": "README.md",
"chars": 11146,
"preview": "# Smart Stepper (also known as the nano zero stepper)\nFirmware to turn a stepper motor into servo motor: see http://misf"
},
{
"path": "firmware/stepper_nano_zero/.cproject",
"chars": 12998,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n<?fileVersion 4.0.0?><cproject storage_type_id=\"org.eclipse.cdt.c"
},
{
"path": "firmware/stepper_nano_zero/.project",
"chars": 1022,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<projectDescription>\n\t<name>NZS</name>\n\t<comment></comment>\n\t<projects>\n\t</projec"
},
{
"path": "firmware/stepper_nano_zero/A1333.cpp",
"chars": 4419,
"preview": "/**********************************************************************\n\tCopyright (C) 2019 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/A1333.h",
"chars": 2346,
"preview": "/**********************************************************************\n\tCopyright (C) 2019 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/A4954.cpp",
"chars": 10628,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/A4954.h",
"chars": 3287,
"preview": "/**********************************************************************\n\tCopyright (C) 2019 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/A5995.cpp",
"chars": 7884,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/A5995.h",
"chars": 3230,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/Adafruit_GFX.cpp",
"chars": 32618,
"preview": "/*\nThis is the core graphics library for all our displays, providing a common\nset of graphics primitives (points, lines,"
},
{
"path": "firmware/stepper_nano_zero/Adafruit_GFX.h",
"chars": 4944,
"preview": "#ifndef _ADAFRUIT_GFX_H\n#define _ADAFRUIT_GFX_H\n\n#if ARDUINO >= 100\n #include \"Arduino.h\"\n #include \"Print.h\"\n#else\n #in"
},
{
"path": "firmware/stepper_nano_zero/Adafruit_SSD1306.cpp",
"chars": 24442,
"preview": "/*********************************************************************\nThis is a library for our Monochrome OLEDs based "
},
{
"path": "firmware/stepper_nano_zero/Adafruit_SSD1306.h",
"chars": 5601,
"preview": "/*********************************************************************\nThis is a library for our Monochrome OLEDs based "
},
{
"path": "firmware/stepper_nano_zero/Flash.cpp",
"chars": 5567,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/Flash.h",
"chars": 2890,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/angle.h",
"chars": 3783,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/as5047d.cpp",
"chars": 8696,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/as5047d.h",
"chars": 2393,
"preview": "/**********************************************************************\n\tCopyright (C) 2019 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/board.h",
"chars": 20436,
"preview": "/**********************************************************************\n/***********************************************"
},
{
"path": "firmware/stepper_nano_zero/calibration.cpp",
"chars": 13688,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/calibration.h",
"chars": 3861,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/command.cpp",
"chars": 9337,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/command.h",
"chars": 5445,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/commands.cpp",
"chars": 38071,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/commands.h",
"chars": 2168,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/eeprom.cpp",
"chars": 8307,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/eeprom.h",
"chars": 2693,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/fet_driver.cpp",
"chars": 37193,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/fet_driver.h",
"chars": 4250,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/ftoa.cpp",
"chars": 5007,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/ftoa.h",
"chars": 2048,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/gfxfont.h",
"chars": 924,
"preview": "// Font structures for newer Adafruit_GFX (1.1 and later).\n// Example fonts are included in 'Fonts' directory.\n// To use"
},
{
"path": "firmware/stepper_nano_zero/glcdfont.c",
"chars": 8486,
"preview": "// This is the 'classic' fixed-space bitmap font for Adafruit_GFX since 1.0.\n// See gfxfont.h for newer custom bitmap fo"
},
{
"path": "firmware/stepper_nano_zero/nonvolatile.cpp",
"chars": 5450,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/nonvolatile.h",
"chars": 4335,
"preview": "/**********************************************************************\n\tCopyright (C) 2018 MisfitTech LLC, All rights"
},
{
"path": "firmware/stepper_nano_zero/nzs.cpp",
"chars": 18694,
"preview": "/*\n * nzs.cpp\n *\n * Created on: Dec 8, 2016\n * Author: trampas\n *\n\tCopyright (C) 2018 MisfitTech, All rights res"
},
{
"path": "firmware/stepper_nano_zero/nzs.h",
"chars": 1370,
"preview": "/*\n * nzs.h\n *\n * Created on: Dec 8, 2016\n * Author: trampas\n *\n\tCopyright (C) 2018 MisfitTech, All rights reser"
},
{
"path": "firmware/stepper_nano_zero/nzs_lcd.cpp",
"chars": 10576,
"preview": "/*\n * nzs_lcd.cpp\n *\n * Created on: Dec 8, 2016\n * Author: trampas\n *\n\tCopyright (C) 2018 MisfitTech, All rights"
},
{
"path": "firmware/stepper_nano_zero/nzs_lcd.h",
"chars": 2142,
"preview": "/*\n * nzs_lcd.h\n *\n * Created on: Dec 8, 2016\n * Author: trampas\n *\n *\n\tCopyright (C) 2018 MisfitTech, All right"
},
{
"path": "firmware/stepper_nano_zero/planner.cpp",
"chars": 5033,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "firmware/stepper_nano_zero/planner.h",
"chars": 2288,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "firmware/stepper_nano_zero/sine.cpp",
"chars": 11984,
"preview": "/*\n * since.cpp\n *\n * Created on: Dec 24, 2016\n * Author: tstern\n *\n\tCopyright (C) 2018 MisfitTech, All rights r"
},
{
"path": "firmware/stepper_nano_zero/sine.h",
"chars": 1325,
"preview": "/**********************************************************************\n * sine.h\n *\n * Created on: Dec 24, 2016\n * "
},
{
"path": "firmware/stepper_nano_zero/stepper_controller.cpp",
"chars": 40878,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "firmware/stepper_nano_zero/stepper_controller.h",
"chars": 6338,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "firmware/stepper_nano_zero/stepper_nano_zero.ino",
"chars": 95,
"preview": "#include \"nzs.h\"\n\nNZS nzs;\n \n\nvoid setup() {\n nzs.begin();\n}\n\n\nvoid loop() {\n\tnzs.loop(); \n}\n"
},
{
"path": "firmware/stepper_nano_zero/steppin.cpp",
"chars": 7822,
"preview": "/*\n * \tCopyright (C) 2018 MisfitTech, All rights reserved.\n\n This program is free software: you can redistribute it"
},
{
"path": "firmware/stepper_nano_zero/steppin.h",
"chars": 1130,
"preview": "/*\n * \tCopyright (C) 2018 MisfitTech, All rights reserved.\n\n This program is free software: you can redistribute it"
},
{
"path": "firmware/stepper_nano_zero/syslog.cpp",
"chars": 5521,
"preview": "/*\n * syslog.c\n *\n * Created on: Sep 14, 2011\n * Author: trampas.stern\n *\n\tCopyright (C) 2018 MisfitTech, All ri"
},
{
"path": "firmware/stepper_nano_zero/syslog.h",
"chars": 8854,
"preview": "/*\n * syslog.h\n *\n * Created on: Sep 14, 2011\n * Author: trampas.stern\n\tCopyright (C) 2018 MisfitTech, All right"
},
{
"path": "firmware/stepper_nano_zero/utils.cpp",
"chars": 1372,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "firmware/stepper_nano_zero/utils.h",
"chars": 1285,
"preview": "/**********************************************************************\n * Author: tstern\n *\n\tCopyright (C) 2018 M"
},
{
"path": "hardware/NZS_A4954_R2.0.PrjPcb",
"chars": 48221,
"preview": "[Design]\nVersion=1.0\nHierarchyMode=0\nChannelRoomNamingStyle=0\nReleasesFolder=\nReleaseVaultGUID=\nReleaseVaultName=\nChanne"
},
{
"path": "hardware/Status Report.Txt",
"chars": 455,
"preview": "Source PCB Design panel.PcbDoc contains embedded board array. It is recomended to generate individual BOM for each embed"
},
{
"path": "hardware/panel-RoundHoles.TXT",
"chars": 10565,
"preview": "M48\n;Layer_Color=9474304\n;FILE_FORMAT=2:5\nINCH,LZ\n;TYPE=PLATED\nT1F00S00C0.00787\nT2F00S00C0.01200\nT5F00S00C0.02700\nT6F00S"
},
{
"path": "hardware/panel-SlotHoles.TXT",
"chars": 780,
"preview": "M48\n;Layer_Color=9474304\n;FILE_FORMAT=2:5\nINCH,LZ\n;TYPE=PLATED\nT3F00S00C0.01968\nT4F00S00C0.02165\n;TYPE=NON_PLATED\n%\nG90\n"
},
{
"path": "hardware/panel-macro.APR_LIB",
"chars": 7753,
"preview": "G04:AMPARAMS|DCode=11|XSize=31.5mil|YSize=35.43mil|CornerRadius=7.87mil|HoleSize=0mil|Usage=FLASHONLY|Rotation=270.000|X"
},
{
"path": "hardware/panel.1",
"chars": 63,
"preview": "G04 Layer_Color=255*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\nM02*\n"
},
{
"path": "hardware/panel.DRR",
"chars": 2225,
"preview": "------------------------------------------------------------------------------------------------------------------------"
},
{
"path": "hardware/panel.EXTREP",
"chars": 1745,
"preview": "------------------------------------------------------------------------------------------\nGerber File Extension Report "
},
{
"path": "hardware/panel.GBL",
"chars": 206706,
"preview": "G04 Layer_Physical_Order=2*\nG04 Layer_Color=16711680*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD13R,0.03347X0.03150*%\n%AD"
},
{
"path": "hardware/panel.GBO",
"chars": 50613,
"preview": "G04 Layer_Color=65280*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD43C,0.00800*%\n%ADD67C,0.00984*%\n%ADD69C,0.00787*%\n%ADD11"
},
{
"path": "hardware/panel.GBP",
"chars": 12192,
"preview": "G04 Layer_Color=128*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD13R,0.03347X0.03150*%\n%ADD24R,0.03543X0.02953*%\n%ADD43C,0."
},
{
"path": "hardware/panel.GBS",
"chars": 24794,
"preview": "G04 Layer_Color=16711935*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD43C,0.00800*%\n%ADD74R,0.04147X0.03950*%\n%ADD85R,0.043"
},
{
"path": "hardware/panel.GD1",
"chars": 107305,
"preview": "G04 Layer_Color=2752767*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD120C,0.00200*%\n%ADD121C,0.00267*%\n%ADD122C,0.00133*%\nD"
},
{
"path": "hardware/panel.GKO",
"chars": 10146,
"preview": "G04 Layer_Color=16711935*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD38C,0.00600*%\n%ADD45C,0.01000*%\nD38*\nX187583Y307201D0"
},
{
"path": "hardware/panel.GM1",
"chars": 68,
"preview": "G04 Layer_Color=16711935*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\nM02*\n"
},
{
"path": "hardware/panel.GM13",
"chars": 22589,
"preview": "G04 Layer_Color=16711935*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD118C,0.00394*%\nD118*\nX206869Y152124D02*\nG03*\nX206869Y"
},
{
"path": "hardware/panel.GM15",
"chars": 33651,
"preview": "G04 Layer_Color=32768*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD118C,0.00394*%\n%ADD119C,0.00197*%\nD118*\nX213744Y171472D0"
},
{
"path": "hardware/panel.GM2",
"chars": 1714,
"preview": "G04 Layer_Color=8388736*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD68C,0.00500*%\nD68*\nX134044Y222344D02*\nG03*\nX134044Y222"
},
{
"path": "hardware/panel.GM3",
"chars": 5599,
"preview": "G04 Layer_Color=12566272*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD40C,0.02000*%\n%ADD117C,0.04724*%\nD40*\nX160800Y531193D"
},
{
"path": "hardware/panel.GML",
"chars": 6789,
"preview": "G04 Layer_Color=12632256*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD49C,0.01000*%\n%ADD50O,0.04724X0.09449*%\n%ADD51O,0.061"
},
{
"path": "hardware/panel.GPB",
"chars": 13438,
"preview": "G04 Layer_Color=255*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD13R,0.03347X0.03150*%\n%ADD24R,0.03543X0.02953*%\n%ADD49C,0."
},
{
"path": "hardware/panel.GPT",
"chars": 22918,
"preview": "G04 Layer_Color=255*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD10R,0.03543X0.03150*%\nG04:AMPARAMS|DCode=11|XSize=31.5mil|"
},
{
"path": "hardware/panel.GTL",
"chars": 134983,
"preview": "G04 Layer_Physical_Order=1*\nG04 Layer_Color=255*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD10R,0.03543X0.03150*%\nG04:AMPA"
},
{
"path": "hardware/panel.GTO",
"chars": 88179,
"preview": "G04 Layer_Color=65535*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD43C,0.00800*%\n%ADD45C,0.01000*%\n%ADD67C,0.00984*%\n%ADD68"
},
{
"path": "hardware/panel.GTP",
"chars": 19728,
"preview": "G04 Layer_Color=8421504*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD10R,0.03543X0.03150*%\nG04:AMPARAMS|DCode=11|XSize=31.5"
},
{
"path": "hardware/panel.GTS",
"chars": 27048,
"preview": "G04 Layer_Color=8388736*\n%FSLAX25Y25*%\n%MOIN*%\nG70*\nG01*\nG75*\n%ADD43C,0.00800*%\n%ADD71R,0.04343X0.03950*%\nG04:AMPARAMS|D"
},
{
"path": "hardware/panel.LDP",
"chars": 292,
"preview": "Layer Pairs Export File for PCB: C:\\Users\\TSTERN\\Google Drive\\projects\\Stepper_feedback\\hardware\\NZS_A4954_R2.0\\panel.Pc"
},
{
"path": "hardware/panel.REP",
"chars": 7038,
"preview": "*************************************************************\nFileName = panel.GBR\nAutoAperture = True\n*****************"
},
{
"path": "hardware/panel.RUL",
"chars": 392,
"preview": "DRC Rules Export File for PCB: C:\\Users\\TSTERN\\Google Drive\\projects\\Stepper_feedback\\hardware\\NZS_A4954_R2.0\\panel.PcbD"
},
{
"path": "hardware/panel.apr",
"chars": 6734,
"preview": "D10 RECTANGULAR 31.496 35.433 0.000 FLASH 270.000\nD12 RECTANGULAR 39.370 70.866 "
}
]
// ... and 2 more files (download for full content)
About this extraction
This page contains the full source code of the Misfittech/nano_stepper GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 84 files (1.3 MB), approximately 566.2k tokens, and a symbol index with 226 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.