├── .gitattributes ├── .gitignore ├── Frame ├── IGESs │ ├── Chassis_Bottom.iges │ ├── Chassis_Top.iges │ ├── Coxa.iges │ ├── Coxa_Cover.iges │ ├── Femur_Backplate.iges │ ├── Femur_Cover_Left.iges │ ├── Femur_Cover_Right.iges │ ├── Femur_Frontplate.iges │ └── Tibia.iges └── STLs │ ├── Chassis_Bottom.stl │ ├── Chassis_Top.stl │ ├── Coxa_Bottom.stl │ ├── Coxa_Cover.stl │ ├── Coxa_Top.stl │ ├── Femur_Backplate.stl │ ├── Femur_Cover_Left.stl │ ├── Femur_Cover_Right.stl │ ├── Femur_Frontplate.stl │ └── Tibia.stl ├── Quad_PS2_SSC32 ├── Jeb.h ├── Jeb_Code.h ├── Jeb_Driver_SSC32.h ├── Jeb_Input_PS2.h ├── Quad_Cfg.h └── Quad_PS2_SSC32.ino ├── ReadMe.md └── libraries ├── Adafruit_NeoPixel ├── .travis.yml ├── Adafruit_NeoPixel.cpp ├── Adafruit_NeoPixel.h ├── COPYING ├── README.md ├── esp8266.c ├── examples │ ├── RGBWstrandtest │ │ ├── .esp8266.test.skip │ │ └── RGBWstrandtest.ino │ ├── buttoncycler │ │ ├── .esp8266.test.skip │ │ └── buttoncycler.ino │ ├── simple │ │ ├── .esp8266.test.skip │ │ └── simple.ino │ └── strandtest │ │ ├── .esp8266.test.skip │ │ └── strandtest.ino ├── keywords.txt └── library.properties ├── PS2X_lib ├── Examples │ └── PS2X_Example │ │ └── PS2X_Example.pde ├── PS2X_lib.cpp ├── PS2X_lib.h └── keywords.txt └── QuadC_PS2_SSC32 ├── Hex_Cfg.h └── QuadC_PS2_SSC32.ino /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ################# 2 | ## Eclipse 3 | ################# 4 | 5 | *.pydevproject 6 | .project 7 | .metadata 8 | bin/ 9 | tmp/ 10 | *.tmp 11 | *.bak 12 | *.swp 13 | *~.nib 14 | local.properties 15 | .classpath 16 | .settings/ 17 | .loadpath 18 | 19 | # External tool builders 20 | .externalToolBuilders/ 21 | 22 | # Locally stored "Eclipse launch configurations" 23 | *.launch 24 | 25 | # CDT-specific 26 | .cproject 27 | 28 | # PDT-specific 29 | .buildpath 30 | 31 | 32 | ################# 33 | ## Visual Studio 34 | ################# 35 | 36 | ## Ignore Visual Studio temporary files, build results, and 37 | ## files generated by popular Visual Studio add-ons. 38 | 39 | # User-specific files 40 | *.suo 41 | *.user 42 | *.sln.docstates 43 | 44 | # Build results 45 | [Dd]ebug/ 46 | [Rr]elease/ 47 | *_i.c 48 | *_p.c 49 | *.ilk 50 | *.meta 51 | *.obj 52 | *.pch 53 | *.pdb 54 | *.pgc 55 | *.pgd 56 | *.rsp 57 | *.sbr 58 | *.tlb 59 | *.tli 60 | *.tlh 61 | *.tmp 62 | *.vspscc 63 | .builds 64 | *.dotCover 65 | 66 | ## TODO: If you have NuGet Package Restore enabled, uncomment this 67 | #packages/ 68 | 69 | # Visual C++ cache files 70 | ipch/ 71 | *.aps 72 | *.ncb 73 | *.opensdf 74 | *.sdf 75 | 76 | # Visual Studio profiler 77 | *.psess 78 | *.vsp 79 | 80 | # ReSharper is a .NET coding add-in 81 | _ReSharper* 82 | 83 | # Installshield output folder 84 | [Ee]xpress 85 | 86 | # DocProject is a documentation generator add-in 87 | DocProject/buildhelp/ 88 | DocProject/Help/*.HxT 89 | DocProject/Help/*.HxC 90 | DocProject/Help/*.hhc 91 | DocProject/Help/*.hhk 92 | DocProject/Help/*.hhp 93 | DocProject/Help/Html2 94 | DocProject/Help/html 95 | 96 | # Click-Once directory 97 | publish 98 | 99 | # Others 100 | [Bb]in 101 | [Oo]bj 102 | sql 103 | TestResults 104 | *.Cache 105 | ClientBin 106 | stylecop.* 107 | ~$* 108 | *.dbmdl 109 | Generated_Code #added for RIA/Silverlight projects 110 | 111 | # Backup & report files from converting an old project file to a newer 112 | # Visual Studio version. Backup files are not needed, because we have git ;-) 113 | _UpgradeReport_Files/ 114 | Backup*/ 115 | UpgradeLog*.XML 116 | 117 | 118 | 119 | ############ 120 | ## Windows 121 | ############ 122 | 123 | # Windows image file caches 124 | Thumbs.db 125 | 126 | # Folder config file 127 | Desktop.ini 128 | 129 | 130 | ############# 131 | ## Python 132 | ############# 133 | 134 | *.py[co] 135 | 136 | # Packages 137 | *.egg 138 | *.egg-info 139 | dist 140 | build 141 | eggs 142 | parts 143 | bin 144 | var 145 | sdist 146 | develop-eggs 147 | .installed.cfg 148 | 149 | # Installer logs 150 | pip-log.txt 151 | 152 | # Unit test / coverage reports 153 | .coverage 154 | .tox 155 | 156 | #Translations 157 | *.mo 158 | 159 | #Mr Developer 160 | .mr.developer.cfg 161 | 162 | # Mac crap 163 | .DS_Store 164 | -------------------------------------------------------------------------------- /Frame/STLs/Chassis_Bottom.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Chassis_Bottom.stl -------------------------------------------------------------------------------- /Frame/STLs/Chassis_Top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Chassis_Top.stl -------------------------------------------------------------------------------- /Frame/STLs/Coxa_Bottom.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Coxa_Bottom.stl -------------------------------------------------------------------------------- /Frame/STLs/Coxa_Cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Coxa_Cover.stl -------------------------------------------------------------------------------- /Frame/STLs/Coxa_Top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Coxa_Top.stl -------------------------------------------------------------------------------- /Frame/STLs/Femur_Backplate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Femur_Backplate.stl -------------------------------------------------------------------------------- /Frame/STLs/Femur_Cover_Left.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Femur_Cover_Left.stl -------------------------------------------------------------------------------- /Frame/STLs/Femur_Cover_Right.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Femur_Cover_Right.stl -------------------------------------------------------------------------------- /Frame/STLs/Femur_Frontplate.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Femur_Frontplate.stl -------------------------------------------------------------------------------- /Frame/STLs/Tibia.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/Frame/STLs/Tibia.stl -------------------------------------------------------------------------------- /Quad_PS2_SSC32/Jeb.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Project: Jebediah the Quadruped Robot 3 | // 4 | // Description: This code controls a quadruped robot with three degrees of 5 | // freedom per leg. 6 | // 7 | // Phoenix.h - This is the first header file that is needed to build 8 | // a Phoenix program for a specific Hex Robot. 9 | // 10 | // 11 | // This file assumes that the main source file either directly or through include 12 | // file has defined all of the configuration information for the specific robot. 13 | // 14 | //============================================================================== 15 | #ifndef _PHOENIX_CORE_H_ 16 | #define _PHOENIX_CORE_H_ 17 | #include 18 | //#include 19 | #if defined(__SAM3X8E__) 20 | #define PROGMEM 21 | #define pgm_read_byte(x) (*((char *)x)) 22 | // #define pgm_read_word(x) (*((short *)(x & 0xfffffffe))) 23 | #define pgm_read_word(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))) 24 | #define pgm_read_byte_near(x) (*((char *)x)) 25 | #define pgm_read_byte_far(x) (*((char *)x)) 26 | // #define pgm_read_word_near(x) (*((short *)(x & 0xfffffffe)) 27 | // #define pgm_read_word_far(x) (*((short *)(x & 0xfffffffe))) 28 | #define pgm_read_word_near(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))) 29 | #define pgm_read_word_far(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))) 30 | #define PSTR(x) x 31 | #endif 32 | 33 | 34 | #ifdef USEXBEE 35 | #include "diyxbee.h" 36 | #endif 37 | 38 | //============================================================================= 39 | //[CONSTANTS] 40 | //============================================================================= 41 | #define BUTTON_DOWN 0 42 | #define BUTTON_UP 1 43 | 44 | #define c1DEC 10 45 | #define c2DEC 100 46 | #define c4DEC 10000 47 | #define c6DEC 1000000 48 | 49 | #ifdef QUADMODE 50 | enum {cRR=0, cRF, cLR, cLF, CNT_LEGS}; 51 | #else 52 | enum {cRR=0, cRM, cRF, cLR, cLM, cLF, CNT_LEGS}; 53 | #endif 54 | 55 | #define WTIMERTICSPERMSMUL 64 // BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192 56 | #define WTIMERTICSPERMSDIV 125 // 57 | #define USEINT_TIMERAV 58 | 59 | 60 | // BUGBUG: to make Dynamic first pass simpl make it a variable. 61 | //#define NUM_GAITS 6 62 | extern const byte NUM_GAITS; 63 | #define SmDiv 4 //"Smooth division" factor for the smooth control function, a value of 3 to 5 is most suitable 64 | extern void GaitSelect(void); 65 | extern short SmoothControl (short CtrlMoveInp, short CtrlMoveOut, byte CtrlDivider); 66 | 67 | 68 | 69 | //----------------------------------------------------------------------------- 70 | // Define Global variables 71 | //----------------------------------------------------------------------------- 72 | extern boolean g_fDebugOutput; 73 | extern boolean g_fEnableServos; // Hack to allow me to turn servo processing off... 74 | extern boolean g_fRobotUpsideDown; // Is the robot upside down? 75 | 76 | 77 | extern void MSound(byte cNotes, ...); 78 | extern boolean CheckVoltage(void); 79 | 80 | void AdjustLegPositionsToBodyHeight(void); 81 | 82 | // debug handler... 83 | extern boolean g_fDBGHandleError; 84 | 85 | #ifdef c4DOF 86 | extern const byte cTarsLength[] PROGMEM; 87 | #endif 88 | 89 | #ifdef OPT_BACKGROUND_PROCESS 90 | #define DoBackgroundProcess() g_ServoDriver.BackgroundProcess() 91 | #else 92 | #define DoBackgroundProcess() 93 | #endif 94 | 95 | #ifdef DEBUG_IOPINS 96 | #define DebugToggle(pin) {digitalWrite(pin, !digitalRead(pin));} 97 | #define DebugWrite(pin, state) {digitalWrite(pin, state);} 98 | #else 99 | #define DebugToggle(pin) {;} 100 | #define DebugWrite(pin, state) {;} 101 | #endif 102 | 103 | 104 | 105 | #ifdef __AVR__ 106 | #if not defined(UBRR1H) 107 | #if cSSC_IN != 0 108 | extern SoftwareSerial SSCSerial; 109 | #endif 110 | #endif 111 | #endif 112 | #if defined(__PIC32MX__) 113 | #if defined F 114 | #undef F 115 | #endif 116 | #define F(X) (X) 117 | #endif 118 | 119 | 120 | 121 | //============================================================================= 122 | //============================================================================= 123 | // Define the class(s) for our Input controllers. 124 | //============================================================================= 125 | //============================================================================= 126 | class InputController { 127 | public: 128 | virtual void Init(void); 129 | virtual void ControlInput(void); 130 | virtual void AllowControllerInterrupts(boolean fAllow); 131 | 132 | private: 133 | } ; 134 | 135 | // Define a function that allows us to define which controllers are to be used. 136 | extern void RegisterInputController(InputController *pic); 137 | 138 | 139 | 140 | typedef struct _Coord3D { 141 | long x; 142 | long y; 143 | long z; 144 | } COORD3D; 145 | 146 | 147 | //============================================================================== 148 | // class ControlState: This is the main structure of data that the Control 149 | // manipulates and is used by the main Phoenix Code to make it do what is 150 | // requested. 151 | //============================================================================== 152 | typedef struct _InControlState { 153 | boolean fRobotOn; //Switch to turn on Phoenix 154 | boolean fPrev_RobotOn; //Previous loop state 155 | //Body position 156 | COORD3D BodyPos; 157 | COORD3D BodyRotOffset; // Body rotation offset; 158 | 159 | //Body Inverse Kinematics 160 | COORD3D BodyRot1; // X -Pitch, Y-Rotation, Z-Roll 161 | 162 | //[gait] 163 | byte GaitType; //Gait type 164 | 165 | short LegLiftHeight; //Current Travel height 166 | COORD3D TravelLength; // X-Z or Length, Y is rotation. 167 | 168 | //[Single Leg Control] 169 | byte SelectedLeg; 170 | COORD3D SLLeg; // 171 | boolean fSLHold; //Single leg control mode 172 | 173 | 174 | //[Balance] 175 | boolean BalanceMode; 176 | 177 | //[TIMING] 178 | byte InputTimeDelay; //Delay that depends on the input to get the "sneaking" effect 179 | word SpeedControl; //Adjustible Delay 180 | byte ForceGaitStepCnt; // new to allow us to force a step even when not moving 181 | } INCONTROLSTATE; 182 | 183 | //============================================================================== 184 | //============================================================================== 185 | // Define the class(s) for Servo Drivers. 186 | //============================================================================== 187 | //============================================================================== 188 | class ServoDriver { 189 | public: 190 | void Init(void); 191 | 192 | word GetBatteryVoltage(void); 193 | 194 | #ifdef OPT_GPPLAYER 195 | inline boolean FIsGPEnabled(void) {return _fGPEnabled;}; 196 | boolean FIsGPSeqDefined(uint8_t iSeq); 197 | inline boolean FIsGPSeqActive(void) {return _fGPActive;}; 198 | void GPStartSeq(uint8_t iSeq); // 0xff - says to abort... 199 | void GPPlayer(void); 200 | uint8_t GPNumSteps(void); // How many steps does the current sequence have 201 | uint8_t GPCurStep(void); // Return which step currently on... 202 | void GPSetSpeedMultiplyer(short sm) ; // Set the Speed multiplier (100 is default) 203 | #endif 204 | void BeginServoUpdate(void); // Start the update 205 | #ifdef c4DOF 206 | void OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1, short sTarsAngle1); 207 | #else 208 | void OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1); 209 | #endif 210 | void CommitServoDriver(word wMoveTime); 211 | void FreeServos(void); 212 | 213 | // Allow for background process to happen... 214 | #ifdef OPT_BACKGROUND_PROCESS 215 | void BackgroundProcess(void); 216 | #endif 217 | 218 | #ifdef OPT_TERMINAL_MONITOR 219 | void ShowTerminalCommandList(void); 220 | boolean ProcessTerminalCommand(byte *psz, byte bLen); 221 | #endif 222 | 223 | private: 224 | 225 | #ifdef OPT_GPPLAYER 226 | boolean _fGPEnabled; // IS GP defined for this servo driver? 227 | boolean _fGPActive; // Is a sequence currently active - May change later when we integrate in sequence timing adjustment code 228 | uint8_t _iSeq; // current sequence we are running 229 | short _sGPSM; // Speed multiplier +-200 230 | #endif 231 | 232 | } ; 233 | 234 | //============================================================================== 235 | // Define Gait structure/class - Hopefully allow specific robots to define their 236 | // own gaits and/or define which of the standard ones they want. 237 | //============================================================================== 238 | typedef struct _PhoenixGait { 239 | byte StepsInGait; //Number of steps in gait 240 | short NomGaitSpeed; //Nominal speed of the gait 241 | 242 | short NrLiftedPos; //Number of positions that a single leg is lifted [1-3] 243 | byte FrontDownPos; //Where the leg should be put down to ground 244 | byte LiftDivFactor; //Normaly: 2, when NrLiftedPos=5: 4 245 | boolean HalfLiftHeigth; //If TRUE the outer positions of the ligted legs will be half height 246 | 247 | short TLDivFactor; //Number of steps that a leg is on the floor while walking 248 | 249 | byte GaitLegNr[CNT_LEGS]; //Init position of the leg 250 | } PHOENIXGAIT; 251 | 252 | 253 | //============================================================================== 254 | //============================================================================== 255 | // Define global class objects 256 | //============================================================================== 257 | //============================================================================== 258 | extern ServoDriver g_ServoDriver; // our global servo driver class 259 | extern InputController g_InputController; // Our Input controller 260 | extern INCONTROLSTATE g_InControlState; // State information that controller changes 261 | 262 | 263 | #endif 264 | 265 | -------------------------------------------------------------------------------- /Quad_PS2_SSC32/Jeb_Driver_SSC32.h: -------------------------------------------------------------------------------- 1 | //==================================================================== 2 | //Project: Jebediah the Quadruped Robot 3 | // 4 | // Description: This code controls a quadruped robot with three degrees of 5 | // freedom per leg. 6 | // 7 | // Phoenix_Driver_SSC32.h - This version is setup to use the SSC-32 to control 8 | // the servos. 9 | //==================================================================== 10 | 11 | //Servo Pin numbers - May be SSC-32 or actual pins on main controller, depending on configuration. 12 | #ifdef QUADMODE 13 | const byte cCoxaPin[] PROGMEM = { 14 | cRRCoxaPin, cRFCoxaPin, cLRCoxaPin, cLFCoxaPin}; 15 | const byte cFemurPin[] PROGMEM = { 16 | cRRFemurPin, cRFFemurPin, cLRFemurPin, cLFFemurPin}; 17 | const byte cTibiaPin[] PROGMEM = { 18 | cRRTibiaPin, cRFTibiaPin, cLRTibiaPin, cLFTibiaPin}; 19 | #ifdef c4DOF 20 | const byte cTarsPin[] PROGMEM = { 21 | cRRTarsPin, cRFTarsPin, cLRTarsPin, cLFTarsPin}; 22 | #endif 23 | #else 24 | const byte cCoxaPin[] PROGMEM = { 25 | cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin}; 26 | const byte cFemurPin[] PROGMEM = { 27 | cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin}; 28 | const byte cTibiaPin[] PROGMEM = { 29 | cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin}; 30 | #ifdef c4DOF 31 | const byte cTarsPin[] PROGMEM = { 32 | cRRTarsPin, cRMTarsPin, cRFTarsPin, cLRTarsPin, cLMTarsPin, cLFTarsPin}; 33 | #endif 34 | #endif 35 | 36 | 37 | // Add support for running on non-mega Arduino boards as well. 38 | #ifdef __AVR__ 39 | #if not defined(UBRR1H) 40 | #if cSSC_IN == 0 41 | #define SSCSerial Serial 42 | #else 43 | SoftwareSerial SSCSerial(cSSC_IN, cSSC_OUT); 44 | #endif 45 | #endif 46 | #endif 47 | 48 | //============================================================================= 49 | // Global - Local to this file only... 50 | //============================================================================= 51 | 52 | // definition of some helper functions 53 | extern int SSCRead (byte* pb, int cb, word wTimeout, word wEOL); 54 | 55 | 56 | //-------------------------------------------------------------------- 57 | //Init 58 | //-------------------------------------------------------------------- 59 | void ServoDriver::Init(void) { 60 | SSCSerial.begin(cSSC_BAUD); 61 | 62 | // Lets do the check for GP Enabled here... 63 | #ifdef OPT_GPPLAYER 64 | char abT[4]; // give a nice large buffer. 65 | byte cbRead; 66 | 67 | _fGPEnabled = false; // starts off assuming that it is not enabled... 68 | _fGPActive = false; 69 | 70 | #ifdef __AVR__ 71 | #if not defined(UBRR1H) 72 | #if cSSC_IN != 0 73 | SSCSerial.listen(); 74 | #endif 75 | #endif 76 | #endif 77 | // Instead of hard checking version numbers instead ask it for 78 | // status of one of the players. If we do not get a response... 79 | // probably does not support 80 | SSCSerial.println(F("QPL0")); 81 | cbRead = SSCRead((byte*)abT, 4, 25000, (word)-1); 82 | 83 | #ifdef DBGSerial 84 | DBGSerial.print(F("Check GP Enable: ")); 85 | DBGSerial.println(cbRead, DEC); 86 | #endif 87 | if (cbRead == 4) 88 | _fGPEnabled = true; // starts off assuming that it is not enabled... 89 | else 90 | MSound (2, 40, 2500, 40, 2500); 91 | #endif 92 | #ifdef cVoltagePin 93 | // Prime the voltage values... 94 | for (byte i=0; i < 8; i++) 95 | GetBatteryVoltage(); 96 | 97 | #endif 98 | } 99 | //-------------------------------------------------------------------- 100 | //GetBatteryVoltage - Maybe should try to minimize when this is called 101 | // as it uses the serial port... Maybe only when we are not interpolating 102 | // or if maybe some minimum time has elapsed... 103 | //-------------------------------------------------------------------- 104 | 105 | #ifdef cVoltagePin 106 | #ifndef CVADR1 107 | #define CVADR1 30 // VD Resistor 1 - reduced as only need ratio... 30K and 10K 108 | #define CVADR2 10 // VD Resistor 2 109 | #endif 110 | 111 | word g_awVoltages[8]={ 112 | 0,0,0,0,0,0,0,0}; 113 | word g_wVoltageSum = 0; 114 | byte g_iVoltages = 0; 115 | 116 | word ServoDriver::GetBatteryVoltage(void) { 117 | g_iVoltages = (++g_iVoltages)&0x7; // setup index to our array... 118 | g_wVoltageSum -= g_awVoltages[g_iVoltages]; 119 | g_awVoltages[g_iVoltages] = analogRead(cVoltagePin); 120 | g_wVoltageSum += g_awVoltages[g_iVoltages]; 121 | 122 | return ((long)((long)g_wVoltageSum*125*(CVADR1+CVADR2))/(long)(2048*(long)CVADR2)); 123 | 124 | } 125 | #endif 126 | 127 | 128 | //============================================================================== 129 | // Quick and dirty helper function to read so many bytes in from the SSC with a timeout and an end of character marker... 130 | //============================================================================== 131 | int SSCRead (byte* pb, int cb, word wTimeout, word wEOL) 132 | { 133 | int ich; 134 | byte* pbIn = pb; 135 | unsigned long ulTimeLastChar = micros(); 136 | while (cb) { 137 | while (!SSCSerial.available()) { 138 | // check for timeout 139 | if ((word)(micros()-ulTimeLastChar) > wTimeout) { 140 | return (int)(pb-pbIn); 141 | } 142 | } 143 | ich = SSCSerial.read(); 144 | *pb++ = (byte)ich; 145 | cb--; 146 | 147 | if ((word)ich == wEOL) 148 | break; // we matched so get out of here. 149 | ulTimeLastChar = micros(); // update to say we received something 150 | } 151 | 152 | return (int)(pb-pbIn); 153 | } 154 | 155 | 156 | 157 | //-------------------------------------------------------------------- 158 | //[GP PLAYER] 159 | //-------------------------------------------------------------------- 160 | #ifdef OPT_GPPLAYER 161 | uint8_t g_bGPCntSteps; 162 | uint8_t g_bGPCurStep; 163 | boolean g_fGPSMChanged; 164 | //-------------------------------------------------------------------- 165 | //[FIsGPSeqDefined] 166 | //-------------------------------------------------------------------- 167 | boolean ServoDriver::FIsGPSeqDefined(uint8_t iSeq) 168 | { 169 | word wGPSeqPtr; 170 | 171 | // See if we can see if this sequence is defined 172 | SSCSerial.print(F("EER -")); 173 | SSCSerial.print(iSeq*2, DEC); 174 | SSCSerial.println(F(";2")); 175 | if ((SSCRead((byte*)&wGPSeqPtr, sizeof(wGPSeqPtr), 1000, 0xffff) == sizeof(wGPSeqPtr)) && (wGPSeqPtr != 0) && (wGPSeqPtr != 0xffff)) { 176 | return true; 177 | } 178 | return false; // nope return error 179 | } 180 | 181 | 182 | //-------------------------------------------------------------------- 183 | // Setup to start sequence number... 184 | //-------------------------------------------------------------------- 185 | void ServoDriver::GPStartSeq(uint8_t iSeq) 186 | { 187 | if (!_fGPActive && (iSeq != 0xff)) { 188 | _fGPActive = true; 189 | _iSeq = iSeq; 190 | g_bGPCntSteps = 0xff; 191 | g_fGPSMChanged = false; 192 | } 193 | else { 194 | _iSeq = iSeq; // Signal for abort 195 | } 196 | 197 | } 198 | 199 | //-------------------------------------------------------------------- 200 | //[GP PLAYER] 201 | //-------------------------------------------------------------------- 202 | void ServoDriver::GPPlayer(void) 203 | { 204 | byte abStat[4]; 205 | byte cbRead; 206 | 207 | if (_fGPActive) { 208 | if (g_bGPCntSteps == 0xff) { 209 | // We have not init yet... 210 | g_bGPCntSteps = GPNumSteps(); // so get the number of steps. 211 | if (g_bGPCntSteps == 0xff) { 212 | _fGPActive = false; // error so bail out of here... 213 | } 214 | else { 215 | g_InputController.AllowControllerInterrupts(false); 216 | 217 | // Since we are monitoring GP Sequence we no longer tell it to only run once... 218 | SSCSerial.print(F("PL0SQ")); 219 | SSCSerial.println(_iSeq, DEC); 220 | delay(20); 221 | while (SSCSerial.read() != -1) // remove anything that was queued up. 222 | ; 223 | g_InputController.AllowControllerInterrupts(true); 224 | } 225 | } 226 | else { 227 | // Player was started up, so lets see what the state is... 228 | g_InputController.AllowControllerInterrupts(false); 229 | if (_iSeq == 0xff) { // User told us to abort 230 | SSCSerial.println(F("PL0")); 231 | _fGPActive=false; 232 | } 233 | else { 234 | SSCSerial.print(F("QPL0\r")); 235 | cbRead = SSCRead((byte*)abStat, sizeof(abStat), 10000, (word)-1); // [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime] 236 | 237 | g_bGPCurStep = abStat[1]; 238 | if ((g_bGPCurStep == (g_bGPCntSteps-1)) && (abStat[3] == 0)) { 239 | // We are done 240 | SSCSerial.println(F("PL0")); 241 | _fGPActive=false; 242 | } 243 | else if (g_fGPSMChanged) { 244 | g_fGPSMChanged = false; 245 | SSCSerial.print(F("PL0SM")); 246 | SSCSerial.println(_sGPSM, DEC); 247 | } 248 | } 249 | g_InputController.AllowControllerInterrupts(true); // Ok to process hserial again... 250 | } 251 | } 252 | } 253 | 254 | //------------------------------------------------------------------------------------------ 255 | //------------------------------------------------------------------------------------------ 256 | uint8_t ServoDriver::GPNumSteps(void) // How many steps does the current sequence have 257 | { 258 | word wSeqStart; 259 | uint8_t bGPCntSteps = 0xff; // assume an error 260 | byte cbRead; 261 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 262 | 263 | // Output command to ssc 264 | SSCSerial.print(F("EER -")); 265 | SSCSerial.print(_iSeq, DEC); 266 | SSCSerial.println(F(";2")); 267 | cbRead = SSCRead((byte*)&wSeqStart, sizeof(wSeqStart), 10000, (word)-1); // Try to get the pointer to sequence 268 | 269 | if ((cbRead == sizeof(wSeqStart)) && (wSeqStart != 0) && (wSeqStart != 0xffff)) { 270 | // Now try to read in the count of steps from the start of the sequence. 271 | // Output command to ssc 272 | SSCSerial.print(F("EER -")); 273 | SSCSerial.print(wSeqStart, DEC); 274 | SSCSerial.println(F(";1")); 275 | cbRead = SSCRead((byte*)bGPCntSteps, sizeof(bGPCntSteps), 10000, (word)-1); // Try to get the pointer to sequence 276 | } 277 | return bGPCntSteps; 278 | } 279 | 280 | //------------------------------------------------------------------------------------------ 281 | //------------------------------------------------------------------------------------------ 282 | uint8_t ServoDriver::GPCurStep(void) // Return which step currently on... 283 | { 284 | return _fGPActive ? g_bGPCurStep : 0xff; 285 | } 286 | 287 | //------------------------------------------------------------------------------------------ 288 | //------------------------------------------------------------------------------------------ 289 | void ServoDriver::GPSetSpeedMultiplyer(short sm) // Set the Speed multiplier (100 is default) 290 | { 291 | if (_sGPSM != sm) { 292 | _sGPSM = sm; 293 | g_fGPSMChanged = true; 294 | } 295 | } 296 | 297 | 298 | #endif // OPT_GPPLAYER 299 | 300 | //------------------------------------------------------------------------------------------ 301 | //[BeginServoUpdate] Does whatever preperation that is needed to starrt a move of our servos 302 | //------------------------------------------------------------------------------------------ 303 | void ServoDriver::BeginServoUpdate(void) // Start the update 304 | { 305 | } 306 | 307 | //------------------------------------------------------------------------------------------ 308 | //[OutputServoInfoForLeg] Do the output to the SSC-32 for the servos associated with 309 | // the Leg number passed in. 310 | //------------------------------------------------------------------------------------------ 311 | #define cPwmDiv 991 //old 1059; 312 | #define cPFConst 592 //old 650 ; 900*(1000/cPwmDiv)+cPFConst must always be 1500 313 | // A PWM/deg factor of 10,09 give cPwmDiv = 991 and cPFConst = 592 314 | // For a modified 5645 (to 180 deg travel): cPwmDiv = 1500 and cPFConst = 900. 315 | #ifdef c4DOF 316 | void ServoDriver::OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1, short sTarsAngle1) 317 | #else 318 | void ServoDriver::OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1) 319 | #endif 320 | { 321 | word wCoxaSSCV; // Coxa value in SSC units 322 | word wFemurSSCV; // 323 | word wTibiaSSCV; // 324 | #ifdef c4DOF 325 | word wTarsSSCV; // 326 | #endif 327 | 328 | //Update Right Legs 329 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 330 | #ifndef QUADMODE 331 | if (LegIndex < (CNT_LEGS/2)) { 332 | wCoxaSSCV = ((long)(-sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst; 333 | wFemurSSCV = ((long)(-sFemurAngle1+900))*1000/cPwmDiv+cPFConst; 334 | wTibiaSSCV = ((long)(-sTibiaAngle1+900))*1000/cPwmDiv+cPFConst; 335 | #ifdef c4DOF 336 | wTarsSSCV = ((long)(-sTarsAngle1+900))*1000/cPwmDiv+cPFConst; 337 | #endif 338 | } 339 | else { 340 | wCoxaSSCV = ((long)(sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst; 341 | wFemurSSCV = ((long)((long)(sFemurAngle1+900))*1000/cPwmDiv+cPFConst); 342 | wTibiaSSCV = ((long)(sTibiaAngle1+900))*1000/cPwmDiv+cPFConst; 343 | #ifdef c4DOF 344 | wTarsSSCV = ((long)(sTarsAngle1+900))*1000/cPwmDiv+cPFConst; 345 | #endif 346 | } 347 | #else 348 | // inverting of the angles is done in the phoenix code for quad mode 349 | wCoxaSSCV = ((long)(sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst; 350 | wFemurSSCV = ((long)((long)(sFemurAngle1+900))*1000/cPwmDiv+cPFConst); 351 | wTibiaSSCV = ((long)(sTibiaAngle1+900))*1000/cPwmDiv+cPFConst; 352 | #ifdef c4DOF 353 | wTarsSSCV = ((long)(sTarsAngle1+900))*1000/cPwmDiv+cPFConst; 354 | #endif 355 | 356 | #endif 357 | 358 | #ifdef cSSC_BINARYMODE 359 | SSCSerial.write(pgm_read_byte(&cCoxaPin[LegIndex]) + 0x80); 360 | SSCSerial.write(wCoxaSSCV >> 8); 361 | SSCSerial.write(wCoxaSSCV & 0xff); 362 | SSCSerial.write(pgm_read_byte(&cFemurPin[LegIndex]) + 0x80); 363 | SSCSerial.write(wFemurSSCV >> 8); 364 | SSCSerial.write(wFemurSSCV & 0xff); 365 | SSCSerial.write(pgm_read_byte(&cTibiaPin[LegIndex]) + 0x80); 366 | SSCSerial.write(wTibiaSSCV >> 8); 367 | SSCSerial.write(wTibiaSSCV & 0xff); 368 | #ifdef c4DOF 369 | if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) { // We allow mix of 3 and 4 DOF legs... 370 | SSCSerial.write(pgm_read_byte(&cTarsPin[LegIndex]) + 0x80); 371 | SSCSerial.write(wTarsSSCV >> 8); 372 | SSCSerial.write(wTarsSSCV & 0xff); 373 | } 374 | #endif 375 | #else 376 | SSCSerial.print("#"); 377 | SSCSerial.print(pgm_read_byte(&cCoxaPin[LegIndex]), DEC); 378 | SSCSerial.print("P"); 379 | SSCSerial.print(wCoxaSSCV, DEC); 380 | SSCSerial.print("#"); 381 | SSCSerial.print(pgm_read_byte(&cFemurPin[LegIndex]), DEC); 382 | SSCSerial.print("P"); 383 | SSCSerial.print(wFemurSSCV, DEC); 384 | SSCSerial.print("#"); 385 | SSCSerial.print(pgm_read_byte(&cTibiaPin[LegIndex]), DEC); 386 | SSCSerial.print("P"); 387 | SSCSerial.print(wTibiaSSCV, DEC); 388 | #ifdef c4DOF 389 | if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) { 390 | SSCSerial.print("#"); 391 | SSCSerial.print(pgm_read_byte(&cTarsPin[LegIndex]), DEC); 392 | SSCSerial.print("P"); 393 | SSCSerial.print(wTarsSSCV, DEC); 394 | } 395 | #endif 396 | #endif 397 | g_InputController.AllowControllerInterrupts(true); // Ok for hserial again... 398 | } 399 | 400 | 401 | //-------------------------------------------------------------------- 402 | //[CommitServoDriver Updates the positions of the servos - This outputs 403 | // as much of the command as we can without committing it. This 404 | // allows us to once the previous update was completed to quickly 405 | // get the next command to start 406 | //-------------------------------------------------------------------- 407 | void ServoDriver::CommitServoDriver(word wMoveTime) 408 | { 409 | #ifdef cSSC_BINARYMODE 410 | byte abOut[3]; 411 | #endif 412 | 413 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 414 | 415 | #ifdef cSSC_BINARYMODE 416 | abOut[0] = 0xA1; 417 | abOut[1] = wMoveTime >> 8; 418 | abOut[2] = wMoveTime & 0xff; 419 | SSCSerial.write(abOut, 3); 420 | #else 421 | //Send 422 | SSCSerial.print("T"); 423 | SSCSerial.println(wMoveTime, DEC); 424 | #endif 425 | 426 | g_InputController.AllowControllerInterrupts(true); 427 | 428 | } 429 | 430 | //-------------------------------------------------------------------- 431 | //[FREE SERVOS] Frees all the servos 432 | //-------------------------------------------------------------------- 433 | void ServoDriver::FreeServos(void) 434 | { 435 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 436 | for (byte LegIndex = 0; LegIndex < 32; LegIndex++) { 437 | SSCSerial.print("#"); 438 | SSCSerial.print(LegIndex, DEC); 439 | SSCSerial.print("P0"); 440 | } 441 | SSCSerial.print("T200\r"); 442 | g_InputController.AllowControllerInterrupts(true); 443 | } 444 | 445 | #ifdef OPT_TERMINAL_MONITOR 446 | extern void FindServoOffsets(void); 447 | extern void SSCForwarder(void); 448 | 449 | //============================================================================== 450 | // ShowTerminalCommandList: Allow the Terminal monitor to call the servo driver 451 | // to allow it to display any additional commands it may have. 452 | //============================================================================== 453 | void ServoDriver::ShowTerminalCommandList(void) 454 | { 455 | #ifdef OPT_FIND_SERVO_OFFSETS 456 | DBGSerial.println(F("O - Enter Servo offset mode")); 457 | #endif 458 | #ifdef OPT_SSC_FORWARDER 459 | DBGSerial.println(F("S - SSC Forwarder")); 460 | #endif 461 | } 462 | 463 | //============================================================================== 464 | // ProcessTerminalCommand: The terminal monitor will call this to see if the 465 | // command the user entered was one added by the servo driver. 466 | //============================================================================== 467 | boolean ServoDriver::ProcessTerminalCommand(byte *psz, byte bLen) 468 | { 469 | #ifdef OPT_FIND_SERVO_OFFSETS 470 | if ((bLen == 1) && ((*psz == 'o') || (*psz == 'O'))) { 471 | FindServoOffsets(); 472 | } 473 | #endif 474 | #ifdef OPT_SSC_FORWARDER 475 | if ((bLen == 1) && ((*psz == 's') || (*psz == 'S'))) { 476 | SSCForwarder(); 477 | } 478 | #endif 479 | return true; // Currently not using the return value 480 | 481 | } 482 | 483 | //============================================================================== 484 | // SSC Forwarder - used to allow things like Lynxterm to talk to the SSC-32 485 | // through the Arduino... Will see if it is fast enough... 486 | //============================================================================== 487 | #ifdef OPT_SSC_FORWARDER 488 | void SSCForwarder(void) 489 | { 490 | int sChar; 491 | int sPrevChar; 492 | DBGSerial.println("SSC Forwarder mode - Enter $ to exit"); 493 | 494 | for (;;) { 495 | if ((sChar = DBGSerial.read()) != -1) { 496 | SSCSerial.write(sChar & 0xff); 497 | if (((sChar == '\n') || (sChar == '\r')) && (sPrevChar == '$')) 498 | break; // exit out of the loop 499 | sPrevChar = sChar; 500 | } 501 | 502 | 503 | if ((sChar = SSCSerial.read()) != -1) { 504 | DBGSerial.write(sChar & 0xff); 505 | } 506 | } 507 | DBGSerial.println("Exited SSC Forwarder mode"); 508 | } 509 | #endif // OPT_SSC_FORWARDER 510 | //============================================================================== 511 | // FindServoOffsets - Find the zero points for each of our servos... 512 | // Will use the new servo function to set the actual pwm rate and see 513 | // how well that works... 514 | //============================================================================== 515 | #ifdef OPT_FIND_SERVO_OFFSETS 516 | #ifndef NUMSERVOSPERLEG 517 | #define NUMSERVOSPERLEG 3 518 | #endif 519 | 520 | void FindServoOffsets() 521 | { 522 | // not clean but... 523 | byte abSSCServoNum[NUMSERVOSPERLEG*CNT_LEGS]; // array of servos... 524 | signed short asOffsets[NUMSERVOSPERLEG*CNT_LEGS]; // we have 18 servos to find/set offsets for... 525 | signed char asOffsetsRead[NUMSERVOSPERLEG*CNT_LEGS]; // array for our read in servos... 526 | 527 | static char *apszLegs[] = { 528 | #ifdef QUADMODE 529 | "RR","RF", "LR", "LF" }; // Leg Order 530 | #else 531 | "RR","RM","RF", "LR", "LM", "LF" }; // Leg Order 532 | #endif 533 | static char *apszLJoints[] = { 534 | " Coxa", " Femur", " Tibia", " tArs" }; // which joint on the leg... 535 | 536 | byte szTemp[5]; 537 | byte cbRead; 538 | 539 | int data; 540 | short sSN ; // which servo number 541 | boolean fNew = true; // is this a new servo to work with? 542 | boolean fExit = false; // when to exit 543 | 544 | if (CheckVoltage()) { 545 | // Voltage is low... 546 | Serial.println("Low Voltage: fix or hit $ to abort"); 547 | while (CheckVoltage()) { 548 | if (Serial.read() == '$') return; 549 | } 550 | } 551 | 552 | // Fill in array of SSC-32 servo numbers 553 | for (sSN=0; sSN < CNT_LEGS; sSN++) { // Make sure all of our servos initialize to 0 offset from saved. 554 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 0] = pgm_read_byte(&cCoxaPin[sSN]); 555 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 1] = pgm_read_byte(&cFemurPin[sSN]); 556 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 2] = pgm_read_byte(&cTibiaPin[sSN]); 557 | #ifdef c4DOF 558 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 3] = pgm_read_byte(&cTarsPin[sSN]); 559 | #endif 560 | } 561 | // now lets loop through and get information and set servos to 1500 562 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++ ) { 563 | asOffsets[sSN] = 0; 564 | asOffsetsRead[sSN] = 0; 565 | 566 | SSCSerial.print("R"); 567 | SSCSerial.println(32+abSSCServoNum[sSN], DEC); 568 | // now read in the current value... Maybe should use atoi... 569 | cbRead = SSCRead((byte*)szTemp, sizeof(szTemp), 10000, 13); 570 | if (cbRead > 0) 571 | asOffsetsRead[sSN] = atoi((const char *)szTemp); 572 | 573 | SSCSerial.print("#"); 574 | SSCSerial.print(abSSCServoNum[sSN], DEC); 575 | SSCSerial.println("P1500"); 576 | } 577 | 578 | // OK lets move all of the servos to their zero point. 579 | Serial.println("Find Servo Zeros.\n$-Exit, +- changes, *-change servo"); 580 | Serial.println(" 0-n Chooses a leg, C-Coxa, F-Femur, T-Tibia"); 581 | 582 | sSN = true; 583 | while(!fExit) { 584 | if (fNew) { 585 | Serial.print("Servo: "); 586 | Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]); 587 | Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]); 588 | Serial.print("("); 589 | Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 590 | Serial.println(")"); 591 | 592 | // Now lets wiggle the servo 593 | SSCSerial.print("#"); 594 | SSCSerial.print(abSSCServoNum[sSN], DEC); 595 | SSCSerial.print("P"); 596 | SSCSerial.print(1500+asOffsets[sSN]+250, DEC); 597 | SSCSerial.println("T250"); 598 | delay(250); 599 | 600 | SSCSerial.print("#"); 601 | SSCSerial.print(abSSCServoNum[sSN], DEC); 602 | SSCSerial.print("P"); 603 | SSCSerial.print(1500+asOffsets[sSN]-250, DEC); 604 | SSCSerial.println("T500"); 605 | delay(500); 606 | 607 | SSCSerial.print("#"); 608 | SSCSerial.print(abSSCServoNum[sSN], DEC); 609 | SSCSerial.print("P"); 610 | SSCSerial.print(1500+asOffsets[sSN], DEC); 611 | SSCSerial.println("T250"); 612 | delay(250); 613 | 614 | fNew = false; 615 | } 616 | 617 | //get user entered data 618 | data = Serial.read(); 619 | //if data received 620 | if (data !=-1) { 621 | if (data == '$') 622 | fExit = true; // not sure how the keypad will map so give NL, CR, LF... all implies exit 623 | 624 | else if ((data == '+') || (data == '-')) { 625 | if (data == '+') 626 | asOffsets[sSN] += 5; // increment by 5us 627 | else 628 | asOffsets[sSN] -= 5; // increment by 5us 629 | 630 | Serial.print(" "); 631 | Serial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 632 | 633 | SSCSerial.print("#"); 634 | SSCSerial.print(abSSCServoNum[sSN], DEC); 635 | SSCSerial.print("P"); 636 | SSCSerial.print(1500+asOffsets[sSN], DEC); 637 | SSCSerial.println("T100"); 638 | } 639 | else if ((data >= '0') && (data <= '5')) { 640 | // direct enter of which servo to change 641 | fNew = true; 642 | sSN = (sSN % NUMSERVOSPERLEG) + (data - '0')*NUMSERVOSPERLEG; 643 | } 644 | else if ((data == 'c') || (data == 'C')) { 645 | fNew = true; 646 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 0; 647 | } 648 | else if ((data == 'f') || (data == 'F')) { 649 | fNew = true; 650 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 1; 651 | } 652 | else if ((data == 't') || (data == 'T')) { 653 | // direct enter of which servo to change 654 | fNew = true; 655 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 2; 656 | } 657 | else if (data == '*') { 658 | // direct enter of which servo to change 659 | fNew = true; 660 | sSN++; 661 | if (sSN == CNT_LEGS*NUMSERVOSPERLEG) 662 | sSN = 0; 663 | } 664 | } 665 | } 666 | Serial.print("Find Servo exit "); 667 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++){ 668 | Serial.print("Servo: "); 669 | Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]); 670 | Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]); 671 | Serial.print("("); 672 | Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 673 | Serial.println(")"); 674 | } 675 | 676 | Serial.print("\nSave Changes? Y/N: "); 677 | 678 | //get user entered data 679 | while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15))) 680 | ; 681 | 682 | if ((data == 'Y') || (data == 'y')) { 683 | // Ok they asked for the data to be saved. We will store the data with a 684 | // number of servos (byte)at the start, followed by a byte for a checksum...followed by our offsets array... 685 | // Currently we store these values starting at EEPROM address 0. May later change... 686 | // 687 | 688 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++ ) { 689 | SSCSerial.print("R"); 690 | SSCSerial.print(32+abSSCServoNum[sSN], DEC); 691 | SSCSerial.print("="); 692 | SSCSerial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 693 | delay(10); 694 | } 695 | 696 | // Then I need to have the SSC-32 reboot in order to use the new values. 697 | delay(10); // give it some time to write stuff out. 698 | SSCSerial.println("GOBOOT"); 699 | delay(5); // Give it a little time 700 | SSCSerial.println("g0000"); // tell it that we are done in the boot section so go run the normall SSC stuff... 701 | delay(500); // Give it some time to boot up... 702 | 703 | } 704 | else { 705 | void LoadServosConfig(); 706 | } 707 | 708 | g_ServoDriver.FreeServos(); 709 | 710 | } 711 | #endif // OPT_FIND_SERVO_OFFSETS 712 | 713 | #endif // 714 | -------------------------------------------------------------------------------- /Quad_PS2_SSC32/Jeb_Input_PS2.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Project: Jebediah the Quadruped Robot 3 | // 4 | // Description: This code controls a quadruped robot with three degrees of 5 | // freedom per leg. 6 | // 7 | // The control input subroutine for the Capers software is placed in this file. 8 | // 9 | // Hardware setup: PS2 version 10 | // 11 | // PS2 CONTROLS: 12 | // [Common Controls] 13 | // Start - turn on/off the bot 14 | // L1Toggle - shift mode 15 | // L2Toggle - rotate mode 16 | // CircleToggle - single leg mode 17 | // Square - toggle balance mode 18 | // TriangleMove - body to 35 mm from the ground (walk pos) 19 | // and back to the ground 20 | // D-Pad up - body up 10 mm 21 | // D-Pad down - body down 10 mm 22 | // D-Pad left - decrease speed with 50mS 23 | // D-Pad right - increase speed with 50mS 24 | // 25 | // Optional: L3 button down, Left stick can adjust leg positions... 26 | // or if OPT_SINGLELEG is not defined may try using Circle 27 | // 28 | // [Walk Controls] 29 | // select - switch gaits 30 | // Left stick - (Walk mode 1) Walk/Strafe 31 | // (Walk mode 2) Disable 32 | // Right stick - (Walk mode 1) Rotate, 33 | // (Walk mode 2) Walk/Rotate 34 | // R1Toggle - double gait travel speed 35 | // R2Toggle - double gait travel length 36 | // 37 | // [Shift Controls] 38 | // Left stick - shift body X/Z 39 | // Right stick - shift body Y and rotate body Y 40 | // 41 | // [Rotate Controls] 42 | // Left stick - rotate body X/Z 43 | // Right Stick - rotate body Y 44 | // 45 | // [Single leg Controls] 46 | // select - switch legs 47 | // Left stick - move Leg X/Z (relative) 48 | // Right stick - move Leg Y (absolute) 49 | // R2Hold/release - leg position 50 | // 51 | // [GP Player Controls] 52 | // select - switch Sequences 53 | // R2Start - sequence 54 | // 55 | //==================================================================== 56 | // [Include files] 57 | #if ARDUINO>99 58 | #include // Arduino 1.0 59 | #else 60 | #include // Arduino 0022 61 | #endif 62 | #include 63 | 64 | //[CONSTANTS] 65 | #define WALKMODE 0 66 | #define TRANSLATEMODE 1 67 | #define ROTATEMODE 2 68 | #define SINGLELEGMODE 3 69 | #define GPPLAYERMODE 4 70 | 71 | 72 | #define cTravelDeadZone 4 //The deadzone for the analog input from the remote 73 | #define MAXPS2ERRORCNT 5 // How many times through the loop will we go before shutting off robot? 74 | 75 | #ifndef MAX_BODY_Y 76 | #define MAX_BODY_Y 100 77 | #endif 78 | 79 | //============================================================================= 80 | // Global - Local to this file only... 81 | //============================================================================= 82 | PS2X ps2x; // create PS2 Controller Class 83 | 84 | 85 | // Define an instance of the Input Controller... 86 | InputController g_InputController; // Our Input controller 87 | 88 | 89 | static short g_BodyYOffset; 90 | static short g_sPS2ErrorCnt; 91 | static short g_BodyYShift; 92 | static byte ControlMode; 93 | static bool DoubleHeightOn; 94 | static bool DoubleTravelOn; 95 | static bool WalkMethod; 96 | byte GPSeq; //Number of the sequence 97 | short g_sGPSMController; // What GPSM value have we calculated. 0xff - Not used yet 98 | 99 | // some external or forward function references. 100 | extern void PS2TurnRobotOff(void); 101 | 102 | //============================================================================== 103 | // This is The function that is called by the Main program to initialize 104 | //the input controller, which in this case is the PS2 controller 105 | //process any commands. 106 | //============================================================================== 107 | 108 | // If both PS2 and XBee are defined then we will become secondary to the xbee 109 | void InputController::Init(void) 110 | { 111 | int error; 112 | 113 | //error = ps2x.config_gamepad(57, 55, 56, 54); // Setup gamepad (clock, command, attention, data) pins 114 | error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT); // Setup gamepad (clock, command, attention, data) pins 115 | 116 | #ifdef DBGSerial 117 | DBGSerial.print("PS2 Init: "); 118 | DBGSerial.println(error, DEC); 119 | #endif 120 | g_BodyYOffset = 0; 121 | g_BodyYShift = 0; 122 | g_sPS2ErrorCnt = 0; // error count 123 | 124 | ControlMode = WALKMODE; 125 | DoubleHeightOn = false; 126 | DoubleTravelOn = false; 127 | WalkMethod = false; 128 | 129 | g_InControlState.SpeedControl = 100; // Sort of migrate stuff in from Devon. 130 | } 131 | 132 | //============================================================================== 133 | // This function is called by the main code to tell us when it is about to 134 | // do a lot of bit-bang outputs and it would like us to minimize any interrupts 135 | // that we do while it is active... 136 | //============================================================================== 137 | void InputController::AllowControllerInterrupts(boolean fAllow) 138 | { 139 | // We don't need to do anything... 140 | } 141 | 142 | //============================================================================== 143 | // This is The main code to input function to read inputs from the PS2 and then 144 | //process any commands. 145 | //============================================================================== 146 | void InputController::ControlInput(void) 147 | { 148 | boolean fAdjustLegPositions = false; 149 | // Then try to receive a packet of information from the PS2. 150 | // Then try to receive a packet of information from the PS2. 151 | ps2x.read_gamepad(); //read controller and set large motor to spin at 'vibrate' speed 152 | 153 | // Wish the library had a valid way to verify that the read_gamepad succeeded... Will hack for now 154 | if ((ps2x.Analog(1) & 0xf0) == 0x70) { 155 | #ifdef DBGSerial 156 | #ifdef DEBUG_PS2_INPUT 157 | if (g_fDebugOutput) { 158 | DBGSerial.print("PS2 Input: "); 159 | DBGSerial.print(ps2x.ButtonDataByte(), HEX); 160 | DBGSerial.print(":"); 161 | DBGSerial.print(ps2x.Analog(PSS_LX), DEC); 162 | DBGSerial.print(" "); 163 | DBGSerial.print(ps2x.Analog(PSS_LY), DEC); 164 | DBGSerial.print(" "); 165 | DBGSerial.print(ps2x.Analog(PSS_RX), DEC); 166 | DBGSerial.print(" "); 167 | DBGSerial.println(ps2x.Analog(PSS_RY), DEC); 168 | } 169 | #endif 170 | #endif 171 | // In an analog mode so should be OK... 172 | g_sPS2ErrorCnt = 0; // clear out error count... 173 | 174 | if (ps2x.ButtonPressed(PSB_START)) {// OK lets try "0" button for Start. 175 | if (g_InControlState.fRobotOn) { 176 | PS2TurnRobotOff(); 177 | } 178 | else { 179 | //Turn on 180 | g_InControlState.fRobotOn = 1; 181 | fAdjustLegPositions = true; 182 | } 183 | } 184 | 185 | if (g_InControlState.fRobotOn) { 186 | // [SWITCH MODES] 187 | 188 | //Translate mode 189 | if (ps2x.ButtonPressed(PSB_L1)) {// L1 Button Test 190 | MSound( 1, 50, 2000); 191 | if (ControlMode != TRANSLATEMODE ) 192 | ControlMode = TRANSLATEMODE; 193 | else { 194 | if (g_InControlState.SelectedLeg==255) 195 | ControlMode = WALKMODE; 196 | else 197 | ControlMode = SINGLELEGMODE; 198 | } 199 | } 200 | 201 | //Rotate mode 202 | if (ps2x.ButtonPressed(PSB_L2)) { // L2 Button Test 203 | MSound( 1, 50, 2000); 204 | if (ControlMode != ROTATEMODE) 205 | ControlMode = ROTATEMODE; 206 | else { 207 | if (g_InControlState.SelectedLeg == 255) 208 | ControlMode = WALKMODE; 209 | else 210 | ControlMode = SINGLELEGMODE; 211 | } 212 | } 213 | 214 | //Single leg mode fNO 215 | if (ps2x.ButtonPressed(PSB_CIRCLE)) {// O - Circle Button Test 216 | if (abs(g_InControlState.TravelLength.x)0) 258 | g_BodyYOffset = 0; 259 | else 260 | g_BodyYOffset = 35; 261 | fAdjustLegPositions = true; 262 | } 263 | 264 | if (ps2x.ButtonPressed(PSB_PAD_UP)) {// D-Up - Button Test 265 | g_BodyYOffset += 10; 266 | 267 | // And see if the legs should adjust... 268 | fAdjustLegPositions = true; 269 | if (g_BodyYOffset > MAX_BODY_Y) 270 | g_BodyYOffset = MAX_BODY_Y; 271 | } 272 | 273 | if (ps2x.ButtonPressed(PSB_PAD_DOWN) && g_BodyYOffset) {// D-Down - Button Test 274 | if (g_BodyYOffset > 10) 275 | g_BodyYOffset -= 10; 276 | else 277 | g_BodyYOffset = 0; // constrain don't go less than zero. 278 | 279 | // And see if the legs should adjust... 280 | fAdjustLegPositions = true; 281 | } 282 | 283 | if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) { // D-Right - Button Test 284 | if (g_InControlState.SpeedControl>0) { 285 | g_InControlState.SpeedControl = g_InControlState.SpeedControl - 50; 286 | MSound( 1, 50, 2000); 287 | } 288 | } 289 | 290 | if (ps2x.ButtonPressed(PSB_PAD_LEFT)) { // D-Left - Button Test 291 | if (g_InControlState.SpeedControl<2000 ) { 292 | g_InControlState.SpeedControl = g_InControlState.SpeedControl + 50; 293 | MSound( 1, 50, 2000); 294 | } 295 | } 296 | 297 | //[Walk functions] 298 | if (ControlMode == WALKMODE) { 299 | //Switch gates 300 | if (ps2x.ButtonPressed(PSB_SELECT) // Select Button Test 301 | && abs(g_InControlState.TravelLength.x) (128+16)) || (ps2x.Analog(PSS_RY) < (128-16))) 403 | { 404 | // We are in speed modify mode... 405 | short sNewGPSM = map(ps2x.Analog(PSS_RY), 0, 255, -200, 200); 406 | if (sNewGPSM != g_sGPSMController) { 407 | g_sGPSMController = sNewGPSM; 408 | g_ServoDriver.GPSetSpeedMultiplyer(g_sGPSMController); 409 | } 410 | 411 | } 412 | } 413 | 414 | //Switch between sequences 415 | if (ps2x.ButtonPressed(PSB_SELECT)) { // Select Button Test 416 | if (!g_ServoDriver.FIsGPSeqActive() ) { 417 | if (GPSeq < 5) { //Max sequence 418 | MSound(1, 50, 1500); 419 | GPSeq = GPSeq+1; 420 | } 421 | else { 422 | MSound(2, 50, 2000, 50, 2250); 423 | GPSeq=0; 424 | } 425 | } 426 | } 427 | //Start Sequence 428 | if (ps2x.ButtonPressed(PSB_R2))// R2 Button Test 429 | if (!g_ServoDriver.FIsGPSeqActive() ) { 430 | g_ServoDriver.GPStartSeq(GPSeq); 431 | g_sGPSMController = 32767; // Say that we are not in Speed modify mode yet... valid ranges are 50-200 (both postive and negative... 432 | } 433 | else { 434 | g_ServoDriver.GPStartSeq(0xff); // tell the GP system to abort if possible... 435 | MSound (2, 50, 2000, 50, 2000); 436 | } 437 | 438 | 439 | } 440 | #endif // OPT_GPPLAYER 441 | 442 | //Calculate walking time delay 443 | g_InControlState.InputTimeDelay = 128 - max(max(abs(ps2x.Analog(PSS_LX) - 128), abs(ps2x.Analog(PSS_LY) - 128)), abs(ps2x.Analog(PSS_RX) - 128)); 444 | } 445 | 446 | //Calculate g_InControlState.BodyPos.y 447 | g_InControlState.BodyPos.y = min(max(g_BodyYOffset + g_BodyYShift, 0), MAX_BODY_Y); 448 | if (fAdjustLegPositions) 449 | AdjustLegPositionsToBodyHeight(); // Put main workings into main program file 450 | } 451 | else { 452 | // We may have lost the PS2... See what we can do to recover... 453 | if (g_sPS2ErrorCnt < MAXPS2ERRORCNT) 454 | g_sPS2ErrorCnt++; // Increment the error count and if to many errors, turn off the robot. 455 | else if (g_InControlState.fRobotOn) 456 | PS2TurnRobotOff(); 457 | ps2x.reconfig_gamepad(); 458 | } 459 | } 460 | 461 | //============================================================================== 462 | // PS2TurnRobotOff - code used couple of places so save a little room... 463 | //============================================================================== 464 | void PS2TurnRobotOff(void) 465 | { 466 | //Turn off 467 | g_InControlState.BodyPos.x = 0; 468 | g_InControlState.BodyPos.y = 0; 469 | g_InControlState.BodyPos.z = 0; 470 | g_InControlState.BodyRot1.x = 0; 471 | g_InControlState.BodyRot1.y = 0; 472 | g_InControlState.BodyRot1.z = 0; 473 | g_InControlState.TravelLength.x = 0; 474 | g_InControlState.TravelLength.z = 0; 475 | g_InControlState.TravelLength.y = 0; 476 | g_BodyYOffset = 0; 477 | g_BodyYShift = 0; 478 | g_InControlState.SelectedLeg = 255; 479 | g_InControlState.fRobotOn = 0; 480 | AdjustLegPositionsToBodyHeight(); // Put main workings into main program file 481 | } 482 | 483 | 484 | 485 | 486 | -------------------------------------------------------------------------------- /Quad_PS2_SSC32/Quad_Cfg.h: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Project: Jebediah the Quadruped Robot 3 | // 4 | // Description: This code controls a quadruped robot with three degrees of 5 | // freedom per leg. 6 | // 7 | // This version of the configuration file is set up to run on the 8 | // Lynxmotion Botboardduino board, which is similar to the Arduino Duemilanove. 9 | // 10 | // This version of configuration file assumes that the servos will be controlled 11 | // by a Lynxmotion servo controller SSC-32 and the user is using a Lynxmotion 12 | // PS2 to control the robot. 13 | // 14 | //==================================================================== 15 | #ifndef HEX_CFG_CHR3_H 16 | #define HEX_CFG_CHR3_H 17 | 18 | //#define DEBUG 19 | 20 | #ifdef __AVR__ 21 | #if defined(UBRR1H) 22 | #define SSCSerial Serial1 23 | #define XBeeSerial Serial3 //Serial2 24 | #else 25 | #define XBeeSerial Serial 26 | #define DontAllowDebug 27 | #endif 28 | #else // For My Pic32 Mega shield... 29 | #define SSCSerial Serial1 30 | #define XBeeSerial Serial3 31 | #endif 32 | 33 | #define QUADMODE // We are building a quadruped rather than the default 34 | // hexapod robot 35 | 36 | //#define USEMULTI 37 | //#define USEXBEE // only allow to be defined on Megas... 38 | #define USEPS2 39 | //#define USECOMMANDER 40 | //#define USESERIAL 41 | 42 | // Do we want Debug Serial Output? 43 | #define DBGSerial Serial 44 | 45 | // Some configurations will not allow this so if one of them undefine it 46 | #if (defined USEXBEE) || (defined USECOMMANDER) 47 | #ifdef DontAllowDebug 48 | #undef DBGSerial 49 | #endif 50 | #endif 51 | 52 | #ifdef USESERIAL 53 | #undef DBGSerial 54 | #endif 55 | 56 | #ifdef DBGSerial 57 | #define OPT_TERMINAL_MONITOR // Only allow this to be defined if we have a debug serial port 58 | #endif 59 | 60 | #ifdef OPT_TERMINAL_MONITOR 61 | //#define OPT_SSC_FORWARDER // only useful if terminal monitor is enabled 62 | #define OPT_FIND_SERVO_OFFSETS // Only useful if terminal monitor is enabled 63 | #endif 64 | 65 | //#define OPT_GPPLAYER 66 | 67 | #define USE_SSC32 68 | #define cSSC_BINARYMODE 1 // Define if your SSC-32 card supports binary mode. 69 | #define cSSC_BAUD 38400 //SSC32 BAUD rate 70 | 71 | //-------------------------------------------------------------------- 72 | // [Debug options] 73 | //#define DEBUG_IOPINS // used to control if we are going to use IO pins for debug support 74 | 75 | //-------------------------------------------------------------------- 76 | //[Botboarduino Pin Numbers] 77 | #define SOUND_PIN 5 // Botboarduino JR pin number 78 | // PS2 controller connections 79 | #define PS2_DAT 6 80 | #define PS2_CMD 7 81 | #define PS2_SEL 8 82 | #define PS2_CLK 9 83 | // Connections to SSC-32 servo controller 84 | #define cSSC_OUT 10 // Output pin for (SSC32 RX) on BotBoard (Yellow) 85 | #define cSSC_IN 11 // Input pin for (SSC32 TX) on BotBoard (Blue) 86 | // Connections to NeoPiexel Ring eye 87 | #define eye_data 3 88 | 89 | // XBee was defined to use a hardware Serial port 90 | #define XBEE_BAUD 38400 91 | #define SERIAL_BAUD 38400 92 | 93 | // Define Analog pin and minimum voltage that we will allow the servos to run 94 | #define cVoltagePin 0 // Use our Analog pin jumper here... 95 | #define cTurnOffVol 470 // 4.7v 96 | #define cTurnOnVol 550 // 5.5V - optional part to say if voltage goes back up, turn it back on... 97 | 98 | //-------------------------------------------------------------------- 99 | //[SSC32 Pin Numbers] 100 | #define cRRCoxaPin 0 //Rear Right Horizontal 101 | #define cRRFemurPin 1 //Rear Right Vertical 102 | #define cRRTibiaPin 2 //Rear Right Tibia 103 | #define cRRTarsPin 3 // Tar 104 | 105 | #define cRFCoxaPin 8 //Front Right Horizontal 106 | #define cRFFemurPin 9 //Front Right Vertical 107 | #define cRFTibiaPin 10 //Front Right Tibia 108 | #define cRFTarsPin 11 // Tar 109 | 110 | #define cLRCoxaPin 16 //Rear Left Horizontal 111 | #define cLRFemurPin 17 //Rear Left Vertical 112 | #define cLRTibiaPin 18 //Rear Left Tibia 113 | #define cLRTarsPin 19 // Tar 114 | 115 | #define cLFCoxaPin 24 //Front Left Horizontal 116 | #define cLFFemurPin 25 //Front Left Vertical 117 | #define cLFTibiaPin 26 //Front Left Tibia 118 | #define cLFTarsPin 27 // Tar 119 | 120 | //-------------------------------------------------------------------- 121 | //[SERVO PULSE INVERSE] 122 | #define cRRCoxaInv 0 123 | #define cRRFemurInv 0 124 | #define cRRTibiaInv 0 125 | #define cRRTarsInv 0 126 | 127 | #define cRFCoxaInv 1 128 | #define cRFFemurInv 1 129 | #define cRFTibiaInv 1 130 | #define cRFTarsInv 1 131 | 132 | #define cLRCoxaInv 1 133 | #define cLRFemurInv 1 134 | #define cLRTibiaInv 1 135 | #define cLRTarsInv 1 136 | 137 | #define cLFCoxaInv 0 138 | #define cLFFemurInv 0 139 | #define cLFTibiaInv 0 140 | #define cLFTarsInv 0 141 | 142 | //-------------------------------------------------------------------- 143 | //[MIN/MAX ANGLES] 144 | #define cRRCoxaMin1 -650 //Mechanical limits of the Right Rear Leg 145 | #define cRRCoxaMax1 650 146 | #define cRRFemurMin1 -1050 147 | #define cRRFemurMax1 750 148 | #define cRRTibiaMin1 -420 149 | #define cRRTibiaMax1 900 150 | #define cRRTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg 151 | #define cRRTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though.. 152 | 153 | #define cRFCoxaMin1 -650 //Mechanical limits of the Right Front Leg 154 | #define cRFCoxaMax1 650 155 | #define cRFFemurMin1 -1050 156 | #define cRFFemurMax1 750 157 | #define cRFTibiaMin1 -420 158 | #define cRFTibiaMax1 900 159 | #define cRFTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg 160 | #define cRFTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though.. 161 | 162 | #define cLRCoxaMin1 -650 //Mechanical limits of the Left Rear Leg 163 | #define cLRCoxaMax1 650 164 | #define cLRFemurMin1 -1050 165 | #define cLRFemurMax1 750 166 | #define cLRTibiaMin1 -420 167 | #define cLRTibiaMax1 900 168 | #define cLRTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg 169 | #define cLRTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though.. 170 | 171 | #define cLFCoxaMin1 -650 //Mechanical limits of the Left Front Leg 172 | #define cLFCoxaMax1 650 173 | #define cLFFemurMin1 -1050 174 | #define cLFFemurMax1 750 175 | #define cLFTibiaMin1 -420 176 | #define cLFTibiaMax1 900 177 | #define cLFTarsMin1 -1300 //4DOF ONLY - In theory the kinematics can reach about -160 deg 178 | #define cLFTarsMax1 500 //4DOF ONLY - The kinematics will never exceed 23 deg though.. 179 | 180 | //-------------------------------------------------------------------- 181 | // [LEG DIMENSIONS] 182 | // Universal dimensions for each leg in mm 183 | // Use these universal dimensions if each of the robot's legs has 184 | // the same dimensions. 185 | #define cXXCoxaLength 65 // Length from coxa servo horn to first femur servo horn 186 | #define cXXFemurLength 103 // Length between femur servo horns 187 | #define cXXTibiaLength 161 // Length from second femur servo horn to end of tibia 188 | #define cXXTarsLength 85 // 4DOF only... 189 | 190 | // Individual dimensions for each leg in mm 191 | #define cRRCoxaLength cXXCoxaLength //Right Rear leg 192 | #define cRRFemurLength cXXFemurLength 193 | #define cRRTibiaLength cXXTibiaLength 194 | #define cRRTarsLength cXXTarsLength //4DOF ONLY 195 | 196 | #define cRFCoxaLength cXXCoxaLength //Rigth front leg 197 | #define cRFFemurLength cXXFemurLength 198 | #define cRFTibiaLength cXXTibiaLength 199 | #define cRFTarsLength cXXTarsLength //4DOF ONLY 200 | 201 | #define cLRCoxaLength cXXCoxaLength //Left Rear leg 202 | #define cLRFemurLength cXXFemurLength 203 | #define cLRTibiaLength cXXTibiaLength 204 | #define cLRTarsLength cXXTarsLength //4DOF ONLY 205 | 206 | #define cLFCoxaLength cXXCoxaLength //Left front leg 207 | #define cLFFemurLength cXXFemurLength 208 | #define cLFTibiaLength cXXTibiaLength 209 | #define cLFTarsLength cXXTarsLength //4DOF ONLY 210 | 211 | 212 | //-------------------------------------------------------------------- 213 | //[BODY DIMENSIONS] 214 | // This first section defines the angle from the center of the body to each of the legs 215 | #define cRRCoxaAngle1 -450 //Default Coxa setup angle, decimals = 1 216 | #define cRFCoxaAngle1 450 //Default Coxa setup angle, decimals = 1 217 | #define cLRCoxaAngle1 -450 //Default Coxa setup angle, decimals = 1 218 | #define cLFCoxaAngle1 450 //Default Coxa setup angle, decimals = 1 219 | 220 | // This second section defines the distances between the center of the body to each of the legs 221 | #define cRROffsetX -70 //Distance X from center of the body to the Right Rear coxa 222 | #define cRROffsetZ 70 //Distance Z from center of the body to the Right Rear coxa 223 | #define cRFOffsetX -70 //Distance X from center of the body to the Right Front coxa 224 | #define cRFOffsetZ -70 //Distance Z from center of the body to the Right Front coxa 225 | #define cLROffsetX 70 //Distance X from center of the body to the Left Rear coxa 226 | #define cLROffsetZ 70 //Distance Z from center of the body to the Left Rear coxa 227 | #define cLFOffsetX 70 //Distance X from center of the body to the Left Front coxa 228 | #define cLFOffsetZ -70 //Distance Z from center of the body to the Left Front coxa 229 | 230 | //-------------------------------------------------------------------- 231 | //[START POSITIONS FEET] 232 | #define cHexInitXZ 110 233 | #define CHexInitXZCos60 55 // COS(60) = .5 234 | #define CHexInitXZSin60 95 // sin(60) = .866 235 | #define CHexInitXZ45 78 // Sin and cos(45) .7071 236 | #define CHexInitY 60 //30 237 | 238 | #if 1 239 | #define cRRInitPosX CHexInitXZ45 //Start positions of the Right Rear leg 240 | #define cRRInitPosY CHexInitY 241 | #define cRRInitPosZ CHexInitXZ45 242 | 243 | #define cRFInitPosX CHexInitXZ45 //Start positions of the Right Front leg 244 | #define cRFInitPosY CHexInitY 245 | #define cRFInitPosZ -CHexInitXZ45 246 | 247 | #define cLRInitPosX CHexInitXZ45 //Start positions of the Left Rear leg 248 | #define cLRInitPosY CHexInitY 249 | #define cLRInitPosZ CHexInitXZ45 250 | 251 | #define cLFInitPosX CHexInitXZ45 //Start positions of the Left Front leg 252 | #define cLFInitPosY CHexInitY 253 | #define cLFInitPosZ -CHexInitXZ45 254 | #endif 255 | //-------------------------------------------------------------------- 256 | //[Tars factors used in formula to calc Tarsus angle relative to the ground] 257 | #define cTarsConst 720 //4DOF ONLY 258 | #define cTarsMulti 2 //4DOF ONLY 259 | #define cTarsFactorA 70 //4DOF ONLY 260 | #define cTarsFactorB 60 //4DOF ONLY 261 | #define cTarsFactorC 50 //4DOF ONLY 262 | 263 | #endif CFG_HEX_H 264 | 265 | -------------------------------------------------------------------------------- /Quad_PS2_SSC32/Quad_PS2_SSC32.ino: -------------------------------------------------------------------------------- 1 | //============================================================================= 2 | // Project: Jebediah the Quadruped Robot 3 | // 4 | // Description: This code controls a quadruped robot with three degrees of 5 | // freedom per leg. 6 | // 7 | // This code is specifically configured for the Lynxmotion BotBoarduino using the 8 | // SSC-32 servo controller. 9 | // 10 | // Before uploading this code to your robot, make sure to properly configure the 11 | // settings in the Quad_CFG.h file. The settings which will need to be configured 12 | // for your specific robot are 13 | // * The lenghts of each leg section 14 | // * The mechanical limits of each servo 15 | // * The dimensions of the body 16 | // * The angular offsets between the center of the body and each leg 17 | // * The distance offset between the center of the body and each leg 18 | // * Feed starting positions 19 | // 20 | //============================================================================= 21 | // Header Files 22 | //============================================================================= 23 | 24 | #define DEFINE_HEX_GLOBALS 25 | #if ARDUINO>99 26 | #include 27 | #else 28 | #endif 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #ifdef __AVR__ 35 | #include 36 | #endif 37 | 38 | #include 39 | #include "Quad_CFG.h" 40 | #include "Jeb.h" 41 | #include "Jeb_Input_PS2.h" 42 | #include "Jeb_Driver_SSC32.h" 43 | #include "Jeb_Code.h" 44 | 45 | -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Jebediah the Quadruped Robot 2 | 3 | ## Project Description 4 | 5 | Jeb is a quadruped robot that the code in this repository is designed to control. Quadruped robots are robots with four legs. Jeb is designed with three degrees of freedom per leg. Each leg is actuated by three servos. The first servo moves the leg forward/backward, the second servo moves the leg up and down, and the third servo bends the leg in the middle. With four legs and three servos per leg, the robot is driven by 12 servos in total. 6 | 7 | The parts necessary to build the robot's frame can be found in the *frame* directory of this repository. The frame is design to be 3D printed. 8 | 9 | ## Attribution 10 | 11 | The code in this repository was originally developed by GitHub user KurtE. 12 | The source of this fork is: [https://github.com/KurtE/Arduino_Phoenix_Parts](https://github.com/KurtE/Arduino_Phoenix_Parts). 13 | 14 | ## Hardware 15 | 16 | Generally speaking, a quadruped robot's electronics system has three main subsystems: the microcontroller, server controller, and control input. 17 | 18 | This code assumes that the quadruped uses the following electronics systems: 19 | 20 | * The microcontroller is a Botboarduino from [Lynxmotion](http://www.lynxmotion.com/c-153-botboarduino.aspx). 21 | * The servo controller is an SSC-32U from [Lynxmotion](http://www.lynxmotion.com/p-1032-ssc-32u-usb-servo-controller.aspx). 22 | * For input the robot uses a wireless Platstation 2 controller like one from [Lynxmotion](http://www.lynxmotion.com/p-1096-ps2-robot-controller-v4.aspx) 23 | 24 | This repository also includes a library for using NeoPixel LED lighting products from [Adafruit](https://www.adafruit.com/category/168). NeoPixels, which 25 | come in a variety of shapes and sizes allow the easy addition of lighting effects or visual feedback to the robot. Jeb uses a 16 LED NeoPixel ring for 26 | the eye. 27 | 28 | Quadruped robots use a total of 12 servos. Since the robot itself can be quite heavy depending upon the construction, it is important to equip 29 | the robot with servos that are strong enough to support the robot's weight. Servos which provide at least 8 kg/cm of torque are 30 | recommended. 31 | 32 | The robot's frame is constructed from 3D printed materials; the exact material used is not particularly important. The design files for the frame can be found in the *frame* directory of this repository. 33 | 34 | ## Installation 35 | 36 | To use this code you need to copy each of the folders inside the *libraries* directory into your Arduino Library directory. 37 | 38 | In the Arduino IDE, open **Quad_PS2_SSC32.ino**. Customize the code as needed to suit your robot's geometry and supplemental hardware (like LEDs). 39 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | sudo: false 3 | before_install: 4 | - source <(curl -SLs https://raw.githubusercontent.com/adafruit/travis-ci-arduino/master/install.sh) 5 | script: 6 | - build_main_platforms 7 | - build_platform trinket 8 | notifications: 9 | email: 10 | on_success: change 11 | on_failure: change 12 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/Adafruit_NeoPixel.cpp: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------- 2 | Arduino library to control a wide variety of WS2811- and WS2812-based RGB 3 | LED devices such as Adafruit FLORA RGB Smart Pixels and NeoPixel strips. 4 | Currently handles 400 and 800 KHz bitstreams on 8, 12 and 16 MHz ATmega 5 | MCUs, with LEDs wired for various color orders. 8 MHz MCUs provide 6 | output on PORTB and PORTD, while 16 MHz chips can handle most output pins 7 | (possible exception with upper PORT registers on the Arduino Mega). 8 | 9 | Written by Phil Burgess / Paint Your Dragon for Adafruit Industries, 10 | contributions by PJRC, Michael Miller and other members of the open 11 | source community. 12 | 13 | Adafruit invests time and resources providing this open source code, 14 | please support Adafruit and open-source hardware by purchasing products 15 | from Adafruit! 16 | 17 | ------------------------------------------------------------------------- 18 | This file is part of the Adafruit NeoPixel library. 19 | 20 | NeoPixel is free software: you can redistribute it and/or modify 21 | it under the terms of the GNU Lesser General Public License as 22 | published by the Free Software Foundation, either version 3 of 23 | the License, or (at your option) any later version. 24 | 25 | NeoPixel is distributed in the hope that it will be useful, 26 | but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | GNU Lesser General Public License for more details. 29 | 30 | You should have received a copy of the GNU Lesser General Public 31 | License along with NeoPixel. If not, see 32 | . 33 | -------------------------------------------------------------------------*/ 34 | 35 | #include "Adafruit_NeoPixel.h" 36 | 37 | // Constructor when length, pin and type are known at compile-time: 38 | Adafruit_NeoPixel::Adafruit_NeoPixel(uint16_t n, uint8_t p, neoPixelType t) : 39 | begun(false), brightness(0), pixels(NULL), endTime(0) 40 | { 41 | updateType(t); 42 | updateLength(n); 43 | setPin(p); 44 | } 45 | 46 | // via Michael Vogt/neophob: empty constructor is used when strand length 47 | // isn't known at compile-time; situations where program config might be 48 | // read from internal flash memory or an SD card, or arrive via serial 49 | // command. If using this constructor, MUST follow up with updateType(), 50 | // updateLength(), etc. to establish the strand type, length and pin number! 51 | Adafruit_NeoPixel::Adafruit_NeoPixel() : 52 | #ifdef NEO_KHZ400 53 | is800KHz(true), 54 | #endif 55 | begun(false), numLEDs(0), numBytes(0), pin(-1), brightness(0), pixels(NULL), 56 | rOffset(1), gOffset(0), bOffset(2), wOffset(1), endTime(0) 57 | { 58 | } 59 | 60 | Adafruit_NeoPixel::~Adafruit_NeoPixel() { 61 | if(pixels) free(pixels); 62 | if(pin >= 0) pinMode(pin, INPUT); 63 | } 64 | 65 | void Adafruit_NeoPixel::begin(void) { 66 | if(pin >= 0) { 67 | pinMode(pin, OUTPUT); 68 | digitalWrite(pin, LOW); 69 | } 70 | begun = true; 71 | } 72 | 73 | void Adafruit_NeoPixel::updateLength(uint16_t n) { 74 | if(pixels) free(pixels); // Free existing data (if any) 75 | 76 | // Allocate new data -- note: ALL PIXELS ARE CLEARED 77 | numBytes = n * ((wOffset == rOffset) ? 3 : 4); 78 | if((pixels = (uint8_t *)malloc(numBytes))) { 79 | memset(pixels, 0, numBytes); 80 | numLEDs = n; 81 | } else { 82 | numLEDs = numBytes = 0; 83 | } 84 | } 85 | 86 | void Adafruit_NeoPixel::updateType(neoPixelType t) { 87 | boolean oldThreeBytesPerPixel = (wOffset == rOffset); // false if RGBW 88 | 89 | wOffset = (t >> 6) & 0b11; // See notes in header file 90 | rOffset = (t >> 4) & 0b11; // regarding R/G/B/W offsets 91 | gOffset = (t >> 2) & 0b11; 92 | bOffset = t & 0b11; 93 | #ifdef NEO_KHZ400 94 | is800KHz = (t < 256); // 400 KHz flag is 1<<8 95 | #endif 96 | 97 | // If bytes-per-pixel has changed (and pixel data was previously 98 | // allocated), re-allocate to new size. Will clear any data. 99 | if(pixels) { 100 | boolean newThreeBytesPerPixel = (wOffset == rOffset); 101 | if(newThreeBytesPerPixel != oldThreeBytesPerPixel) updateLength(numLEDs); 102 | } 103 | } 104 | 105 | #ifdef ESP8266 106 | // ESP8266 show() is external to enforce ICACHE_RAM_ATTR execution 107 | extern "C" void ICACHE_RAM_ATTR espShow( 108 | uint8_t pin, uint8_t *pixels, uint32_t numBytes, uint8_t type); 109 | #endif // ESP8266 110 | 111 | void Adafruit_NeoPixel::show(void) { 112 | 113 | if(!pixels) return; 114 | 115 | // Data latch = 50+ microsecond pause in the output stream. Rather than 116 | // put a delay at the end of the function, the ending time is noted and 117 | // the function will simply hold off (if needed) on issuing the 118 | // subsequent round of data until the latch time has elapsed. This 119 | // allows the mainline code to start generating the next frame of data 120 | // rather than stalling for the latch. 121 | while(!canShow()); 122 | // endTime is a private member (rather than global var) so that mutliple 123 | // instances on different pins can be quickly issued in succession (each 124 | // instance doesn't delay the next). 125 | 126 | // In order to make this code runtime-configurable to work with any pin, 127 | // SBI/CBI instructions are eschewed in favor of full PORT writes via the 128 | // OUT or ST instructions. It relies on two facts: that peripheral 129 | // functions (such as PWM) take precedence on output pins, so our PORT- 130 | // wide writes won't interfere, and that interrupts are globally disabled 131 | // while data is being issued to the LEDs, so no other code will be 132 | // accessing the PORT. The code takes an initial 'snapshot' of the PORT 133 | // state, computes 'pin high' and 'pin low' values, and writes these back 134 | // to the PORT register as needed. 135 | 136 | noInterrupts(); // Need 100% focus on instruction timing 137 | 138 | 139 | #ifdef __AVR__ 140 | // AVR MCUs -- ATmega & ATtiny (no XMEGA) --------------------------------- 141 | 142 | volatile uint16_t 143 | i = numBytes; // Loop counter 144 | volatile uint8_t 145 | *ptr = pixels, // Pointer to next byte 146 | b = *ptr++, // Current byte value 147 | hi, // PORT w/output bit set high 148 | lo; // PORT w/output bit set low 149 | 150 | // Hand-tuned assembly code issues data to the LED drivers at a specific 151 | // rate. There's separate code for different CPU speeds (8, 12, 16 MHz) 152 | // for both the WS2811 (400 KHz) and WS2812 (800 KHz) drivers. The 153 | // datastream timing for the LED drivers allows a little wiggle room each 154 | // way (listed in the datasheets), so the conditions for compiling each 155 | // case are set up for a range of frequencies rather than just the exact 156 | // 8, 12 or 16 MHz values, permitting use with some close-but-not-spot-on 157 | // devices (e.g. 16.5 MHz DigiSpark). The ranges were arrived at based 158 | // on the datasheet figures and have not been extensively tested outside 159 | // the canonical 8/12/16 MHz speeds; there's no guarantee these will work 160 | // close to the extremes (or possibly they could be pushed further). 161 | // Keep in mind only one CPU speed case actually gets compiled; the 162 | // resulting program isn't as massive as it might look from source here. 163 | 164 | // 8 MHz(ish) AVR --------------------------------------------------------- 165 | #if (F_CPU >= 7400000UL) && (F_CPU <= 9500000UL) 166 | 167 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 168 | if(is800KHz) { 169 | #endif 170 | 171 | volatile uint8_t n1, n2 = 0; // First, next bits out 172 | 173 | // Squeezing an 800 KHz stream out of an 8 MHz chip requires code 174 | // specific to each PORT register. At present this is only written 175 | // to work with pins on PORTD or PORTB, the most likely use case -- 176 | // this covers all the pins on the Adafruit Flora and the bulk of 177 | // digital pins on the Arduino Pro 8 MHz (keep in mind, this code 178 | // doesn't even get compiled for 16 MHz boards like the Uno, Mega, 179 | // Leonardo, etc., so don't bother extending this out of hand). 180 | // Additional PORTs could be added if you really need them, just 181 | // duplicate the else and loop and change the PORT. Each add'l 182 | // PORT will require about 150(ish) bytes of program space. 183 | 184 | // 10 instruction clocks per bit: HHxxxxxLLL 185 | // OUT instructions: ^ ^ ^ (T=0,2,7) 186 | 187 | #ifdef PORTD // PORTD isn't present on ATtiny85, etc. 188 | 189 | if(port == &PORTD) { 190 | 191 | hi = PORTD | pinMask; 192 | lo = PORTD & ~pinMask; 193 | n1 = lo; 194 | if(b & 0x80) n1 = hi; 195 | 196 | // Dirty trick: RJMPs proceeding to the next instruction are used 197 | // to delay two clock cycles in one instruction word (rather than 198 | // using two NOPs). This was necessary in order to squeeze the 199 | // loop down to exactly 64 words -- the maximum possible for a 200 | // relative branch. 201 | 202 | asm volatile( 203 | "headD:" "\n\t" // Clk Pseudocode 204 | // Bit 7: 205 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 206 | "mov %[n2] , %[lo]" "\n\t" // 1 n2 = lo 207 | "out %[port] , %[n1]" "\n\t" // 1 PORT = n1 208 | "rjmp .+0" "\n\t" // 2 nop nop 209 | "sbrc %[byte] , 6" "\n\t" // 1-2 if(b & 0x40) 210 | "mov %[n2] , %[hi]" "\n\t" // 0-1 n2 = hi 211 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 212 | "rjmp .+0" "\n\t" // 2 nop nop 213 | // Bit 6: 214 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 215 | "mov %[n1] , %[lo]" "\n\t" // 1 n1 = lo 216 | "out %[port] , %[n2]" "\n\t" // 1 PORT = n2 217 | "rjmp .+0" "\n\t" // 2 nop nop 218 | "sbrc %[byte] , 5" "\n\t" // 1-2 if(b & 0x20) 219 | "mov %[n1] , %[hi]" "\n\t" // 0-1 n1 = hi 220 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 221 | "rjmp .+0" "\n\t" // 2 nop nop 222 | // Bit 5: 223 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 224 | "mov %[n2] , %[lo]" "\n\t" // 1 n2 = lo 225 | "out %[port] , %[n1]" "\n\t" // 1 PORT = n1 226 | "rjmp .+0" "\n\t" // 2 nop nop 227 | "sbrc %[byte] , 4" "\n\t" // 1-2 if(b & 0x10) 228 | "mov %[n2] , %[hi]" "\n\t" // 0-1 n2 = hi 229 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 230 | "rjmp .+0" "\n\t" // 2 nop nop 231 | // Bit 4: 232 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 233 | "mov %[n1] , %[lo]" "\n\t" // 1 n1 = lo 234 | "out %[port] , %[n2]" "\n\t" // 1 PORT = n2 235 | "rjmp .+0" "\n\t" // 2 nop nop 236 | "sbrc %[byte] , 3" "\n\t" // 1-2 if(b & 0x08) 237 | "mov %[n1] , %[hi]" "\n\t" // 0-1 n1 = hi 238 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 239 | "rjmp .+0" "\n\t" // 2 nop nop 240 | // Bit 3: 241 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 242 | "mov %[n2] , %[lo]" "\n\t" // 1 n2 = lo 243 | "out %[port] , %[n1]" "\n\t" // 1 PORT = n1 244 | "rjmp .+0" "\n\t" // 2 nop nop 245 | "sbrc %[byte] , 2" "\n\t" // 1-2 if(b & 0x04) 246 | "mov %[n2] , %[hi]" "\n\t" // 0-1 n2 = hi 247 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 248 | "rjmp .+0" "\n\t" // 2 nop nop 249 | // Bit 2: 250 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 251 | "mov %[n1] , %[lo]" "\n\t" // 1 n1 = lo 252 | "out %[port] , %[n2]" "\n\t" // 1 PORT = n2 253 | "rjmp .+0" "\n\t" // 2 nop nop 254 | "sbrc %[byte] , 1" "\n\t" // 1-2 if(b & 0x02) 255 | "mov %[n1] , %[hi]" "\n\t" // 0-1 n1 = hi 256 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 257 | "rjmp .+0" "\n\t" // 2 nop nop 258 | // Bit 1: 259 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 260 | "mov %[n2] , %[lo]" "\n\t" // 1 n2 = lo 261 | "out %[port] , %[n1]" "\n\t" // 1 PORT = n1 262 | "rjmp .+0" "\n\t" // 2 nop nop 263 | "sbrc %[byte] , 0" "\n\t" // 1-2 if(b & 0x01) 264 | "mov %[n2] , %[hi]" "\n\t" // 0-1 n2 = hi 265 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 266 | "sbiw %[count], 1" "\n\t" // 2 i-- (don't act on Z flag yet) 267 | // Bit 0: 268 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi 269 | "mov %[n1] , %[lo]" "\n\t" // 1 n1 = lo 270 | "out %[port] , %[n2]" "\n\t" // 1 PORT = n2 271 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ 272 | "sbrc %[byte] , 7" "\n\t" // 1-2 if(b & 0x80) 273 | "mov %[n1] , %[hi]" "\n\t" // 0-1 n1 = hi 274 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo 275 | "brne headD" "\n" // 2 while(i) (Z flag set above) 276 | : [byte] "+r" (b), 277 | [n1] "+r" (n1), 278 | [n2] "+r" (n2), 279 | [count] "+w" (i) 280 | : [port] "I" (_SFR_IO_ADDR(PORTD)), 281 | [ptr] "e" (ptr), 282 | [hi] "r" (hi), 283 | [lo] "r" (lo)); 284 | 285 | } else if(port == &PORTB) { 286 | 287 | #endif // PORTD 288 | 289 | // Same as above, just switched to PORTB and stripped of comments. 290 | hi = PORTB | pinMask; 291 | lo = PORTB & ~pinMask; 292 | n1 = lo; 293 | if(b & 0x80) n1 = hi; 294 | 295 | asm volatile( 296 | "headB:" "\n\t" 297 | "out %[port] , %[hi]" "\n\t" 298 | "mov %[n2] , %[lo]" "\n\t" 299 | "out %[port] , %[n1]" "\n\t" 300 | "rjmp .+0" "\n\t" 301 | "sbrc %[byte] , 6" "\n\t" 302 | "mov %[n2] , %[hi]" "\n\t" 303 | "out %[port] , %[lo]" "\n\t" 304 | "rjmp .+0" "\n\t" 305 | "out %[port] , %[hi]" "\n\t" 306 | "mov %[n1] , %[lo]" "\n\t" 307 | "out %[port] , %[n2]" "\n\t" 308 | "rjmp .+0" "\n\t" 309 | "sbrc %[byte] , 5" "\n\t" 310 | "mov %[n1] , %[hi]" "\n\t" 311 | "out %[port] , %[lo]" "\n\t" 312 | "rjmp .+0" "\n\t" 313 | "out %[port] , %[hi]" "\n\t" 314 | "mov %[n2] , %[lo]" "\n\t" 315 | "out %[port] , %[n1]" "\n\t" 316 | "rjmp .+0" "\n\t" 317 | "sbrc %[byte] , 4" "\n\t" 318 | "mov %[n2] , %[hi]" "\n\t" 319 | "out %[port] , %[lo]" "\n\t" 320 | "rjmp .+0" "\n\t" 321 | "out %[port] , %[hi]" "\n\t" 322 | "mov %[n1] , %[lo]" "\n\t" 323 | "out %[port] , %[n2]" "\n\t" 324 | "rjmp .+0" "\n\t" 325 | "sbrc %[byte] , 3" "\n\t" 326 | "mov %[n1] , %[hi]" "\n\t" 327 | "out %[port] , %[lo]" "\n\t" 328 | "rjmp .+0" "\n\t" 329 | "out %[port] , %[hi]" "\n\t" 330 | "mov %[n2] , %[lo]" "\n\t" 331 | "out %[port] , %[n1]" "\n\t" 332 | "rjmp .+0" "\n\t" 333 | "sbrc %[byte] , 2" "\n\t" 334 | "mov %[n2] , %[hi]" "\n\t" 335 | "out %[port] , %[lo]" "\n\t" 336 | "rjmp .+0" "\n\t" 337 | "out %[port] , %[hi]" "\n\t" 338 | "mov %[n1] , %[lo]" "\n\t" 339 | "out %[port] , %[n2]" "\n\t" 340 | "rjmp .+0" "\n\t" 341 | "sbrc %[byte] , 1" "\n\t" 342 | "mov %[n1] , %[hi]" "\n\t" 343 | "out %[port] , %[lo]" "\n\t" 344 | "rjmp .+0" "\n\t" 345 | "out %[port] , %[hi]" "\n\t" 346 | "mov %[n2] , %[lo]" "\n\t" 347 | "out %[port] , %[n1]" "\n\t" 348 | "rjmp .+0" "\n\t" 349 | "sbrc %[byte] , 0" "\n\t" 350 | "mov %[n2] , %[hi]" "\n\t" 351 | "out %[port] , %[lo]" "\n\t" 352 | "sbiw %[count], 1" "\n\t" 353 | "out %[port] , %[hi]" "\n\t" 354 | "mov %[n1] , %[lo]" "\n\t" 355 | "out %[port] , %[n2]" "\n\t" 356 | "ld %[byte] , %a[ptr]+" "\n\t" 357 | "sbrc %[byte] , 7" "\n\t" 358 | "mov %[n1] , %[hi]" "\n\t" 359 | "out %[port] , %[lo]" "\n\t" 360 | "brne headB" "\n" 361 | : [byte] "+r" (b), [n1] "+r" (n1), [n2] "+r" (n2), [count] "+w" (i) 362 | : [port] "I" (_SFR_IO_ADDR(PORTB)), [ptr] "e" (ptr), [hi] "r" (hi), 363 | [lo] "r" (lo)); 364 | 365 | #ifdef PORTD 366 | } // endif PORTB 367 | #endif 368 | 369 | #ifdef NEO_KHZ400 370 | } else { // end 800 KHz, do 400 KHz 371 | 372 | // Timing is more relaxed; unrolling the inner loop for each bit is 373 | // not necessary. Still using the peculiar RJMPs as 2X NOPs, not out 374 | // of need but just to trim the code size down a little. 375 | // This 400-KHz-datastream-on-8-MHz-CPU code is not quite identical 376 | // to the 800-on-16 code later -- the hi/lo timing between WS2811 and 377 | // WS2812 is not simply a 2:1 scale! 378 | 379 | // 20 inst. clocks per bit: HHHHxxxxxxLLLLLLLLLL 380 | // ST instructions: ^ ^ ^ (T=0,4,10) 381 | 382 | volatile uint8_t next, bit; 383 | 384 | hi = *port | pinMask; 385 | lo = *port & ~pinMask; 386 | next = lo; 387 | bit = 8; 388 | 389 | asm volatile( 390 | "head20:" "\n\t" // Clk Pseudocode (T = 0) 391 | "st %a[port], %[hi]" "\n\t" // 2 PORT = hi (T = 2) 392 | "sbrc %[byte] , 7" "\n\t" // 1-2 if(b & 128) 393 | "mov %[next], %[hi]" "\n\t" // 0-1 next = hi (T = 4) 394 | "st %a[port], %[next]" "\n\t" // 2 PORT = next (T = 6) 395 | "mov %[next] , %[lo]" "\n\t" // 1 next = lo (T = 7) 396 | "dec %[bit]" "\n\t" // 1 bit-- (T = 8) 397 | "breq nextbyte20" "\n\t" // 1-2 if(bit == 0) 398 | "rol %[byte]" "\n\t" // 1 b <<= 1 (T = 10) 399 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 12) 400 | "rjmp .+0" "\n\t" // 2 nop nop (T = 14) 401 | "rjmp .+0" "\n\t" // 2 nop nop (T = 16) 402 | "rjmp .+0" "\n\t" // 2 nop nop (T = 18) 403 | "rjmp head20" "\n\t" // 2 -> head20 (next bit out) 404 | "nextbyte20:" "\n\t" // (T = 10) 405 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 12) 406 | "nop" "\n\t" // 1 nop (T = 13) 407 | "ldi %[bit] , 8" "\n\t" // 1 bit = 8 (T = 14) 408 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ (T = 16) 409 | "sbiw %[count], 1" "\n\t" // 2 i-- (T = 18) 410 | "brne head20" "\n" // 2 if(i != 0) -> (next byte) 411 | : [port] "+e" (port), 412 | [byte] "+r" (b), 413 | [bit] "+r" (bit), 414 | [next] "+r" (next), 415 | [count] "+w" (i) 416 | : [hi] "r" (hi), 417 | [lo] "r" (lo), 418 | [ptr] "e" (ptr)); 419 | } 420 | #endif // NEO_KHZ400 421 | 422 | // 12 MHz(ish) AVR -------------------------------------------------------- 423 | #elif (F_CPU >= 11100000UL) && (F_CPU <= 14300000UL) 424 | 425 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 426 | if(is800KHz) { 427 | #endif 428 | 429 | // In the 12 MHz case, an optimized 800 KHz datastream (no dead time 430 | // between bytes) requires a PORT-specific loop similar to the 8 MHz 431 | // code (but a little more relaxed in this case). 432 | 433 | // 15 instruction clocks per bit: HHHHxxxxxxLLLLL 434 | // OUT instructions: ^ ^ ^ (T=0,4,10) 435 | 436 | volatile uint8_t next; 437 | 438 | #ifdef PORTD 439 | 440 | if(port == &PORTD) { 441 | 442 | hi = PORTD | pinMask; 443 | lo = PORTD & ~pinMask; 444 | next = lo; 445 | if(b & 0x80) next = hi; 446 | 447 | // Don't "optimize" the OUT calls into the bitTime subroutine; 448 | // we're exploiting the RCALL and RET as 3- and 4-cycle NOPs! 449 | asm volatile( 450 | "headD:" "\n\t" // (T = 0) 451 | "out %[port], %[hi]" "\n\t" // (T = 1) 452 | "rcall bitTimeD" "\n\t" // Bit 7 (T = 15) 453 | "out %[port], %[hi]" "\n\t" 454 | "rcall bitTimeD" "\n\t" // Bit 6 455 | "out %[port], %[hi]" "\n\t" 456 | "rcall bitTimeD" "\n\t" // Bit 5 457 | "out %[port], %[hi]" "\n\t" 458 | "rcall bitTimeD" "\n\t" // Bit 4 459 | "out %[port], %[hi]" "\n\t" 460 | "rcall bitTimeD" "\n\t" // Bit 3 461 | "out %[port], %[hi]" "\n\t" 462 | "rcall bitTimeD" "\n\t" // Bit 2 463 | "out %[port], %[hi]" "\n\t" 464 | "rcall bitTimeD" "\n\t" // Bit 1 465 | // Bit 0: 466 | "out %[port] , %[hi]" "\n\t" // 1 PORT = hi (T = 1) 467 | "rjmp .+0" "\n\t" // 2 nop nop (T = 3) 468 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ (T = 5) 469 | "out %[port] , %[next]" "\n\t" // 1 PORT = next (T = 6) 470 | "mov %[next] , %[lo]" "\n\t" // 1 next = lo (T = 7) 471 | "sbrc %[byte] , 7" "\n\t" // 1-2 if(b & 0x80) (T = 8) 472 | "mov %[next] , %[hi]" "\n\t" // 0-1 next = hi (T = 9) 473 | "nop" "\n\t" // 1 (T = 10) 474 | "out %[port] , %[lo]" "\n\t" // 1 PORT = lo (T = 11) 475 | "sbiw %[count], 1" "\n\t" // 2 i-- (T = 13) 476 | "brne headD" "\n\t" // 2 if(i != 0) -> (next byte) 477 | "rjmp doneD" "\n\t" 478 | "bitTimeD:" "\n\t" // nop nop nop (T = 4) 479 | "out %[port], %[next]" "\n\t" // 1 PORT = next (T = 5) 480 | "mov %[next], %[lo]" "\n\t" // 1 next = lo (T = 6) 481 | "rol %[byte]" "\n\t" // 1 b <<= 1 (T = 7) 482 | "sbrc %[byte], 7" "\n\t" // 1-2 if(b & 0x80) (T = 8) 483 | "mov %[next], %[hi]" "\n\t" // 0-1 next = hi (T = 9) 484 | "nop" "\n\t" // 1 (T = 10) 485 | "out %[port], %[lo]" "\n\t" // 1 PORT = lo (T = 11) 486 | "ret" "\n\t" // 4 nop nop nop nop (T = 15) 487 | "doneD:" "\n" 488 | : [byte] "+r" (b), 489 | [next] "+r" (next), 490 | [count] "+w" (i) 491 | : [port] "I" (_SFR_IO_ADDR(PORTD)), 492 | [ptr] "e" (ptr), 493 | [hi] "r" (hi), 494 | [lo] "r" (lo)); 495 | 496 | } else if(port == &PORTB) { 497 | 498 | #endif // PORTD 499 | 500 | hi = PORTB | pinMask; 501 | lo = PORTB & ~pinMask; 502 | next = lo; 503 | if(b & 0x80) next = hi; 504 | 505 | // Same as above, just set for PORTB & stripped of comments 506 | asm volatile( 507 | "headB:" "\n\t" 508 | "out %[port], %[hi]" "\n\t" 509 | "rcall bitTimeB" "\n\t" 510 | "out %[port], %[hi]" "\n\t" 511 | "rcall bitTimeB" "\n\t" 512 | "out %[port], %[hi]" "\n\t" 513 | "rcall bitTimeB" "\n\t" 514 | "out %[port], %[hi]" "\n\t" 515 | "rcall bitTimeB" "\n\t" 516 | "out %[port], %[hi]" "\n\t" 517 | "rcall bitTimeB" "\n\t" 518 | "out %[port], %[hi]" "\n\t" 519 | "rcall bitTimeB" "\n\t" 520 | "out %[port], %[hi]" "\n\t" 521 | "rcall bitTimeB" "\n\t" 522 | "out %[port] , %[hi]" "\n\t" 523 | "rjmp .+0" "\n\t" 524 | "ld %[byte] , %a[ptr]+" "\n\t" 525 | "out %[port] , %[next]" "\n\t" 526 | "mov %[next] , %[lo]" "\n\t" 527 | "sbrc %[byte] , 7" "\n\t" 528 | "mov %[next] , %[hi]" "\n\t" 529 | "nop" "\n\t" 530 | "out %[port] , %[lo]" "\n\t" 531 | "sbiw %[count], 1" "\n\t" 532 | "brne headB" "\n\t" 533 | "rjmp doneB" "\n\t" 534 | "bitTimeB:" "\n\t" 535 | "out %[port], %[next]" "\n\t" 536 | "mov %[next], %[lo]" "\n\t" 537 | "rol %[byte]" "\n\t" 538 | "sbrc %[byte], 7" "\n\t" 539 | "mov %[next], %[hi]" "\n\t" 540 | "nop" "\n\t" 541 | "out %[port], %[lo]" "\n\t" 542 | "ret" "\n\t" 543 | "doneB:" "\n" 544 | : [byte] "+r" (b), [next] "+r" (next), [count] "+w" (i) 545 | : [port] "I" (_SFR_IO_ADDR(PORTB)), [ptr] "e" (ptr), [hi] "r" (hi), 546 | [lo] "r" (lo)); 547 | 548 | #ifdef PORTD 549 | } 550 | #endif 551 | 552 | #ifdef NEO_KHZ400 553 | } else { // 400 KHz 554 | 555 | // 30 instruction clocks per bit: HHHHHHxxxxxxxxxLLLLLLLLLLLLLLL 556 | // ST instructions: ^ ^ ^ (T=0,6,15) 557 | 558 | volatile uint8_t next, bit; 559 | 560 | hi = *port | pinMask; 561 | lo = *port & ~pinMask; 562 | next = lo; 563 | bit = 8; 564 | 565 | asm volatile( 566 | "head30:" "\n\t" // Clk Pseudocode (T = 0) 567 | "st %a[port], %[hi]" "\n\t" // 2 PORT = hi (T = 2) 568 | "sbrc %[byte] , 7" "\n\t" // 1-2 if(b & 128) 569 | "mov %[next], %[hi]" "\n\t" // 0-1 next = hi (T = 4) 570 | "rjmp .+0" "\n\t" // 2 nop nop (T = 6) 571 | "st %a[port], %[next]" "\n\t" // 2 PORT = next (T = 8) 572 | "rjmp .+0" "\n\t" // 2 nop nop (T = 10) 573 | "rjmp .+0" "\n\t" // 2 nop nop (T = 12) 574 | "rjmp .+0" "\n\t" // 2 nop nop (T = 14) 575 | "nop" "\n\t" // 1 nop (T = 15) 576 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 17) 577 | "rjmp .+0" "\n\t" // 2 nop nop (T = 19) 578 | "dec %[bit]" "\n\t" // 1 bit-- (T = 20) 579 | "breq nextbyte30" "\n\t" // 1-2 if(bit == 0) 580 | "rol %[byte]" "\n\t" // 1 b <<= 1 (T = 22) 581 | "rjmp .+0" "\n\t" // 2 nop nop (T = 24) 582 | "rjmp .+0" "\n\t" // 2 nop nop (T = 26) 583 | "rjmp .+0" "\n\t" // 2 nop nop (T = 28) 584 | "rjmp head30" "\n\t" // 2 -> head30 (next bit out) 585 | "nextbyte30:" "\n\t" // (T = 22) 586 | "nop" "\n\t" // 1 nop (T = 23) 587 | "ldi %[bit] , 8" "\n\t" // 1 bit = 8 (T = 24) 588 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ (T = 26) 589 | "sbiw %[count], 1" "\n\t" // 2 i-- (T = 28) 590 | "brne head30" "\n" // 1-2 if(i != 0) -> (next byte) 591 | : [port] "+e" (port), 592 | [byte] "+r" (b), 593 | [bit] "+r" (bit), 594 | [next] "+r" (next), 595 | [count] "+w" (i) 596 | : [hi] "r" (hi), 597 | [lo] "r" (lo), 598 | [ptr] "e" (ptr)); 599 | } 600 | #endif // NEO_KHZ400 601 | 602 | // 16 MHz(ish) AVR -------------------------------------------------------- 603 | #elif (F_CPU >= 15400000UL) && (F_CPU <= 19000000L) 604 | 605 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 606 | if(is800KHz) { 607 | #endif 608 | 609 | // WS2811 and WS2812 have different hi/lo duty cycles; this is 610 | // similar but NOT an exact copy of the prior 400-on-8 code. 611 | 612 | // 20 inst. clocks per bit: HHHHHxxxxxxxxLLLLLLL 613 | // ST instructions: ^ ^ ^ (T=0,5,13) 614 | 615 | volatile uint8_t next, bit; 616 | 617 | hi = *port | pinMask; 618 | lo = *port & ~pinMask; 619 | next = lo; 620 | bit = 8; 621 | 622 | asm volatile( 623 | "head20:" "\n\t" // Clk Pseudocode (T = 0) 624 | "st %a[port], %[hi]" "\n\t" // 2 PORT = hi (T = 2) 625 | "sbrc %[byte], 7" "\n\t" // 1-2 if(b & 128) 626 | "mov %[next], %[hi]" "\n\t" // 0-1 next = hi (T = 4) 627 | "dec %[bit]" "\n\t" // 1 bit-- (T = 5) 628 | "st %a[port], %[next]" "\n\t" // 2 PORT = next (T = 7) 629 | "mov %[next] , %[lo]" "\n\t" // 1 next = lo (T = 8) 630 | "breq nextbyte20" "\n\t" // 1-2 if(bit == 0) (from dec above) 631 | "rol %[byte]" "\n\t" // 1 b <<= 1 (T = 10) 632 | "rjmp .+0" "\n\t" // 2 nop nop (T = 12) 633 | "nop" "\n\t" // 1 nop (T = 13) 634 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 15) 635 | "nop" "\n\t" // 1 nop (T = 16) 636 | "rjmp .+0" "\n\t" // 2 nop nop (T = 18) 637 | "rjmp head20" "\n\t" // 2 -> head20 (next bit out) 638 | "nextbyte20:" "\n\t" // (T = 10) 639 | "ldi %[bit] , 8" "\n\t" // 1 bit = 8 (T = 11) 640 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ (T = 13) 641 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 15) 642 | "nop" "\n\t" // 1 nop (T = 16) 643 | "sbiw %[count], 1" "\n\t" // 2 i-- (T = 18) 644 | "brne head20" "\n" // 2 if(i != 0) -> (next byte) 645 | : [port] "+e" (port), 646 | [byte] "+r" (b), 647 | [bit] "+r" (bit), 648 | [next] "+r" (next), 649 | [count] "+w" (i) 650 | : [ptr] "e" (ptr), 651 | [hi] "r" (hi), 652 | [lo] "r" (lo)); 653 | 654 | #ifdef NEO_KHZ400 655 | } else { // 400 KHz 656 | 657 | // The 400 KHz clock on 16 MHz MCU is the most 'relaxed' version. 658 | 659 | // 40 inst. clocks per bit: HHHHHHHHxxxxxxxxxxxxLLLLLLLLLLLLLLLLLLLL 660 | // ST instructions: ^ ^ ^ (T=0,8,20) 661 | 662 | volatile uint8_t next, bit; 663 | 664 | hi = *port | pinMask; 665 | lo = *port & ~pinMask; 666 | next = lo; 667 | bit = 8; 668 | 669 | asm volatile( 670 | "head40:" "\n\t" // Clk Pseudocode (T = 0) 671 | "st %a[port], %[hi]" "\n\t" // 2 PORT = hi (T = 2) 672 | "sbrc %[byte] , 7" "\n\t" // 1-2 if(b & 128) 673 | "mov %[next] , %[hi]" "\n\t" // 0-1 next = hi (T = 4) 674 | "rjmp .+0" "\n\t" // 2 nop nop (T = 6) 675 | "rjmp .+0" "\n\t" // 2 nop nop (T = 8) 676 | "st %a[port], %[next]" "\n\t" // 2 PORT = next (T = 10) 677 | "rjmp .+0" "\n\t" // 2 nop nop (T = 12) 678 | "rjmp .+0" "\n\t" // 2 nop nop (T = 14) 679 | "rjmp .+0" "\n\t" // 2 nop nop (T = 16) 680 | "rjmp .+0" "\n\t" // 2 nop nop (T = 18) 681 | "rjmp .+0" "\n\t" // 2 nop nop (T = 20) 682 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 22) 683 | "nop" "\n\t" // 1 nop (T = 23) 684 | "mov %[next] , %[lo]" "\n\t" // 1 next = lo (T = 24) 685 | "dec %[bit]" "\n\t" // 1 bit-- (T = 25) 686 | "breq nextbyte40" "\n\t" // 1-2 if(bit == 0) 687 | "rol %[byte]" "\n\t" // 1 b <<= 1 (T = 27) 688 | "nop" "\n\t" // 1 nop (T = 28) 689 | "rjmp .+0" "\n\t" // 2 nop nop (T = 30) 690 | "rjmp .+0" "\n\t" // 2 nop nop (T = 32) 691 | "rjmp .+0" "\n\t" // 2 nop nop (T = 34) 692 | "rjmp .+0" "\n\t" // 2 nop nop (T = 36) 693 | "rjmp .+0" "\n\t" // 2 nop nop (T = 38) 694 | "rjmp head40" "\n\t" // 2 -> head40 (next bit out) 695 | "nextbyte40:" "\n\t" // (T = 27) 696 | "ldi %[bit] , 8" "\n\t" // 1 bit = 8 (T = 28) 697 | "ld %[byte] , %a[ptr]+" "\n\t" // 2 b = *ptr++ (T = 30) 698 | "rjmp .+0" "\n\t" // 2 nop nop (T = 32) 699 | "st %a[port], %[lo]" "\n\t" // 2 PORT = lo (T = 34) 700 | "rjmp .+0" "\n\t" // 2 nop nop (T = 36) 701 | "sbiw %[count], 1" "\n\t" // 2 i-- (T = 38) 702 | "brne head40" "\n" // 1-2 if(i != 0) -> (next byte) 703 | : [port] "+e" (port), 704 | [byte] "+r" (b), 705 | [bit] "+r" (bit), 706 | [next] "+r" (next), 707 | [count] "+w" (i) 708 | : [ptr] "e" (ptr), 709 | [hi] "r" (hi), 710 | [lo] "r" (lo)); 711 | } 712 | #endif // NEO_KHZ400 713 | 714 | #else 715 | #error "CPU SPEED NOT SUPPORTED" 716 | #endif // end F_CPU ifdefs on __AVR__ 717 | 718 | // END AVR ---------------------------------------------------------------- 719 | 720 | 721 | #elif defined(__arm__) 722 | 723 | // ARM MCUs -- Teensy 3.0, 3.1, LC, Arduino Due --------------------------- 724 | 725 | #if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 & 3.1 726 | #define CYCLES_800_T0H (F_CPU / 4000000) 727 | #define CYCLES_800_T1H (F_CPU / 1250000) 728 | #define CYCLES_800 (F_CPU / 800000) 729 | #define CYCLES_400_T0H (F_CPU / 2000000) 730 | #define CYCLES_400_T1H (F_CPU / 833333) 731 | #define CYCLES_400 (F_CPU / 400000) 732 | 733 | uint8_t *p = pixels, 734 | *end = p + numBytes, pix, mask; 735 | volatile uint8_t *set = portSetRegister(pin), 736 | *clr = portClearRegister(pin); 737 | uint32_t cyc; 738 | 739 | ARM_DEMCR |= ARM_DEMCR_TRCENA; 740 | ARM_DWT_CTRL |= ARM_DWT_CTRL_CYCCNTENA; 741 | 742 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 743 | if(is800KHz) { 744 | #endif 745 | cyc = ARM_DWT_CYCCNT + CYCLES_800; 746 | while(p < end) { 747 | pix = *p++; 748 | for(mask = 0x80; mask; mask >>= 1) { 749 | while(ARM_DWT_CYCCNT - cyc < CYCLES_800); 750 | cyc = ARM_DWT_CYCCNT; 751 | *set = 1; 752 | if(pix & mask) { 753 | while(ARM_DWT_CYCCNT - cyc < CYCLES_800_T1H); 754 | } else { 755 | while(ARM_DWT_CYCCNT - cyc < CYCLES_800_T0H); 756 | } 757 | *clr = 1; 758 | } 759 | } 760 | while(ARM_DWT_CYCCNT - cyc < CYCLES_800); 761 | #ifdef NEO_KHZ400 762 | } else { // 400 kHz bitstream 763 | cyc = ARM_DWT_CYCCNT + CYCLES_400; 764 | while(p < end) { 765 | pix = *p++; 766 | for(mask = 0x80; mask; mask >>= 1) { 767 | while(ARM_DWT_CYCCNT - cyc < CYCLES_400); 768 | cyc = ARM_DWT_CYCCNT; 769 | *set = 1; 770 | if(pix & mask) { 771 | while(ARM_DWT_CYCCNT - cyc < CYCLES_400_T1H); 772 | } else { 773 | while(ARM_DWT_CYCCNT - cyc < CYCLES_400_T0H); 774 | } 775 | *clr = 1; 776 | } 777 | } 778 | while(ARM_DWT_CYCCNT - cyc < CYCLES_400); 779 | } 780 | #endif // NEO_KHZ400 781 | 782 | #elif defined(__MKL26Z64__) // Teensy-LC 783 | 784 | #if F_CPU == 48000000 785 | uint8_t *p = pixels, 786 | pix, count, dly, 787 | bitmask = digitalPinToBitMask(pin); 788 | volatile uint8_t *reg = portSetRegister(pin); 789 | uint32_t num = numBytes; 790 | asm volatile( 791 | "L%=_begin:" "\n\t" 792 | "ldrb %[pix], [%[p], #0]" "\n\t" 793 | "lsl %[pix], #24" "\n\t" 794 | "movs %[count], #7" "\n\t" 795 | "L%=_loop:" "\n\t" 796 | "lsl %[pix], #1" "\n\t" 797 | "bcs L%=_loop_one" "\n\t" 798 | "L%=_loop_zero:" 799 | "strb %[bitmask], [%[reg], #0]" "\n\t" 800 | "movs %[dly], #4" "\n\t" 801 | "L%=_loop_delay_T0H:" "\n\t" 802 | "sub %[dly], #1" "\n\t" 803 | "bne L%=_loop_delay_T0H" "\n\t" 804 | "strb %[bitmask], [%[reg], #4]" "\n\t" 805 | "movs %[dly], #13" "\n\t" 806 | "L%=_loop_delay_T0L:" "\n\t" 807 | "sub %[dly], #1" "\n\t" 808 | "bne L%=_loop_delay_T0L" "\n\t" 809 | "b L%=_next" "\n\t" 810 | "L%=_loop_one:" 811 | "strb %[bitmask], [%[reg], #0]" "\n\t" 812 | "movs %[dly], #13" "\n\t" 813 | "L%=_loop_delay_T1H:" "\n\t" 814 | "sub %[dly], #1" "\n\t" 815 | "bne L%=_loop_delay_T1H" "\n\t" 816 | "strb %[bitmask], [%[reg], #4]" "\n\t" 817 | "movs %[dly], #4" "\n\t" 818 | "L%=_loop_delay_T1L:" "\n\t" 819 | "sub %[dly], #1" "\n\t" 820 | "bne L%=_loop_delay_T1L" "\n\t" 821 | "nop" "\n\t" 822 | "L%=_next:" "\n\t" 823 | "sub %[count], #1" "\n\t" 824 | "bne L%=_loop" "\n\t" 825 | "lsl %[pix], #1" "\n\t" 826 | "bcs L%=_last_one" "\n\t" 827 | "L%=_last_zero:" 828 | "strb %[bitmask], [%[reg], #0]" "\n\t" 829 | "movs %[dly], #4" "\n\t" 830 | "L%=_last_delay_T0H:" "\n\t" 831 | "sub %[dly], #1" "\n\t" 832 | "bne L%=_last_delay_T0H" "\n\t" 833 | "strb %[bitmask], [%[reg], #4]" "\n\t" 834 | "movs %[dly], #10" "\n\t" 835 | "L%=_last_delay_T0L:" "\n\t" 836 | "sub %[dly], #1" "\n\t" 837 | "bne L%=_last_delay_T0L" "\n\t" 838 | "b L%=_repeat" "\n\t" 839 | "L%=_last_one:" 840 | "strb %[bitmask], [%[reg], #0]" "\n\t" 841 | "movs %[dly], #13" "\n\t" 842 | "L%=_last_delay_T1H:" "\n\t" 843 | "sub %[dly], #1" "\n\t" 844 | "bne L%=_last_delay_T1H" "\n\t" 845 | "strb %[bitmask], [%[reg], #4]" "\n\t" 846 | "movs %[dly], #1" "\n\t" 847 | "L%=_last_delay_T1L:" "\n\t" 848 | "sub %[dly], #1" "\n\t" 849 | "bne L%=_last_delay_T1L" "\n\t" 850 | "nop" "\n\t" 851 | "L%=_repeat:" "\n\t" 852 | "add %[p], #1" "\n\t" 853 | "sub %[num], #1" "\n\t" 854 | "bne L%=_begin" "\n\t" 855 | "L%=_done:" "\n\t" 856 | : [p] "+r" (p), 857 | [pix] "=&r" (pix), 858 | [count] "=&r" (count), 859 | [dly] "=&r" (dly), 860 | [num] "+r" (num) 861 | : [bitmask] "r" (bitmask), 862 | [reg] "r" (reg) 863 | ); 864 | #else 865 | #error "Sorry, only 48 MHz is supported, please set Tools > CPU Speed to 48 MHz" 866 | #endif // F_CPU == 48000000 867 | 868 | #elif defined(__SAMD21G18A__) // Arduino Zero 869 | 870 | // Tried this with a timer/counter, couldn't quite get adequate 871 | // resolution. So yay, you get a load of goofball NOPs... 872 | 873 | uint8_t *ptr, *end, p, bitMask, portNum; 874 | uint32_t pinMask; 875 | 876 | portNum = g_APinDescription[pin].ulPort; 877 | pinMask = 1ul << g_APinDescription[pin].ulPin; 878 | ptr = pixels; 879 | end = ptr + numBytes; 880 | p = *ptr++; 881 | bitMask = 0x80; 882 | 883 | volatile uint32_t *set = &(PORT->Group[portNum].OUTSET.reg), 884 | *clr = &(PORT->Group[portNum].OUTCLR.reg); 885 | 886 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 887 | if(is800KHz) { 888 | #endif 889 | for(;;) { 890 | *set = pinMask; 891 | asm("nop; nop; nop; nop; nop; nop; nop; nop;"); 892 | if(p & bitMask) { 893 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 894 | "nop; nop; nop; nop; nop; nop; nop; nop;" 895 | "nop; nop; nop; nop;"); 896 | *clr = pinMask; 897 | } else { 898 | *clr = pinMask; 899 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 900 | "nop; nop; nop; nop; nop; nop; nop; nop;" 901 | "nop; nop; nop; nop;"); 902 | } 903 | if(bitMask >>= 1) { 904 | asm("nop; nop; nop; nop; nop; nop; nop; nop; nop;"); 905 | } else { 906 | if(ptr >= end) break; 907 | p = *ptr++; 908 | bitMask = 0x80; 909 | } 910 | } 911 | #ifdef NEO_KHZ400 912 | } else { // 400 KHz bitstream 913 | for(;;) { 914 | *set = pinMask; 915 | asm("nop; nop; nop; nop; nop; nop; nop; nop; nop; nop; nop;"); 916 | if(p & bitMask) { 917 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 918 | "nop; nop; nop; nop; nop; nop; nop; nop;" 919 | "nop; nop; nop; nop; nop; nop; nop; nop;" 920 | "nop; nop; nop;"); 921 | *clr = pinMask; 922 | } else { 923 | *clr = pinMask; 924 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 925 | "nop; nop; nop; nop; nop; nop; nop; nop;" 926 | "nop; nop; nop; nop; nop; nop; nop; nop;" 927 | "nop; nop; nop;"); 928 | } 929 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 930 | "nop; nop; nop; nop; nop; nop; nop; nop;" 931 | "nop; nop; nop; nop; nop; nop; nop; nop;" 932 | "nop; nop; nop; nop; nop; nop; nop; nop;"); 933 | if(bitMask >>= 1) { 934 | asm("nop; nop; nop; nop; nop; nop; nop;"); 935 | } else { 936 | if(ptr >= end) break; 937 | p = *ptr++; 938 | bitMask = 0x80; 939 | } 940 | } 941 | } 942 | #endif 943 | 944 | #elif defined (ARDUINO_STM32_FEATHER) // FEATHER WICED (120MHz) 945 | 946 | // Tried this with a timer/counter, couldn't quite get adequate 947 | // resolution. So yay, you get a load of goofball NOPs... 948 | 949 | uint8_t *ptr, *end, p, bitMask; 950 | uint32_t pinMask; 951 | 952 | pinMask = BIT(PIN_MAP[pin].gpio_bit); 953 | ptr = pixels; 954 | end = ptr + numBytes; 955 | p = *ptr++; 956 | bitMask = 0x80; 957 | 958 | volatile uint16_t *set = &(PIN_MAP[pin].gpio_device->regs->BSRRL); 959 | volatile uint16_t *clr = &(PIN_MAP[pin].gpio_device->regs->BSRRH); 960 | 961 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 962 | if(is800KHz) { 963 | #endif 964 | for(;;) { 965 | if(p & bitMask) { // ONE 966 | // High 800ns 967 | *set = pinMask; 968 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 969 | "nop; nop; nop; nop; nop; nop; nop; nop;" 970 | "nop; nop; nop; nop; nop; nop; nop; nop;" 971 | "nop; nop; nop; nop; nop; nop; nop; nop;" 972 | "nop; nop; nop; nop; nop; nop; nop; nop;" 973 | "nop; nop; nop; nop; nop; nop; nop; nop;" 974 | "nop; nop; nop; nop; nop; nop; nop; nop;" 975 | "nop; nop; nop; nop; nop; nop; nop; nop;" 976 | "nop; nop; nop; nop; nop; nop; nop; nop;" 977 | "nop; nop; nop; nop; nop; nop; nop; nop;" 978 | "nop; nop; nop; nop; nop; nop; nop; nop;" 979 | "nop; nop; nop; nop; nop; nop;"); 980 | // Low 450ns 981 | *clr = pinMask; 982 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 983 | "nop; nop; nop; nop; nop; nop; nop; nop;" 984 | "nop; nop; nop; nop; nop; nop; nop; nop;" 985 | "nop; nop; nop; nop; nop; nop; nop; nop;" 986 | "nop; nop; nop; nop; nop; nop;"); 987 | } else { // ZERO 988 | // High 400ns 989 | *set = pinMask; 990 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 991 | "nop; nop; nop; nop; nop; nop; nop; nop;" 992 | "nop; nop; nop; nop; nop; nop; nop; nop;" 993 | "nop; nop; nop; nop; nop; nop; nop; nop;" 994 | "nop; nop; nop; nop; nop; nop; nop; nop;" 995 | "nop;"); 996 | // Low 850ns 997 | *clr = pinMask; 998 | asm("nop; nop; nop; nop; nop; nop; nop; nop;" 999 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1000 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1001 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1002 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1003 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1004 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1005 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1006 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1007 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1008 | "nop; nop; nop; nop; nop; nop; nop; nop;" 1009 | "nop; nop; nop; nop;"); 1010 | } 1011 | if(bitMask >>= 1) { 1012 | // Move on to the next pixel 1013 | asm("nop;"); 1014 | } else { 1015 | if(ptr >= end) break; 1016 | p = *ptr++; 1017 | bitMask = 0x80; 1018 | } 1019 | } 1020 | #ifdef NEO_KHZ400 1021 | } else { // 400 KHz bitstream 1022 | // ToDo! 1023 | } 1024 | #endif 1025 | 1026 | #else // Other ARM architecture -- Presumed Arduino Due 1027 | 1028 | #define SCALE VARIANT_MCK / 2UL / 1000000UL 1029 | #define INST (2UL * F_CPU / VARIANT_MCK) 1030 | #define TIME_800_0 ((int)(0.40 * SCALE + 0.5) - (5 * INST)) 1031 | #define TIME_800_1 ((int)(0.80 * SCALE + 0.5) - (5 * INST)) 1032 | #define PERIOD_800 ((int)(1.25 * SCALE + 0.5) - (5 * INST)) 1033 | #define TIME_400_0 ((int)(0.50 * SCALE + 0.5) - (5 * INST)) 1034 | #define TIME_400_1 ((int)(1.20 * SCALE + 0.5) - (5 * INST)) 1035 | #define PERIOD_400 ((int)(2.50 * SCALE + 0.5) - (5 * INST)) 1036 | 1037 | int pinMask, time0, time1, period, t; 1038 | Pio *port; 1039 | volatile WoReg *portSet, *portClear, *timeValue, *timeReset; 1040 | uint8_t *p, *end, pix, mask; 1041 | 1042 | pmc_set_writeprotect(false); 1043 | pmc_enable_periph_clk((uint32_t)TC3_IRQn); 1044 | TC_Configure(TC1, 0, 1045 | TC_CMR_WAVE | TC_CMR_WAVSEL_UP | TC_CMR_TCCLKS_TIMER_CLOCK1); 1046 | TC_Start(TC1, 0); 1047 | 1048 | pinMask = g_APinDescription[pin].ulPin; // Don't 'optimize' these into 1049 | port = g_APinDescription[pin].pPort; // declarations above. Want to 1050 | portSet = &(port->PIO_SODR); // burn a few cycles after 1051 | portClear = &(port->PIO_CODR); // starting timer to minimize 1052 | timeValue = &(TC1->TC_CHANNEL[0].TC_CV); // the initial 'while'. 1053 | timeReset = &(TC1->TC_CHANNEL[0].TC_CCR); 1054 | p = pixels; 1055 | end = p + numBytes; 1056 | pix = *p++; 1057 | mask = 0x80; 1058 | 1059 | #ifdef NEO_KHZ400 // 800 KHz check needed only if 400 KHz support enabled 1060 | if(is800KHz) { 1061 | #endif 1062 | time0 = TIME_800_0; 1063 | time1 = TIME_800_1; 1064 | period = PERIOD_800; 1065 | #ifdef NEO_KHZ400 1066 | } else { // 400 KHz bitstream 1067 | time0 = TIME_400_0; 1068 | time1 = TIME_400_1; 1069 | period = PERIOD_400; 1070 | } 1071 | #endif 1072 | 1073 | for(t = time0;; t = time0) { 1074 | if(pix & mask) t = time1; 1075 | while(*timeValue < period); 1076 | *portSet = pinMask; 1077 | *timeReset = TC_CCR_CLKEN | TC_CCR_SWTRG; 1078 | while(*timeValue < t); 1079 | *portClear = pinMask; 1080 | if(!(mask >>= 1)) { // This 'inside-out' loop logic utilizes 1081 | if(p >= end) break; // idle time to minimize inter-byte delays. 1082 | pix = *p++; 1083 | mask = 0x80; 1084 | } 1085 | } 1086 | while(*timeValue < period); // Wait for last bit 1087 | TC_Stop(TC1, 0); 1088 | 1089 | #endif // end Due 1090 | 1091 | // END ARM ---------------------------------------------------------------- 1092 | 1093 | 1094 | #elif defined(ESP8266) 1095 | 1096 | // ESP8266 ---------------------------------------------------------------- 1097 | 1098 | // ESP8266 show() is external to enforce ICACHE_RAM_ATTR execution 1099 | espShow(pin, pixels, numBytes, is800KHz); 1100 | 1101 | #elif defined(__ARDUINO_ARC__) 1102 | 1103 | // Arduino 101 ----------------------------------------------------------- 1104 | 1105 | #define NOPx7 { __builtin_arc_nop(); \ 1106 | __builtin_arc_nop(); __builtin_arc_nop(); \ 1107 | __builtin_arc_nop(); __builtin_arc_nop(); \ 1108 | __builtin_arc_nop(); __builtin_arc_nop(); } 1109 | 1110 | PinDescription *pindesc = &g_APinDescription[pin]; 1111 | register uint32_t loop = 8 * numBytes; // one loop to handle all bytes and all bits 1112 | register uint8_t *p = pixels; 1113 | register uint32_t currByte = (uint32_t) (*p); 1114 | register uint32_t currBit = 0x80 & currByte; 1115 | register uint32_t bitCounter = 0; 1116 | register uint32_t first = 1; 1117 | 1118 | // The loop is unusual. Very first iteration puts all the way LOW to the wire - 1119 | // constant LOW does not affect NEOPIXEL, so there is no visible effect displayed. 1120 | // During that very first iteration CPU caches instructions in the loop. 1121 | // Because of the caching process, "CPU slows down". NEOPIXEL pulse is very time sensitive 1122 | // that's why we let the CPU cache first and we start regular pulse from 2nd iteration 1123 | if (pindesc->ulGPIOType == SS_GPIO) { 1124 | register uint32_t reg = pindesc->ulGPIOBase + SS_GPIO_SWPORTA_DR; 1125 | uint32_t reg_val = __builtin_arc_lr((volatile uint32_t)reg); 1126 | register uint32_t reg_bit_high = reg_val | (1 << pindesc->ulGPIOId); 1127 | register uint32_t reg_bit_low = reg_val & ~(1 << pindesc->ulGPIOId); 1128 | 1129 | loop += 1; // include first, special iteration 1130 | while(loop--) { 1131 | if(!first) { 1132 | currByte <<= 1; 1133 | bitCounter++; 1134 | } 1135 | 1136 | // 1 is >550ns high and >450ns low; 0 is 200..500ns high and >450ns low 1137 | __builtin_arc_sr(first ? reg_bit_low : reg_bit_high, (volatile uint32_t)reg); 1138 | if(currBit) { // ~400ns HIGH (740ns overall) 1139 | NOPx7 1140 | NOPx7 1141 | } 1142 | // ~340ns HIGH 1143 | NOPx7 1144 | __builtin_arc_nop(); 1145 | 1146 | // 820ns LOW; per spec, max allowed low here is 5000ns */ 1147 | __builtin_arc_sr(reg_bit_low, (volatile uint32_t)reg); 1148 | NOPx7 1149 | NOPx7 1150 | 1151 | if(bitCounter >= 8) { 1152 | bitCounter = 0; 1153 | currByte = (uint32_t) (*++p); 1154 | } 1155 | 1156 | currBit = 0x80 & currByte; 1157 | first = 0; 1158 | } 1159 | } else if(pindesc->ulGPIOType == SOC_GPIO) { 1160 | register uint32_t reg = pindesc->ulGPIOBase + SOC_GPIO_SWPORTA_DR; 1161 | uint32_t reg_val = MMIO_REG_VAL(reg); 1162 | register uint32_t reg_bit_high = reg_val | (1 << pindesc->ulGPIOId); 1163 | register uint32_t reg_bit_low = reg_val & ~(1 << pindesc->ulGPIOId); 1164 | 1165 | loop += 1; // include first, special iteration 1166 | while(loop--) { 1167 | if(!first) { 1168 | currByte <<= 1; 1169 | bitCounter++; 1170 | } 1171 | MMIO_REG_VAL(reg) = first ? reg_bit_low : reg_bit_high; 1172 | if(currBit) { // ~430ns HIGH (740ns overall) 1173 | NOPx7 1174 | NOPx7 1175 | __builtin_arc_nop(); 1176 | } 1177 | // ~310ns HIGH 1178 | NOPx7 1179 | 1180 | // 850ns LOW; per spec, max allowed low here is 5000ns */ 1181 | MMIO_REG_VAL(reg) = reg_bit_low; 1182 | NOPx7 1183 | NOPx7 1184 | 1185 | if(bitCounter >= 8) { 1186 | bitCounter = 0; 1187 | currByte = (uint32_t) (*++p); 1188 | } 1189 | 1190 | currBit = 0x80 & currByte; 1191 | first = 0; 1192 | } 1193 | } 1194 | 1195 | #endif 1196 | 1197 | 1198 | // END ARCHITECTURE SELECT ------------------------------------------------ 1199 | 1200 | 1201 | interrupts(); 1202 | endTime = micros(); // Save EOD time for latch on next call 1203 | } 1204 | 1205 | // Set the output pin number 1206 | void Adafruit_NeoPixel::setPin(uint8_t p) { 1207 | if(begun && (pin >= 0)) pinMode(pin, INPUT); 1208 | pin = p; 1209 | if(begun) { 1210 | pinMode(p, OUTPUT); 1211 | digitalWrite(p, LOW); 1212 | } 1213 | #ifdef __AVR__ 1214 | port = portOutputRegister(digitalPinToPort(p)); 1215 | pinMask = digitalPinToBitMask(p); 1216 | #endif 1217 | } 1218 | 1219 | // Set pixel color from separate R,G,B components: 1220 | void Adafruit_NeoPixel::setPixelColor( 1221 | uint16_t n, uint8_t r, uint8_t g, uint8_t b) { 1222 | 1223 | if(n < numLEDs) { 1224 | if(brightness) { // See notes in setBrightness() 1225 | r = (r * brightness) >> 8; 1226 | g = (g * brightness) >> 8; 1227 | b = (b * brightness) >> 8; 1228 | } 1229 | uint8_t *p; 1230 | if(wOffset == rOffset) { // Is an RGB-type strip 1231 | p = &pixels[n * 3]; // 3 bytes per pixel 1232 | } else { // Is a WRGB-type strip 1233 | p = &pixels[n * 4]; // 4 bytes per pixel 1234 | p[wOffset] = 0; // But only R,G,B passed -- set W to 0 1235 | } 1236 | p[rOffset] = r; // R,G,B always stored 1237 | p[gOffset] = g; 1238 | p[bOffset] = b; 1239 | } 1240 | } 1241 | 1242 | void Adafruit_NeoPixel::setPixelColor( 1243 | uint16_t n, uint8_t r, uint8_t g, uint8_t b, uint8_t w) { 1244 | 1245 | if(n < numLEDs) { 1246 | if(brightness) { // See notes in setBrightness() 1247 | r = (r * brightness) >> 8; 1248 | g = (g * brightness) >> 8; 1249 | b = (b * brightness) >> 8; 1250 | w = (w * brightness) >> 8; 1251 | } 1252 | uint8_t *p; 1253 | if(wOffset == rOffset) { // Is an RGB-type strip 1254 | p = &pixels[n * 3]; // 3 bytes per pixel (ignore W) 1255 | } else { // Is a WRGB-type strip 1256 | p = &pixels[n * 4]; // 4 bytes per pixel 1257 | p[wOffset] = w; // Store W 1258 | } 1259 | p[rOffset] = r; // Store R,G,B 1260 | p[gOffset] = g; 1261 | p[bOffset] = b; 1262 | } 1263 | } 1264 | 1265 | // Set pixel color from 'packed' 32-bit RGB color: 1266 | void Adafruit_NeoPixel::setPixelColor(uint16_t n, uint32_t c) { 1267 | if(n < numLEDs) { 1268 | uint8_t *p, 1269 | r = (uint8_t)(c >> 16), 1270 | g = (uint8_t)(c >> 8), 1271 | b = (uint8_t)c; 1272 | if(brightness) { // See notes in setBrightness() 1273 | r = (r * brightness) >> 8; 1274 | g = (g * brightness) >> 8; 1275 | b = (b * brightness) >> 8; 1276 | } 1277 | if(wOffset == rOffset) { 1278 | p = &pixels[n * 3]; 1279 | } else { 1280 | p = &pixels[n * 4]; 1281 | uint8_t w = (uint8_t)(c >> 24); 1282 | p[wOffset] = brightness ? ((w * brightness) >> 8) : w; 1283 | } 1284 | p[rOffset] = r; 1285 | p[gOffset] = g; 1286 | p[bOffset] = b; 1287 | } 1288 | } 1289 | 1290 | // Convert separate R,G,B into packed 32-bit RGB color. 1291 | // Packed format is always RGB, regardless of LED strand color order. 1292 | uint32_t Adafruit_NeoPixel::Color(uint8_t r, uint8_t g, uint8_t b) { 1293 | return ((uint32_t)r << 16) | ((uint32_t)g << 8) | b; 1294 | } 1295 | 1296 | // Convert separate R,G,B,W into packed 32-bit WRGB color. 1297 | // Packed format is always WRGB, regardless of LED strand color order. 1298 | uint32_t Adafruit_NeoPixel::Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) { 1299 | return ((uint32_t)w << 24) | ((uint32_t)r << 16) | ((uint32_t)g << 8) | b; 1300 | } 1301 | 1302 | // Query color from previously-set pixel (returns packed 32-bit RGB value) 1303 | uint32_t Adafruit_NeoPixel::getPixelColor(uint16_t n) const { 1304 | if(n >= numLEDs) return 0; // Out of bounds, return no color. 1305 | 1306 | uint8_t *p; 1307 | 1308 | if(wOffset == rOffset) { // Is RGB-type device 1309 | p = &pixels[n * 3]; 1310 | if(brightness) { 1311 | // Stored color was decimated by setBrightness(). Returned value 1312 | // attempts to scale back to an approximation of the original 24-bit 1313 | // value used when setting the pixel color, but there will always be 1314 | // some error -- those bits are simply gone. Issue is most 1315 | // pronounced at low brightness levels. 1316 | return (((uint32_t)(p[rOffset] << 8) / brightness) << 16) | 1317 | (((uint32_t)(p[gOffset] << 8) / brightness) << 8) | 1318 | ( (uint32_t)(p[bOffset] << 8) / brightness ); 1319 | } else { 1320 | // No brightness adjustment has been made -- return 'raw' color 1321 | return ((uint32_t)p[rOffset] << 16) | 1322 | ((uint32_t)p[gOffset] << 8) | 1323 | (uint32_t)p[bOffset]; 1324 | } 1325 | } else { // Is RGBW-type device 1326 | p = &pixels[n * 4]; 1327 | if(brightness) { // Return scaled color 1328 | return (((uint32_t)(p[wOffset] << 8) / brightness) << 24) | 1329 | (((uint32_t)(p[rOffset] << 8) / brightness) << 16) | 1330 | (((uint32_t)(p[gOffset] << 8) / brightness) << 8) | 1331 | ( (uint32_t)(p[bOffset] << 8) / brightness ); 1332 | } else { // Return raw color 1333 | return ((uint32_t)p[wOffset] << 24) | 1334 | ((uint32_t)p[rOffset] << 16) | 1335 | ((uint32_t)p[gOffset] << 8) | 1336 | (uint32_t)p[bOffset]; 1337 | } 1338 | } 1339 | } 1340 | 1341 | // Returns pointer to pixels[] array. Pixel data is stored in device- 1342 | // native format and is not translated here. Application will need to be 1343 | // aware of specific pixel data format and handle colors appropriately. 1344 | uint8_t *Adafruit_NeoPixel::getPixels(void) const { 1345 | return pixels; 1346 | } 1347 | 1348 | uint16_t Adafruit_NeoPixel::numPixels(void) const { 1349 | return numLEDs; 1350 | } 1351 | 1352 | // Adjust output brightness; 0=darkest (off), 255=brightest. This does 1353 | // NOT immediately affect what's currently displayed on the LEDs. The 1354 | // next call to show() will refresh the LEDs at this level. However, 1355 | // this process is potentially "lossy," especially when increasing 1356 | // brightness. The tight timing in the WS2811/WS2812 code means there 1357 | // aren't enough free cycles to perform this scaling on the fly as data 1358 | // is issued. So we make a pass through the existing color data in RAM 1359 | // and scale it (subsequent graphics commands also work at this 1360 | // brightness level). If there's a significant step up in brightness, 1361 | // the limited number of steps (quantization) in the old data will be 1362 | // quite visible in the re-scaled version. For a non-destructive 1363 | // change, you'll need to re-render the full strip data. C'est la vie. 1364 | void Adafruit_NeoPixel::setBrightness(uint8_t b) { 1365 | // Stored brightness value is different than what's passed. 1366 | // This simplifies the actual scaling math later, allowing a fast 1367 | // 8x8-bit multiply and taking the MSB. 'brightness' is a uint8_t, 1368 | // adding 1 here may (intentionally) roll over...so 0 = max brightness 1369 | // (color values are interpreted literally; no scaling), 1 = min 1370 | // brightness (off), 255 = just below max brightness. 1371 | uint8_t newBrightness = b + 1; 1372 | if(newBrightness != brightness) { // Compare against prior value 1373 | // Brightness has changed -- re-scale existing data in RAM 1374 | uint8_t c, 1375 | *ptr = pixels, 1376 | oldBrightness = brightness - 1; // De-wrap old brightness value 1377 | uint16_t scale; 1378 | if(oldBrightness == 0) scale = 0; // Avoid /0 1379 | else if(b == 255) scale = 65535 / oldBrightness; 1380 | else scale = (((uint16_t)newBrightness << 8) - 1) / oldBrightness; 1381 | for(uint16_t i=0; i> 8; 1384 | } 1385 | brightness = newBrightness; 1386 | } 1387 | } 1388 | 1389 | //Return the brightness value 1390 | uint8_t Adafruit_NeoPixel::getBrightness(void) const { 1391 | return brightness - 1; 1392 | } 1393 | 1394 | void Adafruit_NeoPixel::clear() { 1395 | memset(pixels, 0, numBytes); 1396 | } 1397 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/Adafruit_NeoPixel.h: -------------------------------------------------------------------------------- 1 | /*-------------------------------------------------------------------- 2 | This file is part of the Adafruit NeoPixel library. 3 | 4 | NeoPixel is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU Lesser General Public License as 6 | published by the Free Software Foundation, either version 3 of 7 | the License, or (at your option) any later version. 8 | 9 | NeoPixel is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU Lesser General Public License for more details. 13 | 14 | You should have received a copy of the GNU Lesser General Public 15 | License along with NeoPixel. If not, see 16 | . 17 | --------------------------------------------------------------------*/ 18 | 19 | #ifndef ADAFRUIT_NEOPIXEL_H 20 | #define ADAFRUIT_NEOPIXEL_H 21 | 22 | #if (ARDUINO >= 100) 23 | #include 24 | #else 25 | #include 26 | #include 27 | #endif 28 | 29 | // The order of primary colors in the NeoPixel data stream can vary 30 | // among device types, manufacturers and even different revisions of 31 | // the same item. The third parameter to the Adafruit_NeoPixel 32 | // constructor encodes the per-pixel byte offsets of the red, green 33 | // and blue primaries (plus white, if present) in the data stream -- 34 | // the following #defines provide an easier-to-use named version for 35 | // each permutation. e.g. NEO_GRB indicates a NeoPixel-compatible 36 | // device expecting three bytes per pixel, with the first byte 37 | // containing the green value, second containing red and third 38 | // containing blue. The in-memory representation of a chain of 39 | // NeoPixels is the same as the data-stream order; no re-ordering of 40 | // bytes is required when issuing data to the chain. 41 | 42 | // Bits 5,4 of this value are the offset (0-3) from the first byte of 43 | // a pixel to the location of the red color byte. Bits 3,2 are the 44 | // green offset and 1,0 are the blue offset. If it is an RGBW-type 45 | // device (supporting a white primary in addition to R,G,B), bits 7,6 46 | // are the offset to the white byte...otherwise, bits 7,6 are set to 47 | // the same value as 5,4 (red) to indicate an RGB (not RGBW) device. 48 | // i.e. binary representation: 49 | // 0bWWRRGGBB for RGBW devices 50 | // 0bRRRRGGBB for RGB 51 | 52 | // RGB NeoPixel permutations; white and red offsets are always same 53 | // Offset: W R G B 54 | #define NEO_RGB ((0 << 6) | (0 << 4) | (1 << 2) | (2)) 55 | #define NEO_RBG ((0 << 6) | (0 << 4) | (2 << 2) | (1)) 56 | #define NEO_GRB ((1 << 6) | (1 << 4) | (0 << 2) | (2)) 57 | #define NEO_GBR ((2 << 6) | (2 << 4) | (0 << 2) | (1)) 58 | #define NEO_BRG ((1 << 6) | (1 << 4) | (2 << 2) | (0)) 59 | #define NEO_BGR ((2 << 6) | (2 << 4) | (1 << 2) | (0)) 60 | 61 | // RGBW NeoPixel permutations; all 4 offsets are distinct 62 | // Offset: W R G B 63 | #define NEO_WRGB ((0 << 6) | (1 << 4) | (2 << 2) | (3)) 64 | #define NEO_WRBG ((0 << 6) | (1 << 4) | (3 << 2) | (2)) 65 | #define NEO_WGRB ((0 << 6) | (2 << 4) | (1 << 2) | (3)) 66 | #define NEO_WGBR ((0 << 6) | (3 << 4) | (1 << 2) | (2)) 67 | #define NEO_WBRG ((0 << 6) | (2 << 4) | (3 << 2) | (1)) 68 | #define NEO_WBGR ((0 << 6) | (3 << 4) | (2 << 2) | (1)) 69 | 70 | #define NEO_RWGB ((1 << 6) | (0 << 4) | (2 << 2) | (3)) 71 | #define NEO_RWBG ((1 << 6) | (0 << 4) | (3 << 2) | (2)) 72 | #define NEO_RGWB ((2 << 6) | (0 << 4) | (1 << 2) | (3)) 73 | #define NEO_RGBW ((3 << 6) | (0 << 4) | (1 << 2) | (2)) 74 | #define NEO_RBWG ((2 << 6) | (0 << 4) | (3 << 2) | (1)) 75 | #define NEO_RBGW ((3 << 6) | (0 << 4) | (2 << 2) | (1)) 76 | 77 | #define NEO_GWRB ((1 << 6) | (2 << 4) | (0 << 2) | (3)) 78 | #define NEO_GWBR ((1 << 6) | (3 << 4) | (0 << 2) | (2)) 79 | #define NEO_GRWB ((2 << 6) | (1 << 4) | (0 << 2) | (3)) 80 | #define NEO_GRBW ((3 << 6) | (1 << 4) | (0 << 2) | (2)) 81 | #define NEO_GBWR ((2 << 6) | (3 << 4) | (0 << 2) | (1)) 82 | #define NEO_GBRW ((3 << 6) | (2 << 4) | (0 << 2) | (1)) 83 | 84 | #define NEO_BWRG ((1 << 6) | (2 << 4) | (3 << 2) | (0)) 85 | #define NEO_BWGR ((1 << 6) | (3 << 4) | (2 << 2) | (0)) 86 | #define NEO_BRWG ((2 << 6) | (1 << 4) | (3 << 2) | (0)) 87 | #define NEO_BRGW ((3 << 6) | (1 << 4) | (2 << 2) | (0)) 88 | #define NEO_BGWR ((2 << 6) | (3 << 4) | (1 << 2) | (0)) 89 | #define NEO_BGRW ((3 << 6) | (2 << 4) | (1 << 2) | (0)) 90 | 91 | // Add NEO_KHZ400 to the color order value to indicate a 400 KHz 92 | // device. All but the earliest v1 NeoPixels expect an 800 KHz data 93 | // stream, this is the default if unspecified. Because flash space 94 | // is very limited on ATtiny devices (e.g. Trinket, Gemma), v1 95 | // NeoPixels aren't handled by default on those chips, though it can 96 | // be enabled by removing the ifndef/endif below -- but code will be 97 | // bigger. Conversely, can disable the NEO_KHZ400 line on other MCUs 98 | // to remove v1 support and save a little space. 99 | 100 | #define NEO_KHZ800 0x0000 // 800 KHz datastream 101 | #ifndef __AVR_ATtiny85__ 102 | #define NEO_KHZ400 0x0100 // 400 KHz datastream 103 | #endif 104 | 105 | // If 400 KHz support is enabled, the third parameter to the constructor 106 | // requires a 16-bit value (in order to select 400 vs 800 KHz speed). 107 | // If only 800 KHz is enabled (as is default on ATtiny), an 8-bit value 108 | // is sufficient to encode pixel color order, saving some space. 109 | 110 | #ifdef NEO_KHZ400 111 | typedef uint16_t neoPixelType; 112 | #else 113 | typedef uint8_t neoPixelType; 114 | #endif 115 | 116 | class Adafruit_NeoPixel { 117 | 118 | public: 119 | 120 | // Constructor: number of LEDs, pin number, LED type 121 | Adafruit_NeoPixel(uint16_t n, uint8_t p=6, neoPixelType t=NEO_GRB + NEO_KHZ800); 122 | Adafruit_NeoPixel(void); 123 | ~Adafruit_NeoPixel(); 124 | 125 | void 126 | begin(void), 127 | show(void), 128 | setPin(uint8_t p), 129 | setPixelColor(uint16_t n, uint8_t r, uint8_t g, uint8_t b), 130 | setPixelColor(uint16_t n, uint8_t r, uint8_t g, uint8_t b, uint8_t w), 131 | setPixelColor(uint16_t n, uint32_t c), 132 | setBrightness(uint8_t), 133 | clear(), 134 | updateLength(uint16_t n), 135 | updateType(neoPixelType t); 136 | uint8_t 137 | *getPixels(void) const, 138 | getBrightness(void) const; 139 | uint16_t 140 | numPixels(void) const; 141 | static uint32_t 142 | Color(uint8_t r, uint8_t g, uint8_t b), 143 | Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w); 144 | uint32_t 145 | getPixelColor(uint16_t n) const; 146 | inline bool 147 | canShow(void) { return (micros() - endTime) >= 50L; } 148 | 149 | private: 150 | 151 | boolean 152 | #ifdef NEO_KHZ400 // If 400 KHz NeoPixel support enabled... 153 | is800KHz, // ...true if 800 KHz pixels 154 | #endif 155 | begun; // true if begin() previously called 156 | uint16_t 157 | numLEDs, // Number of RGB LEDs in strip 158 | numBytes; // Size of 'pixels' buffer below (3 or 4 bytes/pixel) 159 | int8_t 160 | pin; // Output pin number (-1 if not yet set) 161 | uint8_t 162 | brightness, 163 | *pixels, // Holds LED color values (3 or 4 bytes each) 164 | rOffset, // Index of red byte within each 3- or 4-byte pixel 165 | gOffset, // Index of green byte 166 | bOffset, // Index of blue byte 167 | wOffset; // Index of white byte (same as rOffset if no white) 168 | uint32_t 169 | endTime; // Latch timing reference 170 | #ifdef __AVR__ 171 | volatile uint8_t 172 | *port; // Output PORT register 173 | uint8_t 174 | pinMask; // Output PORT bitmask 175 | #endif 176 | 177 | }; 178 | 179 | #endif // ADAFRUIT_NEOPIXEL_H 180 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/README.md: -------------------------------------------------------------------------------- 1 | # Adafruit NeoPixel Library [![Build Status](https://travis-ci.org/adafruit/Adafruit_NeoPixel.svg?branch=master)](https://travis-ci.org/adafruit/Adafruit_NeoPixel) 2 | 3 | Arduino library for controlling single-wire-based LED pixels and strip such as the [Adafruit 60 LED/meter Digital LED strip][strip], the [Adafruit FLORA RGB Smart Pixel][flora], the [Adafruit Breadboard-friendly RGB Smart Pixel][pixel], the [Adafruit NeoPixel Stick][stick], and the [Adafruit NeoPixel Shield][shield]. 4 | 5 | After downloading, rename folder to 'Adafruit_NeoPixel' and install in Arduino Libraries folder. Restart Arduino IDE, then open File->Sketchbook->Library->Adafruit_NeoPixel->strandtest sketch. 6 | 7 | [flora]: http://adafruit.com/products/1060 8 | [strip]: http://adafruit.com/products/1138 9 | [pixel]: http://adafruit.com/products/1312 10 | [stick]: http://adafruit.com/products/1426 11 | [shield]: http://adafruit.com/products/1430 12 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/esp8266.c: -------------------------------------------------------------------------------- 1 | // This is a mash-up of the Due show() code + insights from Michael Miller's 2 | // ESP8266 work for the NeoPixelBus library: github.com/Makuna/NeoPixelBus 3 | // Needs to be a separate .c file to enforce ICACHE_RAM_ATTR execution. 4 | 5 | #ifdef ESP8266 6 | 7 | #include 8 | #include 9 | 10 | static uint32_t _getCycleCount(void) __attribute__((always_inline)); 11 | static inline uint32_t _getCycleCount(void) { 12 | uint32_t ccount; 13 | __asm__ __volatile__("rsr %0,ccount":"=a" (ccount)); 14 | return ccount; 15 | } 16 | 17 | void ICACHE_RAM_ATTR espShow( 18 | uint8_t pin, uint8_t *pixels, uint32_t numBytes, boolean is800KHz) { 19 | 20 | #define CYCLES_800_T0H (F_CPU / 2500000) // 0.4us 21 | #define CYCLES_800_T1H (F_CPU / 1250000) // 0.8us 22 | #define CYCLES_800 (F_CPU / 800000) // 1.25us per bit 23 | #define CYCLES_400_T0H (F_CPU / 2000000) // 0.5uS 24 | #define CYCLES_400_T1H (F_CPU / 833333) // 1.2us 25 | #define CYCLES_400 (F_CPU / 400000) // 2.5us per bit 26 | 27 | uint8_t *p, *end, pix, mask; 28 | uint32_t t, time0, time1, period, c, startTime, pinMask; 29 | 30 | pinMask = _BV(pin); 31 | p = pixels; 32 | end = p + numBytes; 33 | pix = *p++; 34 | mask = 0x80; 35 | startTime = 0; 36 | 37 | #ifdef NEO_KHZ400 38 | if(is800KHz) { 39 | #endif 40 | time0 = CYCLES_800_T0H; 41 | time1 = CYCLES_800_T1H; 42 | period = CYCLES_800; 43 | #ifdef NEO_KHZ400 44 | } else { // 400 KHz bitstream 45 | time0 = CYCLES_400_T0H; 46 | time1 = CYCLES_400_T1H; 47 | period = CYCLES_400; 48 | } 49 | #endif 50 | 51 | for(t = time0;; t = time0) { 52 | if(pix & mask) t = time1; // Bit high duration 53 | while(((c = _getCycleCount()) - startTime) < period); // Wait for bit start 54 | GPIO_REG_WRITE(GPIO_OUT_W1TS_ADDRESS, pinMask); // Set high 55 | startTime = c; // Save start time 56 | while(((c = _getCycleCount()) - startTime) < t); // Wait high duration 57 | GPIO_REG_WRITE(GPIO_OUT_W1TC_ADDRESS, pinMask); // Set low 58 | if(!(mask >>= 1)) { // Next bit/byte 59 | if(p >= end) break; 60 | pix = *p++; 61 | mask = 0x80; 62 | } 63 | } 64 | while((_getCycleCount() - startTime) < period); // Wait for last bit 65 | } 66 | 67 | #endif // ESP8266 68 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/examples/RGBWstrandtest/.esp8266.test.skip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/libraries/Adafruit_NeoPixel/examples/RGBWstrandtest/.esp8266.test.skip -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/examples/RGBWstrandtest/RGBWstrandtest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #ifdef __AVR__ 3 | #include 4 | #endif 5 | 6 | #define PIN 6 7 | 8 | #define NUM_LEDS 60 9 | 10 | #define BRIGHTNESS 50 11 | 12 | Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_LEDS, PIN, NEO_GRBW + NEO_KHZ800); 13 | 14 | int gamma[] = { 15 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 17 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 18 | 2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 5, 5, 5, 19 | 5, 6, 6, 6, 6, 7, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 20 | 10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16, 21 | 17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25, 22 | 25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36, 23 | 37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50, 24 | 51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68, 25 | 69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89, 26 | 90, 92, 93, 95, 96, 98, 99,101,102,104,105,107,109,110,112,114, 27 | 115,117,119,120,122,124,126,127,129,131,133,135,137,138,140,142, 28 | 144,146,148,150,152,154,156,158,160,162,164,167,169,171,173,175, 29 | 177,180,182,184,186,189,191,193,196,198,200,203,205,208,210,213, 30 | 215,218,220,223,225,228,231,233,236,239,241,244,247,249,252,255 }; 31 | 32 | 33 | void setup() { 34 | Serial.begin(115200); 35 | // This is for Trinket 5V 16MHz, you can remove these three lines if you are not using a Trinket 36 | #if defined (__AVR_ATtiny85__) 37 | if (F_CPU == 16000000) clock_prescale_set(clock_div_1); 38 | #endif 39 | // End of trinket special code 40 | strip.setBrightness(BRIGHTNESS); 41 | strip.begin(); 42 | strip.show(); // Initialize all pixels to 'off' 43 | } 44 | 45 | void loop() { 46 | // Some example procedures showing how to display to the pixels: 47 | colorWipe(strip.Color(255, 0, 0), 50); // Red 48 | colorWipe(strip.Color(0, 255, 0), 50); // Green 49 | colorWipe(strip.Color(0, 0, 255), 50); // Blue 50 | colorWipe(strip.Color(0, 0, 0, 255), 50); // White 51 | 52 | whiteOverRainbow(20,75,5); 53 | 54 | pulseWhite(5); 55 | 56 | // fullWhite(); 57 | // delay(2000); 58 | 59 | rainbowFade2White(3,3,1); 60 | 61 | 62 | } 63 | 64 | // Fill the dots one after the other with a color 65 | void colorWipe(uint32_t c, uint8_t wait) { 66 | for(uint16_t i=0; i= 0 ; j--){ 83 | for(uint16_t i=0; i 255 - fadeMax ){ 121 | fadeVal--; 122 | } 123 | 124 | strip.show(); 125 | delay(wait); 126 | } 127 | 128 | } 129 | 130 | 131 | 132 | delay(500); 133 | 134 | 135 | for(int k = 0 ; k < whiteLoops ; k ++){ 136 | 137 | for(int j = 0; j < 256 ; j++){ 138 | 139 | for(uint16_t i=0; i < strip.numPixels(); i++) { 140 | strip.setPixelColor(i, strip.Color(0,0,0, gamma[j] ) ); 141 | } 142 | strip.show(); 143 | } 144 | 145 | delay(2000); 146 | for(int j = 255; j >= 0 ; j--){ 147 | 148 | for(uint16_t i=0; i < strip.numPixels(); i++) { 149 | strip.setPixelColor(i, strip.Color(0,0,0, gamma[j] ) ); 150 | } 151 | strip.show(); 152 | } 153 | } 154 | 155 | delay(500); 156 | 157 | 158 | } 159 | 160 | void whiteOverRainbow(uint8_t wait, uint8_t whiteSpeed, uint8_t whiteLength ) { 161 | 162 | if(whiteLength >= strip.numPixels()) whiteLength = strip.numPixels() - 1; 163 | 164 | int head = whiteLength - 1; 165 | int tail = 0; 166 | 167 | int loops = 3; 168 | int loopNum = 0; 169 | 170 | static unsigned long lastTime = 0; 171 | 172 | 173 | while(true){ 174 | for(int j=0; j<256; j++) { 175 | for(uint16_t i=0; i= tail && i <= head) || (tail > head && i >= tail) || (tail > head && i <= head) ){ 177 | strip.setPixelColor(i, strip.Color(0,0,0, 255 ) ); 178 | } 179 | else{ 180 | strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255)); 181 | } 182 | 183 | } 184 | 185 | if(millis() - lastTime > whiteSpeed) { 186 | head++; 187 | tail++; 188 | if(head == strip.numPixels()){ 189 | loopNum++; 190 | } 191 | lastTime = millis(); 192 | } 193 | 194 | if(loopNum == loops) return; 195 | 196 | head%=strip.numPixels(); 197 | tail%=strip.numPixels(); 198 | strip.show(); 199 | delay(wait); 200 | } 201 | } 202 | 203 | } 204 | void fullWhite() { 205 | 206 | for(uint16_t i=0; i> 8); 255 | } 256 | uint8_t green(uint32_t c) { 257 | return (c >> 16); 258 | } 259 | uint8_t blue(uint32_t c) { 260 | return (c); 261 | } 262 | 263 | 264 | -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/examples/buttoncycler/.esp8266.test.skip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Quadruped_Robot/cffaee053aa4b6604095282171e35a59ce702ed8/libraries/Adafruit_NeoPixel/examples/buttoncycler/.esp8266.test.skip -------------------------------------------------------------------------------- /libraries/Adafruit_NeoPixel/examples/buttoncycler/buttoncycler.ino: -------------------------------------------------------------------------------- 1 | // This is a demonstration on how to use an input device to trigger changes on your neo pixels. 2 | // You should wire a momentary push button to connect from ground to a digital IO pin. When you 3 | // press the button it will change to a new pixel animation. Note that you need to press the 4 | // button once to start the first animation! 5 | 6 | #include 7 | 8 | #define BUTTON_PIN 2 // Digital IO pin connected to the button. This will be 9 | // driven with a pull-up resistor so the switch should 10 | // pull the pin to ground momentarily. On a high -> low 11 | // transition the button press logic will execute. 12 | 13 | #define PIXEL_PIN 6 // Digital IO pin connected to the NeoPixels. 14 | 15 | #define PIXEL_COUNT 16 16 | 17 | // Parameter 1 = number of pixels in strip, neopixel stick has 8 18 | // Parameter 2 = pin number (most are valid) 19 | // Parameter 3 = pixel type flags, add together as needed: 20 | // NEO_RGB Pixels are wired for RGB bitstream 21 | // NEO_GRB Pixels are wired for GRB bitstream, correct for neopixel stick 22 | // NEO_KHZ400 400 KHz bitstream (e.g. FLORA pixels) 23 | // NEO_KHZ800 800 KHz bitstream (e.g. High Density LED strip), correct for neopixel stick 24 | Adafruit_NeoPixel strip = Adafruit_NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800); 25 | 26 | bool oldState = HIGH; 27 | int showType = 0; 28 | 29 | void setup() { 30 | pinMode(BUTTON_PIN, INPUT_PULLUP); 31 | strip.begin(); 32 | strip.show(); // Initialize all pixels to 'off' 33 | } 34 | 35 | void loop() { 36 | // Get current button state. 37 | bool newState = digitalRead(BUTTON_PIN); 38 | 39 | // Check if state changed from high to low (button press). 40 | if (newState == LOW && oldState == HIGH) { 41 | // Short delay to debounce button. 42 | delay(20); 43 | // Check if button is still low after debounce. 44 | newState = digitalRead(BUTTON_PIN); 45 | if (newState == LOW) { 46 | showType++; 47 | if (showType > 9) 48 | showType=0; 49 | startShow(showType); 50 | } 51 | } 52 | 53 | // Set the last button state to the old state. 54 | oldState = newState; 55 | } 56 | 57 | void startShow(int i) { 58 | switch(i){ 59 | case 0: colorWipe(strip.Color(0, 0, 0), 50); // Black/off 60 | break; 61 | case 1: colorWipe(strip.Color(255, 0, 0), 50); // Red 62 | break; 63 | case 2: colorWipe(strip.Color(0, 255, 0), 50); // Green 64 | break; 65 | case 3: colorWipe(strip.Color(0, 0, 255), 50); // Blue 66 | break; 67 | case 4: theaterChase(strip.Color(127, 127, 127), 50); // White 68 | break; 69 | case 5: theaterChase(strip.Color(127, 0, 0), 50); // Red 70 | break; 71 | case 6: theaterChase(strip.Color( 0, 0, 127), 50); // Blue 72 | break; 73 | case 7: rainbow(20); 74 | break; 75 | case 8: rainbowCycle(20); 76 | break; 77 | case 9: theaterChaseRainbow(50); 78 | break; 79 | } 80 | } 81 | 82 | // Fill the dots one after the other with a color 83 | void colorWipe(uint32_t c, uint8_t wait) { 84 | for(uint16_t i=0; i 5 | #ifdef __AVR__ 6 | #include 7 | #endif 8 | 9 | // Which pin on the Arduino is connected to the NeoPixels? 10 | // On a Trinket or Gemma we suggest changing this to 1 11 | #define PIN 6 12 | 13 | // How many NeoPixels are attached to the Arduino? 14 | #define NUMPIXELS 16 15 | 16 | // When we setup the NeoPixel library, we tell it how many pixels, and which pin to use to send signals. 17 | // Note that for older NeoPixel strips you might need to change the third parameter--see the strandtest 18 | // example for more information on possible values. 19 | Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800); 20 | 21 | int delayval = 500; // delay for half a second 22 | 23 | void setup() { 24 | // This is for Trinket 5V 16MHz, you can remove these three lines if you are not using a Trinket 25 | #if defined (__AVR_ATtiny85__) 26 | if (F_CPU == 16000000) clock_prescale_set(clock_div_1); 27 | #endif 28 | // End of trinket special code 29 | 30 | pixels.begin(); // This initializes the NeoPixel library. 31 | } 32 | 33 | void loop() { 34 | 35 | // For a set of NeoPixels the first NeoPixel is 0, second is 1, all the way up to the count of pixels minus one. 36 | 37 | for(int i=0;i 2 | #ifdef __AVR__ 3 | #include 4 | #endif 5 | 6 | #define PIN 6 7 | 8 | // Parameter 1 = number of pixels in strip 9 | // Parameter 2 = Arduino pin number (most are valid) 10 | // Parameter 3 = pixel type flags, add together as needed: 11 | // NEO_KHZ800 800 KHz bitstream (most NeoPixel products w/WS2812 LEDs) 12 | // NEO_KHZ400 400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers) 13 | // NEO_GRB Pixels are wired for GRB bitstream (most NeoPixel products) 14 | // NEO_RGB Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2) 15 | // NEO_RGBW Pixels are wired for RGBW bitstream (NeoPixel RGBW products) 16 | Adafruit_NeoPixel strip = Adafruit_NeoPixel(60, PIN, NEO_GRB + NEO_KHZ800); 17 | 18 | // IMPORTANT: To reduce NeoPixel burnout risk, add 1000 uF capacitor across 19 | // pixel power leads, add 300 - 500 Ohm resistor on first pixel's data input 20 | // and minimize distance between Arduino and first pixel. Avoid connecting 21 | // on a live circuit...if you must, connect GND first. 22 | 23 | void setup() { 24 | // This is for Trinket 5V 16MHz, you can remove these three lines if you are not using a Trinket 25 | #if defined (__AVR_ATtiny85__) 26 | if (F_CPU == 16000000) clock_prescale_set(clock_div_1); 27 | #endif 28 | // End of trinket special code 29 | 30 | 31 | strip.begin(); 32 | strip.show(); // Initialize all pixels to 'off' 33 | } 34 | 35 | void loop() { 36 | // Some example procedures showing how to display to the pixels: 37 | colorWipe(strip.Color(255, 0, 0), 50); // Red 38 | colorWipe(strip.Color(0, 255, 0), 50); // Green 39 | colorWipe(strip.Color(0, 0, 255), 50); // Blue 40 | //colorWipe(strip.Color(0, 0, 0, 255), 50); // White RGBW 41 | // Send a theater pixel chase in... 42 | theaterChase(strip.Color(127, 127, 127), 50); // White 43 | theaterChase(strip.Color(127, 0, 0), 50); // Red 44 | theaterChase(strip.Color(0, 0, 127), 50); // Blue 45 | 46 | rainbow(20); 47 | rainbowCycle(20); 48 | theaterChaseRainbow(50); 49 | } 50 | 51 | // Fill the dots one after the other with a color 52 | void colorWipe(uint32_t c, uint8_t wait) { 53 | for(uint16_t i=0; i 5 | sentence=Arduino library for controlling single-wire-based LED pixels and strip. 6 | paragraph=Arduino library for controlling single-wire-based LED pixels and strip. 7 | category=Display 8 | url=https://github.com/adafruit/Adafruit_NeoPixel 9 | architectures=* 10 | -------------------------------------------------------------------------------- /libraries/PS2X_lib/Examples/PS2X_Example/PS2X_Example.pde: -------------------------------------------------------------------------------- 1 | #include //for v1.6 2 | 3 | PS2X ps2x; // create PS2 Controller Class 4 | 5 | //right now, the library does NOT support hot pluggable controllers, meaning 6 | //you must always either restart your Arduino after you conect the controller, 7 | //or call config_gamepad(pins) again after connecting the controller. 8 | int error = 0; 9 | byte type = 0; 10 | byte vibrate = 0; 11 | 12 | void setup(){ 13 | Serial.begin(57600); 14 | 15 | //CHANGES for v1.6 HERE!!! **************PAY ATTENTION************* 16 | 17 | error = ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error 18 | 19 | if(error == 0){ 20 | Serial.println("Found Controller, configured successful"); 21 | Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;"); 22 | Serial.println("holding L1 or R1 will print out the analog stick values."); 23 | Serial.println("Go to www.billporter.info for updates and to report bugs."); 24 | } 25 | 26 | else if(error == 1) 27 | Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips"); 28 | 29 | else if(error == 2) 30 | Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips"); 31 | 32 | else if(error == 3) 33 | Serial.println("Controller refusing to enter Pressures mode, may not support it. "); 34 | 35 | //Serial.print(ps2x.Analog(1), HEX); 36 | 37 | type = ps2x.readType(); 38 | switch(type) { 39 | case 0: 40 | Serial.println("Unknown Controller type"); 41 | break; 42 | case 1: 43 | Serial.println("DualShock Controller Found"); 44 | break; 45 | case 2: 46 | Serial.println("GuitarHero Controller Found"); 47 | break; 48 | } 49 | 50 | } 51 | 52 | void loop(){ 53 | /* You must Read Gamepad to get new values 54 | Read GamePad and set vibration values 55 | ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255) 56 | if you don't enable the rumble, use ps2x.read_gamepad(); with no values 57 | 58 | you should call this at least once a second 59 | */ 60 | 61 | 62 | 63 | if(error == 1) //skip loop if no controller found 64 | return; 65 | 66 | if(type == 2){ //Guitar Hero Controller 67 | 68 | ps2x.read_gamepad(); //read controller 69 | 70 | if(ps2x.ButtonPressed(GREEN_FRET)) 71 | Serial.println("Green Fret Pressed"); 72 | if(ps2x.ButtonPressed(RED_FRET)) 73 | Serial.println("Red Fret Pressed"); 74 | if(ps2x.ButtonPressed(YELLOW_FRET)) 75 | Serial.println("Yellow Fret Pressed"); 76 | if(ps2x.ButtonPressed(BLUE_FRET)) 77 | Serial.println("Blue Fret Pressed"); 78 | if(ps2x.ButtonPressed(ORANGE_FRET)) 79 | Serial.println("Orange Fret Pressed"); 80 | 81 | 82 | if(ps2x.ButtonPressed(STAR_POWER)) 83 | Serial.println("Star Power Command"); 84 | 85 | if(ps2x.Button(UP_STRUM)) //will be TRUE as long as button is pressed 86 | Serial.println("Up Strum"); 87 | if(ps2x.Button(DOWN_STRUM)) 88 | Serial.println("DOWN Strum"); 89 | 90 | 91 | if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed 92 | Serial.println("Start is being held"); 93 | if(ps2x.Button(PSB_SELECT)) 94 | Serial.println("Select is being held"); 95 | 96 | 97 | if(ps2x.Button(ORANGE_FRET)) // print stick value IF TRUE 98 | { 99 | Serial.print("Wammy Bar Position:"); 100 | Serial.println(ps2x.Analog(WHAMMY_BAR), DEC); 101 | } 102 | } 103 | 104 | else { //DualShock Controller 105 | 106 | ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed 107 | 108 | if(ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed 109 | Serial.println("Start is being held"); 110 | if(ps2x.Button(PSB_SELECT)) 111 | Serial.println("Select is being held"); 112 | 113 | 114 | if(ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed 115 | Serial.print("Up held this hard: "); 116 | Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC); 117 | } 118 | if(ps2x.Button(PSB_PAD_RIGHT)){ 119 | Serial.print("Right held this hard: "); 120 | Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC); 121 | } 122 | if(ps2x.Button(PSB_PAD_LEFT)){ 123 | Serial.print("LEFT held this hard: "); 124 | Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC); 125 | } 126 | if(ps2x.Button(PSB_PAD_DOWN)){ 127 | Serial.print("DOWN held this hard: "); 128 | Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC); 129 | } 130 | 131 | 132 | vibrate = ps2x.Analog(PSAB_BLUE); //this will set the large motor vibrate speed based on 133 | //how hard you press the blue (X) button 134 | 135 | if (ps2x.NewButtonState()) //will be TRUE if any button changes state (on to off, or off to on) 136 | { 137 | 138 | 139 | 140 | if(ps2x.Button(PSB_L3)) 141 | Serial.println("L3 pressed"); 142 | if(ps2x.Button(PSB_R3)) 143 | Serial.println("R3 pressed"); 144 | if(ps2x.Button(PSB_L2)) 145 | Serial.println("L2 pressed"); 146 | if(ps2x.Button(PSB_R2)) 147 | Serial.println("R2 pressed"); 148 | if(ps2x.Button(PSB_GREEN)) 149 | Serial.println("Triangle pressed"); 150 | 151 | } 152 | 153 | 154 | if(ps2x.ButtonPressed(PSB_RED)) //will be TRUE if button was JUST pressed 155 | Serial.println("Circle just pressed"); 156 | 157 | if(ps2x.ButtonReleased(PSB_PINK)) //will be TRUE if button was JUST released 158 | Serial.println("Square just released"); 159 | 160 | if(ps2x.NewButtonState(PSB_BLUE)) //will be TRUE if button was JUST pressed OR released 161 | Serial.println("X just changed"); 162 | 163 | 164 | if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) // print stick values if either is TRUE 165 | { 166 | Serial.print("Stick Values:"); 167 | Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX 168 | Serial.print(","); 169 | Serial.print(ps2x.Analog(PSS_LX), DEC); 170 | Serial.print(","); 171 | Serial.print(ps2x.Analog(PSS_RY), DEC); 172 | Serial.print(","); 173 | Serial.println(ps2x.Analog(PSS_RX), DEC); 174 | } 175 | 176 | 177 | } 178 | 179 | 180 | delay(50); 181 | 182 | } 183 | -------------------------------------------------------------------------------- /libraries/PS2X_lib/PS2X_lib.cpp: -------------------------------------------------------------------------------- 1 | #include "PS2X_lib.h" 2 | #include 3 | #include 4 | #include 5 | 6 | #ifndef __SAM3X8E__ 7 | #include 8 | #endif 9 | #if ARDUINO > 22 10 | #include "Arduino.h" 11 | #else 12 | #include "WProgram.h" 13 | #include "pins_arduino.h" 14 | #endif 15 | 16 | 17 | 18 | static byte enter_config[]={0x01,0x43,0x00,0x01,0x00}; 19 | static byte set_mode[]={0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00}; 20 | static byte set_bytes_large[]={0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00}; 21 | static byte exit_config[]={0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A}; 22 | static byte enable_rumble[]={0x01,0x4D,0x00,0x00,0x01}; 23 | static byte type_read[]={0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A}; 24 | 25 | boolean PS2X::NewButtonState() { 26 | return ((last_buttons ^ buttons) > 0); 27 | 28 | } 29 | 30 | boolean PS2X::NewButtonState(unsigned int button) { 31 | return (((last_buttons ^ buttons) & button) > 0); 32 | } 33 | 34 | boolean PS2X::ButtonPressed(unsigned int button) { 35 | return(NewButtonState(button) & Button(button)); 36 | } 37 | 38 | boolean PS2X::ButtonReleased(unsigned int button) { 39 | return((NewButtonState(button)) & ((~last_buttons & button) > 0)); 40 | } 41 | 42 | boolean PS2X::Button(uint16_t button) { 43 | return ((~buttons & button) > 0); 44 | } 45 | 46 | unsigned int PS2X::ButtonDataByte() { 47 | return (~buttons); 48 | } 49 | 50 | byte PS2X::Analog(byte button) { 51 | return PS2data[button]; 52 | } 53 | unsigned char PS2X::_gamepad_shiftinout (char byte) { 54 | 55 | 56 | unsigned char tmp = 0; 57 | for(i=0;i<8;i++) { 58 | 59 | if(CHK(byte,i)) CMD_SET(); 60 | else CMD_CLR(); 61 | CLK_CLR(); 62 | 63 | delayMicroseconds(CTRL_CLK); 64 | 65 | if(DAT_CHK()) SET(tmp,i); 66 | CLK_SET(); 67 | #if CTRL_CLK_HIGH 68 | delayMicroseconds(CTRL_CLK_HIGH); 69 | #endif 70 | } 71 | CMD_SET(); 72 | delayMicroseconds(CTRL_BYTE_DELAY); 73 | return tmp; 74 | } 75 | 76 | void PS2X::read_gamepad() { 77 | read_gamepad(false, 0x00); 78 | } 79 | 80 | 81 | boolean PS2X::read_gamepad(boolean motor1, byte motor2) { 82 | double temp = millis() - last_read; 83 | 84 | if (temp > 1500) //waited to long 85 | reconfig_gamepad(); 86 | 87 | if(temp < read_delay) //waited too short 88 | delay(read_delay - temp); 89 | 90 | 91 | 92 | if(motor2 != 0x00) 93 | motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin 94 | 95 | char dword[9] = {0x01,0x42,0,motor1,motor2,0,0,0,0}; 96 | byte dword2[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; 97 | 98 | // Try a few times to get valid data... 99 | for (byte RetryCnt = 0; RetryCnt < 5; RetryCnt++) { 100 | CMD_SET(); 101 | CLK_SET(); 102 | ATT_CLR(); // low enable joystick 103 | 104 | delayMicroseconds(CTRL_BYTE_DELAY); 105 | //Send the command to send button and joystick data; 106 | 107 | for (int i = 0; i<9; i++) { 108 | PS2data[i] = _gamepad_shiftinout(dword[i]); 109 | } 110 | 111 | 112 | if(PS2data[1] == 0x79) { //if controller is in full data return mode, get the rest of data 113 | for (int i = 0; i<12; i++) { 114 | PS2data[i+9] = _gamepad_shiftinout(dword2[i]); 115 | } 116 | } 117 | 118 | ATT_SET(); // HI disable joystick 119 | // Check to see if we received valid data or not. We should be in analog mode for our data 120 | // to be valie 121 | if ((PS2data[1] & 0xf0) == 0x70) 122 | break; 123 | 124 | // If we got to here, we are not in analog mode, try to recover... 125 | reconfig_gamepad(); // try to get back into Analog mode. 126 | delay(read_delay); 127 | } 128 | 129 | // If we get here and still not in analog mode, try increasing the read_delay... 130 | if ((PS2data[1] & 0xf0) != 0x70) { 131 | if (read_delay < 10) 132 | read_delay++; // see if this helps out... 133 | } 134 | 135 | 136 | #ifdef PS2X_COM_DEBUG 137 | Serial.println("OUT:IN"); 138 | for(int i=0; i<9; i++){ 139 | Serial.print(dword[i], HEX); 140 | Serial.print(":"); 141 | Serial.print(PS2data[i], HEX); 142 | Serial.print(" "); 143 | } 144 | for (int i = 0; i<12; i++) { 145 | Serial.print(dword2[i], HEX); 146 | Serial.print(":"); 147 | Serial.print(PS2data[i+9], HEX); 148 | Serial.print(" "); 149 | } 150 | Serial.println(""); 151 | #endif 152 | 153 | last_buttons = buttons; //store the previous buttons states 154 | 155 | #if defined(__AVR__) 156 | buttons = *(uint16_t*)(PS2data+3); //store as one value for multiple functions 157 | #else 158 | buttons = (uint16_t)(PS2data[4] << 8) + PS2data[3]; //store as one value for multiple functions 159 | #endif 160 | last_read = millis(); 161 | return ((PS2data[1] & 0xf0) == 0x70); 162 | } 163 | 164 | byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat) { 165 | return config_gamepad(clk, cmd, att, dat, false, false); 166 | } 167 | 168 | 169 | byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) { 170 | 171 | byte temp[sizeof(type_read)]; 172 | 173 | #ifdef __AVR__ 174 | _clk_mask = digitalPinToBitMask(clk); 175 | _clk_oreg = portOutputRegister(digitalPinToPort(clk)); 176 | _cmd_mask = digitalPinToBitMask(cmd); 177 | _cmd_oreg = portOutputRegister(digitalPinToPort(cmd)); 178 | _att_mask = digitalPinToBitMask(att); 179 | _att_oreg = portOutputRegister(digitalPinToPort(att)); 180 | _dat_mask = digitalPinToBitMask(dat); 181 | _dat_ireg = portInputRegister(digitalPinToPort(dat)); 182 | #elif defined(__SAM3X8E__) 183 | // Defines for Arduino UNO. 184 | _clk = clk; 185 | _cmd = cmd; 186 | _att = att; 187 | _dat = dat; 188 | #else 189 | uint32_t lport; // Port number for this pin 190 | _clk_mask = digitalPinToBitMask(clk); 191 | lport = digitalPinToPort(clk); 192 | _clk_lport_set = portOutputRegister(lport) + 2; 193 | _clk_lport_clr = portOutputRegister(lport) + 1; 194 | 195 | _cmd_mask = digitalPinToBitMask(cmd); 196 | lport = digitalPinToPort(cmd); 197 | _cmd_lport_set = portOutputRegister(lport) + 2; 198 | _cmd_lport_clr = portOutputRegister(lport) + 1; 199 | 200 | _att_mask = digitalPinToBitMask(att); 201 | lport = digitalPinToPort(att); 202 | _att_lport_set = portOutputRegister(lport) + 2; 203 | _att_lport_clr = portOutputRegister(lport) + 1; 204 | 205 | _dat_mask = digitalPinToBitMask(dat); 206 | _dat_lport = portInputRegister(digitalPinToPort(dat)); 207 | 208 | #endif 209 | 210 | pinMode(clk, OUTPUT); //configure ports 211 | pinMode(att, OUTPUT); 212 | pinMode(cmd, OUTPUT); 213 | 214 | #if defined(__AVR__) 215 | pinMode(dat, INPUT); 216 | digitalWrite(dat, HIGH); //enable pull-up 217 | #elif defined(__SAM3X8E__) 218 | pinMode(dat, INPUT_PULLUP); 219 | #endif 220 | 221 | CMD_SET(); // SET(*_cmd_oreg,_cmd_mask); 222 | CLK_SET(); 223 | 224 | //new error checking. First, read gamepad a few times to see if it's talking 225 | read_gamepad(); 226 | read_gamepad(); 227 | 228 | //see if it talked 229 | if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79){ //see if mode came back. If still anything but 41, 73 or 79, then it's not talking 230 | #ifdef PS2X_DEBUG 231 | Serial.println("Controller mode not matched or no controller found"); 232 | Serial.print("Expected 0x41 or 0x73, got "); 233 | Serial.println(PS2data[1], HEX); 234 | #endif 235 | 236 | return 1; //return error code 1 237 | } 238 | 239 | //try setting mode, increasing delays if need be. 240 | read_delay = 1; 241 | 242 | for(int y = 0; y <= 10; y++) 243 | { 244 | sendCommandString(enter_config, sizeof(enter_config)); //start config run 245 | 246 | //read type 247 | delayMicroseconds(CTRL_BYTE_DELAY); 248 | 249 | CMD_SET(); 250 | CLK_SET(); 251 | ATT_CLR(); // low enable joystick 252 | 253 | delayMicroseconds(CTRL_BYTE_DELAY); 254 | 255 | for (int i = 0; i<9; i++) { 256 | temp[i] = _gamepad_shiftinout(type_read[i]); 257 | } 258 | 259 | ATT_SET(); // HI disable joystick 260 | 261 | controller_type = temp[3]; 262 | 263 | sendCommandString(set_mode, sizeof(set_mode)); 264 | if(rumble){ sendCommandString(enable_rumble, sizeof(enable_rumble)); en_Rumble = true; } 265 | if(pressures){ sendCommandString(set_bytes_large, sizeof(set_bytes_large)); en_Pressures = true; } 266 | sendCommandString(exit_config, sizeof(exit_config)); 267 | 268 | read_gamepad(); 269 | 270 | if(pressures){ 271 | if(PS2data[1] == 0x79) 272 | break; 273 | if(PS2data[1] == 0x73) 274 | return 3; 275 | } 276 | 277 | if(PS2data[1] == 0x73) 278 | break; 279 | 280 | if(y == 10){ 281 | #ifdef PS2X_DEBUG 282 | Serial.println("Controller not accepting commands"); 283 | Serial.print("mode stil set at"); 284 | Serial.println(PS2data[1], HEX); 285 | #endif 286 | return 2; //exit function with error 287 | } 288 | 289 | read_delay += 1; //add 1ms to read_delay 290 | } 291 | 292 | return 0; //no error if here 293 | } 294 | 295 | 296 | 297 | void PS2X::sendCommandString(byte string[], byte len) { 298 | 299 | 300 | #ifdef PS2X_COM_DEBUG 301 | byte temp[len]; 302 | ATT_CLR(); // low enable joystick 303 | delayMicroseconds(CTRL_BYTE_DELAY); 304 | 305 | for (int y=0; y < len; y++) 306 | temp[y] = _gamepad_shiftinout(string[y]); 307 | 308 | ATT_SET(); //high disable joystick 309 | delay(read_delay); //wait a few 310 | 311 | Serial.println("OUT:IN Configure"); 312 | for(int i=0; i 72 | * 73 | ******************************************************************/ 74 | 75 | 76 | // $$$$$$$$$$$$ DEBUG ENABLE SECTION $$$$$$$$$$$$$$$$ 77 | // to debug ps2 controller, uncomment these two lines to print out debug to uart 78 | 79 | //#define PS2X_DEBUG 80 | //#define PS2X_COM_DEBUG 81 | 82 | 83 | #ifndef PS2X_lib_h 84 | #define PS2X_lib_h 85 | #if ARDUINO > 22 86 | #include "Arduino.h" 87 | #else 88 | #include "WProgram.h" 89 | #endif 90 | 91 | #include 92 | #include 93 | #include 94 | #ifdef __AVR__ 95 | #include 96 | 97 | #define CTRL_CLK 4 98 | #define CTRL_BYTE_DELAY 3 99 | #else 100 | // Pic32... 101 | #include 102 | #define CTRL_CLK 5 103 | #define CTRL_CLK_HIGH 5 104 | #define CTRL_BYTE_DELAY 4 105 | #endif 106 | 107 | //These are our button constants 108 | #define PSB_SELECT 0x0001 109 | #define PSB_L3 0x0002 110 | #define PSB_R3 0x0004 111 | #define PSB_START 0x0008 112 | #define PSB_PAD_UP 0x0010 113 | #define PSB_PAD_RIGHT 0x0020 114 | #define PSB_PAD_DOWN 0x0040 115 | #define PSB_PAD_LEFT 0x0080 116 | #define PSB_L2 0x0100 117 | #define PSB_R2 0x0200 118 | #define PSB_L1 0x0400 119 | #define PSB_R1 0x0800 120 | #define PSB_GREEN 0x1000 121 | #define PSB_RED 0x2000 122 | #define PSB_BLUE 0x4000 123 | #define PSB_PINK 0x8000 124 | #define PSB_TRIANGLE 0x1000 125 | #define PSB_CIRCLE 0x2000 126 | #define PSB_CROSS 0x4000 127 | #define PSB_SQUARE 0x8000 128 | 129 | //Guitar button constants 130 | #define GREEN_FRET 0x0200 131 | #define RED_FRET 0x2000 132 | #define YELLOW_FRET 0x1000 133 | #define BLUE_FRET 0x4000 134 | #define ORANGE_FRET 0x8000 135 | #define STAR_POWER 0x0100 136 | #define UP_STRUM 0x0010 137 | #define DOWN_STRUM 0x0040 138 | #define WHAMMY_BAR 8 139 | 140 | //These are stick values 141 | #define PSS_RX 5 142 | #define PSS_RY 6 143 | #define PSS_LX 7 144 | #define PSS_LY 8 145 | 146 | //These are analog buttons 147 | #define PSAB_PAD_RIGHT 9 148 | #define PSAB_PAD_UP 11 149 | #define PSAB_PAD_DOWN 12 150 | #define PSAB_PAD_LEFT 10 151 | #define PSAB_L2 19 152 | #define PSAB_R2 20 153 | #define PSAB_L1 17 154 | #define PSAB_R1 18 155 | #define PSAB_GREEN 13 156 | #define PSAB_RED 14 157 | #define PSAB_BLUE 15 158 | #define PSAB_PINK 16 159 | #define PSAB_TRIANGLE 13 160 | #define PSAB_CIRCLE 14 161 | #define PSAB_CROSS 15 162 | #define PSAB_SQUARE 16 163 | 164 | 165 | #define SET(x,y) (x|=(1<99 24 | #include 25 | #else 26 | #endif 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include "Hex_CFG.h" 33 | #include "Phoenix.h" 34 | #include "Phoenix_Input_PS2.h" 35 | #include "Phoenix_Driver_SSC32.h" 36 | #include "Phoenix_Code.h" 37 | 38 | --------------------------------------------------------------------------------