├── .gitignore ├── Capers_II_PS2_SSC32 ├── Capers_II.h ├── Capers_II_Code.h ├── Capers_II_Driver_SSC32.h ├── Capers_II_Input_PS2.h └── Capers_II_PS2_SSC32.ino ├── Controls ├── blank controller.psd ├── common controls.png ├── common controls.psd ├── single leg controls.png ├── single leg controls.psd ├── walk controls.png └── walk controls.psd ├── ReadMe.md ├── Wiring diagrams ├── Controller connections.jpg ├── Jumper Diagrams │ ├── 5V VS jumpers 2.jpg │ ├── 5V VS jumpers.jpg │ ├── RT TX power jumper.jpg │ ├── VS VL jumper.jpg │ ├── button jumpers.jpg │ ├── ext usb jumper.jpg │ └── speaker jumper.jpg └── electronics wiring.jpg └── servo_center └── servo_center.ino /.gitignore: -------------------------------------------------------------------------------- 1 | Images/ 2 | Intro Video/ 3 | .gitignore 4 | -------------------------------------------------------------------------------- /Capers_II_PS2_SSC32/Capers_II.h: -------------------------------------------------------------------------------- 1 | // Project: Capers II Hexapod 2 | // Author: Toglefritz 3 | // 4 | // Attribution: 5 | // Programmer: Jeroen Janssen [aka Xan] 6 | // Kurt Eckhardt(KurtE) converted to C and Arduino 7 | // Kare Halvorsen aka Zenta - Makes everything work correctly! 8 | 9 | 10 | #ifndef _PHOENIX_CORE_H_ 11 | #define _PHOENIX_CORE_H_ 12 | #include 13 | //#include 14 | #if defined(__SAM3X8E__) 15 | #define PROGMEM 16 | #define pgm_read_byte(x) (*((char *)x)) 17 | // #define pgm_read_word(x) (*((short *)(x & 0xfffffffe))) 18 | #define pgm_read_word(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))) 19 | #define pgm_read_byte_near(x) (*((char *)x)) 20 | #define pgm_read_byte_far(x) (*((char *)x)) 21 | // #define pgm_read_word_near(x) (*((short *)(x & 0xfffffffe)) 22 | // #define pgm_read_word_far(x) (*((short *)(x & 0xfffffffe))) 23 | #define pgm_read_word_near(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x))) 24 | #define pgm_read_word_far(x) ( ((*((unsigned char *)x + 1)) << 8) + (*((unsigned char *)x)))) 25 | #define PSTR(x) x 26 | #endif 27 | 28 | #ifdef USEXBEE 29 | #include "diyxbee.h" 30 | #endif 31 | 32 | //============================================================================= 33 | //[CONSTANTS] 34 | //============================================================================= 35 | #define BUTTON_DOWN 0 36 | #define BUTTON_UP 1 37 | 38 | #define c1DEC 10 39 | #define c2DEC 100 40 | #define c4DEC 10000 41 | #define c6DEC 1000000 42 | 43 | #ifdef QUADMODE 44 | enum { 45 | cRR=0, cRF, cLR, cLF, CNT_LEGS}; 46 | #else 47 | enum { 48 | cRR=0, cRM, cRF, cLR, cLM, cLF, CNT_LEGS}; 49 | #endif 50 | 51 | #define WTIMERTICSPERMSMUL 64 // BAP28 is 16mhz need a multiplyer and divider to make the conversion with /8192 52 | #define WTIMERTICSPERMSDIV 125 // 53 | #define USEINT_TIMERAV 54 | 55 | 56 | // BUGBUG: to make Dynamic first pass simpl make it a variable. 57 | extern byte NUM_GAITS; 58 | #define SmDiv 4 //"Smooth division" factor for the smooth control function, a value of 3 to 5 is most suitable 59 | extern void GaitSelect(void); 60 | extern short SmoothControl (short CtrlMoveInp, short CtrlMoveOut, byte CtrlDivider); 61 | 62 | 63 | 64 | //----------------------------------------------------------------------------- 65 | // Define Global variables 66 | //----------------------------------------------------------------------------- 67 | extern boolean g_fDebugOutput; 68 | extern boolean g_fEnableServos; // Hack to allow me to turn servo processing off... 69 | extern boolean g_fRobotUpsideDown; // Is the robot upside down? 70 | 71 | 72 | extern void MSound(byte cNotes, ...); 73 | extern boolean CheckVoltage(void); 74 | 75 | extern word GetLegsXZLength(void); 76 | extern void AdjustLegPositions(word XZLength1); 77 | extern void AdjustLegPositionsToBodyHeight(); 78 | extern void ResetLegInitAngles(void); 79 | extern void RotateLegInitAngles (int iDeltaAngle); 80 | extern long GetCmdLineNum(byte **ppszCmdLine); 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 | #ifdef OPT_TERMINAL_MONITOR_IC // Allow Input controller to define stuff as well 133 | void ShowTerminalCommandList(void); 134 | boolean ProcessTerminalCommand(byte *psz, byte bLen); 135 | #endif 136 | 137 | private: 138 | } 139 | ; 140 | 141 | // Define a function that allows us to define which controllers are to be used. 142 | extern void RegisterInputController(InputController *pic); 143 | 144 | 145 | 146 | typedef struct _Coord3D { 147 | long x; 148 | long y; 149 | long z; 150 | } 151 | COORD3D; 152 | 153 | //============================================================================== 154 | // Define Gait structure/class - Hopefully allow specific robots to define their 155 | // own gaits and/or define which of the standard ones they want. 156 | //============================================================================== 157 | typedef struct _PhoenixGait { 158 | short NomGaitSpeed; //Nominal speed of the gait 159 | byte StepsInGait; //Number of steps in gait 160 | byte NrLiftedPos; //Number of positions that a single leg is lifted [1-3] 161 | byte FrontDownPos; //Where the leg should be put down to ground 162 | byte LiftDivFactor; //Normaly: 2, when NrLiftedPos=5: 4 163 | byte TLDivFactor; //Number of steps that a leg is on the floor while walking 164 | byte HalfLiftHeight; // How high to lift at halfway up. 165 | 166 | #ifdef QUADMODE 167 | // Extra information used in the Quad balance mode 168 | word COGAngleStart1; // COG shifting starting angle 169 | word COGAngleStep1; // COG Angle Steps in degrees 170 | byte COGRadius; // COG Radius; the amount the body shifts 171 | boolean COGCCW; // COG Gait sequence runs counter clock wise 172 | #endif 173 | byte GaitLegNr[CNT_LEGS]; //Init position of the leg 174 | #ifdef DISPLAY_GAIT_NAMES 175 | PGM_P pszName; // The gait name 176 | #endif 177 | } 178 | PHOENIXGAIT; 179 | 180 | #ifdef DISPLAY_GAIT_NAMES 181 | #define GAITNAME(name) ,name 182 | #else 183 | #define GAITNAME(name) 184 | #endif 185 | 186 | //============================================================================== 187 | // class ControlState: This is the main structure of data that the Control 188 | // manipulates and is used by the main Phoenix Code to make it do what is 189 | // requested. 190 | //============================================================================== 191 | typedef struct _InControlState { 192 | boolean fRobotOn; //Switch to turn on Phoenix 193 | boolean fPrev_RobotOn; //Previous loop state 194 | //Body position 195 | COORD3D BodyPos; 196 | COORD3D BodyRotOffset; // Body rotation offset; 197 | 198 | //Body Inverse Kinematics 199 | COORD3D BodyRot1; // X -Pitch, Y-Rotation, Z-Roll 200 | 201 | //[gait] 202 | byte GaitType; //Gait type 203 | byte GaitStep; //Actual current step in gait 204 | PHOENIXGAIT gaitCur; // Definition of the current gait 205 | 206 | short LegLiftHeight; //Current Travel height 207 | COORD3D TravelLength; // X-Z or Length, Y is rotation. 208 | 209 | #ifdef cTurretRotPin 210 | // Turret information 211 | int TurretRotAngle1; // Rotation of turrent in 10ths of degree 212 | int TurretTiltAngle1; // the tile for the turret 213 | #endif 214 | 215 | //[Single Leg Control] 216 | #ifdef OPT_SINGLELEG 217 | byte SelectedLeg; 218 | COORD3D SLLeg; // 219 | boolean fSLHold; //Single leg control mode 220 | #endif 221 | 222 | //[Balance] 223 | boolean BalanceMode; 224 | 225 | //[TIMING] 226 | byte InputTimeDelay; //Delay that depends on the input to get the "sneaking" effect 227 | word SpeedControl; //Adjustible Delay 228 | byte ForceGaitStepCnt; // new to allow us to force a step even when not moving 229 | 230 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 231 | short aCoxaInitAngle1[CNT_LEGS]; 232 | #endif 233 | 234 | // 235 | 236 | } 237 | INCONTROLSTATE; 238 | 239 | //============================================================================== 240 | //============================================================================== 241 | // Define the class(s) for Servo Drivers. 242 | //============================================================================== 243 | //============================================================================== 244 | class ServoDriver { 245 | public: 246 | void Init(void); 247 | 248 | uint16_t GetBatteryVoltage(void); 249 | 250 | #ifdef OPT_GPPLAYER 251 | inline boolean FIsGPEnabled(void) { 252 | return _fGPEnabled; 253 | }; 254 | boolean FIsGPSeqDefined(uint8_t iSeq); 255 | inline boolean FIsGPSeqActive(void) { 256 | return _fGPActive; 257 | }; 258 | void GPStartSeq(uint8_t iSeq); // 0xff - says to abort... 259 | void GPPlayer(void); 260 | uint8_t GPNumSteps(void); // How many steps does the current sequence have 261 | uint8_t GPCurStep(void); // Return which step currently on... 262 | void GPSetSpeedMultiplyer(short sm) ; // Set the Speed multiplier (100 is default) 263 | #endif 264 | void BeginServoUpdate(void); // Start the update 265 | #ifdef c4DOF 266 | void OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1, short sTarsAngle1); 267 | #else 268 | void OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1); 269 | #endif 270 | #ifdef cTurretRotPin 271 | void OutputServoInfoForTurret(short sRotateAngle1, short sTiltAngle1); 272 | #endif 273 | void CommitServoDriver(word wMoveTime); 274 | void FreeServos(void); 275 | 276 | void IdleTime(void); // called when the main loop when the robot is not on 277 | 278 | // Allow for background process to happen... 279 | #ifdef OPT_BACKGROUND_PROCESS 280 | void BackgroundProcess(void); 281 | #endif 282 | 283 | #ifdef OPT_TERMINAL_MONITOR 284 | void ShowTerminalCommandList(void); 285 | boolean ProcessTerminalCommand(byte *psz, byte bLen); 286 | #endif 287 | 288 | private: 289 | 290 | #ifdef OPT_GPPLAYER 291 | boolean _fGPEnabled; // IS GP defined for this servo driver? 292 | boolean _fGPActive; // Is a sequence currently active - May change later when we integrate in sequence timing adjustment code 293 | uint8_t _iSeq; // current sequence we are running 294 | short _sGPSM; // Speed multiplier +-200 295 | #endif 296 | 297 | } 298 | ; 299 | 300 | //============================================================================== 301 | //============================================================================== 302 | // Define global class objects 303 | //============================================================================== 304 | //============================================================================== 305 | extern ServoDriver g_ServoDriver; // our global servo driver class 306 | extern InputController g_InputController; // Our Input controller 307 | extern INCONTROLSTATE g_InControlState; // State information that controller changes 308 | 309 | 310 | #endif 311 | 312 | -------------------------------------------------------------------------------- /Capers_II_PS2_SSC32/Capers_II_Code.h: -------------------------------------------------------------------------------- 1 | // Project: Capers II Hexapod 2 | // Author: Toglefritz 3 | 4 | /* 5 | Controlling hexapod robots requires the use of a type of mathematics called inverse kinematics. For an introduction 6 | to inverse kinematics for walking robots, visit http://toglefritz.com/inverse-kinematics-for-walking-robots-an-introduction/. 7 | This file contains all of the actual math used to control the robot. The inverse kinematics calculations follow a process 8 | 9 | * The inverse kinematics calculations start with direct inputs from the robot designer. These inputs include information 10 | about the physical construction of the robot including the dimensions for the legs and the robot body, and constraints 11 | on the range of motion for the servos. We also have control inputs which are provided by the PS2 driver code. 12 | * The actual inverse kinematics calculations begin with calculations based on the position and orientation of the body. 13 | * From the body inverse kinematics calculations, do inverse kinematics for each of the six legs. These calculations result 14 | in angles for all 18 leg servos on the robot. 15 | * Check the servo angles from the inverse kinematics calculations against the mechanical limits of the servos based on the 16 | physical design of the robot. These limits are defined in the robot configuration. 17 | * Move the servos. 18 | * Repeat the inverse kinematics calculations again for the next loop. 19 | */ 20 | 21 | #define DEFINE_HEX_GLOBALS 22 | #if ARDUINO>99 23 | #include 24 | #else 25 | #endif 26 | #include 27 | #define BalanceDivFactor CNT_LEGS 28 | 29 | // Only compile in Debug code if we have something to output to 30 | #ifdef DBGSerial 31 | #define DEBUG 32 | //#define DEBUG_X 33 | #endif 34 | 35 | 36 | // //////////// TRIG TABLES //////////// 37 | // The inverse kinematics used to control the robot use a whole bunch of trignometric functions. The trig 38 | // functions are quite demanding for a low power processor like the one on an Arduino. Therefore, in order to 39 | // improve performance, a table of pre-computed trig values is used instead of actually doing the trig functions. 40 | // The code will look up values in the tables below rather than performing the trig functions. 41 | 42 | // ArcCosinus Table 43 | // The biggest error is near cos = 1 and has a biggest value of 3*0.012098rad = 0.521 deg. 44 | //- Cos 0 to 0.9 is done by steps of 0.0079 rad. [1/127] 45 | //- Cos 0.9 to 0.99 is done by steps of 0.0008 rad [0.1/127] 46 | //- Cos 0.99 to 1 is done by step of 0.0002 rad [0.01/64] 47 | // Since the tables are overlapping the full range of 127+127+64 is not necessary. Total bytes: 277 48 | 49 | static const byte GetACos[] PROGMEM = { 50 | 255,254,252,251,250,249,247,246,245,243,242,241,240,238,237,236,234,233,232,231,229,228,227,225, 51 | 224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,201,200,199,197,196,195,193, 52 | 192,190,189,188,186,185,183,182,181,179,178,176,175,173,172,170,169,167,166,164,163,161,160,158, 53 | 157,155,154,152,150,149,147,146,144,142,141,139,137,135,134,132,130,128,127,125,123,121,119,117, 54 | 115,113,111,109,107,105,103,101,98,96,94,92,89,87,84,81,79,76,73,73,73,72,72,72,71,71,71,70,70, 55 | 70,70,69,69,69,68,68,68,67,67,67,66,66,66,65,65,65,64,64,64,63,63,63,62,62,62,61,61,61,60,60,59, 56 | 59,59,58,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,47, 57 | 46,46,45,45,44,44,43,43,42,42,41,41,40,40,39,39,38,37,37,36,36,35,34,34,33,33,32,31,31,30,29,28, 58 | 28,27,26,25,24,23,23,23,23,22,22,22,22,21,21,21,21,20,20,20,19,19,19,19,18,18,18,17,17,17,17,16, 59 | 16,16,15,15,15,14,14,13,13,13,12,12,11,11,10,10,9,9,8,7,6,6,5,3,0 };// 60 | 61 | // Sin table 90 deg, persision 0.5 deg [180 values] 62 | static const word GetSin[] PROGMEM = { 63 | 0, 87, 174, 261, 348, 436, 523, 610, 697, 784, 871, 958, 1045, 1132, 1218, 1305, 1391, 1478, 1564, 64 | 1650, 1736, 1822, 1908, 1993, 2079, 2164, 2249, 2334, 2419, 2503, 2588, 2672, 2756, 2840, 2923, 3007, 65 | 3090, 3173, 3255, 3338, 3420, 3502, 3583, 3665, 3746, 3826, 3907, 3987, 4067, 4146, 4226, 4305, 4383, 66 | 4461, 4539, 4617, 4694, 4771, 4848, 4924, 4999, 5075, 5150, 5224, 5299, 5372, 5446, 5519, 5591, 5664, 67 | 5735, 5807, 5877, 5948, 6018, 6087, 6156, 6225, 6293, 6360, 6427, 6494, 6560, 6626, 6691, 6755, 6819, 68 | 6883, 6946, 7009, 7071, 7132, 7193, 7253, 7313, 7372, 7431, 7489, 7547, 7604, 7660, 7716, 7771, 7826, 69 | 7880, 7933, 7986, 8038, 8090, 8141, 8191, 8241, 8290, 8338, 8386, 8433, 8480, 8526, 8571, 8616, 8660, 70 | 8703, 8746, 8788, 8829, 8870, 8910, 8949, 8987, 9025, 9063, 9099, 9135, 9170, 9205, 9238, 9271, 9304, 71 | 9335, 9366, 9396, 9426, 9455, 9483, 9510, 9537, 9563, 9588, 9612, 9636, 9659, 9681, 9702, 9723, 9743, 72 | 9762, 9781, 9799, 9816, 9832, 9848, 9862, 9876, 9890, 9902, 9914, 9925, 9935, 9945, 9953, 9961, 9969, 73 | 9975, 9981, 9986, 9990, 9993, 9996, 9998, 9999, 10000 };// 74 | 75 | 76 | // Build tables for Leg configuration like I/O and MIN/ Max values to easy access values using a FOR loop 77 | // Constants are still defined as single values in the cfg file to make it easy to read/configure 78 | 79 | // //////////// SERVO INVERT //////////// 80 | // The block of definitions below establishes which of the servos will operated in an inverted direction 81 | #ifndef cRRCoxaInv 82 | #define cRRCoxaInv 1 83 | #endif 84 | #ifndef cRMCoxaInv 85 | #define cRMCoxaInv 1 86 | #endif 87 | #ifndef cRFCoxaInv 88 | #define cRFCoxaInv 1 89 | #endif 90 | 91 | #ifndef cLRCoxaInv 92 | #define cLRCoxaInv 0 93 | #endif 94 | #ifndef cLMCoxaInv 95 | #define cLMCoxaInv 0 96 | #endif 97 | #ifndef cLFCoxaInv 98 | #define cLFCoxaInv 0 99 | #endif 100 | 101 | #ifndef cRRFemurInv 102 | #define cRRFemurInv 1 103 | #endif 104 | #ifndef cRMFemurInv 105 | #define cRMFemurInv 1 106 | #endif 107 | #ifndef cRFFemurInv 108 | #define cRFFemurInv 1 109 | #endif 110 | 111 | #ifndef cLRFemurInv 112 | #define cLRFemurInv 0 113 | #endif 114 | #ifndef cLMFemurInv 115 | #define cLMFemurInv 0 116 | #endif 117 | #ifndef cLFFemurInv 118 | #define cLFFemurInv 0 119 | #endif 120 | 121 | #ifndef cRRTibiaInv 122 | #define cRRTibiaInv 1 123 | #endif 124 | #ifndef cRMTibiaInv 125 | #define cRMTibiaInv 1 126 | #endif 127 | #ifndef cRFTibiaInv 128 | #define cRFTibiaInv 1 129 | #endif 130 | 131 | #ifndef cLRTibiaInv 132 | #define cLRTibiaInv 0 133 | #endif 134 | #ifndef cLMTibiaInv 135 | #define cLMTibiaInv 0 136 | #endif 137 | #ifndef cLFTibiaInv 138 | #define cLFTibiaInv 0 139 | #endif 140 | 141 | #ifndef cRRTarsInv 142 | #define cRRTarsInv 1 143 | #endif 144 | #ifndef cRMTarsInv 145 | #define cRMTarsInv 1 146 | #endif 147 | #ifndef cRFTarsInv 148 | #define cRFTarsInv 1 149 | #endif 150 | 151 | #ifndef cLRTarsInv 152 | #define cLRTarsInv 0 153 | #endif 154 | #ifndef cLMTarsInv 155 | #define cLMTarsInv 0 156 | #endif 157 | #ifndef cLFTarsInv 158 | #define cLFTarsInv 0 159 | #endif 160 | 161 | // Also define default BalanceDelay 162 | #ifndef BALANCE_DELAY 163 | #define BALANCE_DELAY 100 164 | #endif 165 | 166 | 167 | // //////////// SERVO HORN OFFSETS //////////// 168 | // Standard Hexapod... 169 | // Servo Horn offsets 170 | #ifdef cRRFemurHornOffset1 // per leg configuration 171 | static const short cFemurHornOffset1[] PROGMEM = { 172 | cRRFemurHornOffset1, cRMFemurHornOffset1, cRFFemurHornOffset1, cLRFemurHornOffset1, cLMFemurHornOffset1, cLFFemurHornOffset1}; 173 | #define CFEMURHORNOFFSET1(LEGI) ((short)pgm_read_word(&cFemurHornOffset1[LEGI])) 174 | #else // Fixed per leg, if not defined 0 175 | #ifndef cFemurHornOffset1 176 | #define cFemurHornOffset1 0 177 | #endif 178 | #define CFEMURHORNOFFSET1(LEGI) (cFemurHornOffset1) 179 | #endif 180 | 181 | #ifdef cRRTibiaHornOffset1 // per leg configuration 182 | static const short cTibiaHornOffset1[] PROGMEM = { 183 | cRRTibiaHornOffset1, cRMTibiaHornOffset1, cRFTibiaHornOffset1, cLRTibiaHornOffset1, cLMTibiaHornOffset1, cLFTibiaHornOffset1}; 184 | #define CTIBIAHORNOFFSET1(LEGI) ((short)pgm_read_word(&cTibiaHornOffset1[LEGI])) 185 | #else // Fixed per leg, if not defined 0 186 | #ifndef cTibiaHornOffset1 187 | #define cTibiaHornOffset1 0 188 | #endif 189 | #define CTIBIAHORNOFFSET1(LEGI) (cTibiaHornOffset1) 190 | #endif 191 | 192 | 193 | // /////////////////////////////////////////////////////////////////////////////// 194 | // ////////////////////////// CONFIGURATION INFO /////////////////////////// 195 | // ////////////////////////////////////////////////////////////////////////////// 196 | // The Capers_II_PS2_SSC32 file contains configuration options for the robot. This section 197 | // of the code takes data from the robot configuration and sets it up for use by the rest 198 | // of the code. 199 | 200 | 201 | // ////////////////// MIN, MAX VALUES ////////////////// 202 | // The robot configuration contains the ability to input bounds on the range of motion 203 | // for the servos so that the code cannot drive the servos farther than the physical 204 | // construction of the robot allows. These values will be put into tables for the code 205 | // to access them later. 206 | 207 | #ifndef SERVOS_DO_MINMAX 208 | const short cCoxaMin1[] PROGMEM = { 209 | cRRCoxaMin1, cRMCoxaMin1, cRFCoxaMin1, cLRCoxaMin1, cLMCoxaMin1, cLFCoxaMin1}; 210 | const short cCoxaMax1[] PROGMEM = { 211 | cRRCoxaMax1, cRMCoxaMax1, cRFCoxaMax1, cLRCoxaMax1, cLMCoxaMax1, cLFCoxaMax1}; 212 | const short cFemurMin1[] PROGMEM ={ 213 | cRRFemurMin1, cRMFemurMin1, cRFFemurMin1, cLRFemurMin1, cLMFemurMin1, cLFFemurMin1}; 214 | const short cFemurMax1[] PROGMEM ={ 215 | cRRFemurMax1, cRMFemurMax1, cRFFemurMax1, cLRFemurMax1, cLMFemurMax1, cLFFemurMax1}; 216 | const short cTibiaMin1[] PROGMEM ={ 217 | cRRTibiaMin1, cRMTibiaMin1, cRFTibiaMin1, cLRTibiaMin1, cLMTibiaMin1, cLFTibiaMin1}; 218 | const short cTibiaMax1[] PROGMEM = { 219 | cRRTibiaMax1, cRMTibiaMax1, cRFTibiaMax1, cLRTibiaMax1, cLMTibiaMax1, cLFTibiaMax1}; 220 | #endif 221 | 222 | // Servo inverse direction 223 | const bool cCoxaInv[] = {cRRCoxaInv, cRMCoxaInv, cRFCoxaInv, cLRCoxaInv, cLMCoxaInv, cLFCoxaInv}; 224 | bool cFemurInv[] = {cRRFemurInv, cRMFemurInv, cRFFemurInv, cLRFemurInv, cLMFemurInv, cLFFemurInv}; 225 | const bool cTibiaInv[] = {cRRTibiaInv, cRMTibiaInv, cRFTibiaInv, cLRTibiaInv, cLMTibiaInv, cLFTibiaInv}; 226 | 227 | // ////////////////// LEG LENGTHS ////////////////// 228 | // In the robot configuration, the lengths of each segment of each leg are defined. For easy access 229 | // by the rest of the code, these values will be placed into a table. 230 | 231 | const byte cCoxaLength[] PROGMEM = { 232 | cRRCoxaLength, cRMCoxaLength, cRFCoxaLength, cLRCoxaLength, cLMCoxaLength, cLFCoxaLength}; 233 | const byte cFemurLength[] PROGMEM = { 234 | cRRFemurLength, cRMFemurLength, cRFFemurLength, cLRFemurLength, cLMFemurLength, cLFFemurLength}; 235 | const byte cTibiaLength[] PROGMEM = { 236 | cRRTibiaLength, cRMTibiaLength, cRFTibiaLength, cLRTibiaLength, cLMTibiaLength, cLFTibiaLength}; 237 | 238 | // ////////////////// BODY OFFSETS ////////////////// 239 | // In the robot configuration, the physical dimensions of the body are represented 240 | // as offsets between the center of the body and the coxa servo horn (this is the 241 | // first joint on each leg). This section takes the offset values and places those 242 | // values into tables for the rest of the code to access. 243 | 244 | const short cOffsetX[] PROGMEM = { 245 | cRROffsetX, cRMOffsetX, cRFOffsetX, cLROffsetX, cLMOffsetX, cLFOffsetX}; 246 | const short cOffsetZ[] PROGMEM = { 247 | cRROffsetZ, cRMOffsetZ, cRFOffsetZ, cLROffsetZ, cLMOffsetZ, cLFOffsetZ}; 248 | 249 | // Default leg angle 250 | const short cCoxaAngle1[] PROGMEM = { 251 | cRRCoxaAngle1, cRMCoxaAngle1, cRFCoxaAngle1, cLRCoxaAngle1, cLMCoxaAngle1, cLFCoxaAngle1}; 252 | 253 | #ifdef cRRInitCoxaAngle1 // We can set different angles for the legs than just where they servo horns are set... 254 | const short cCoxaInitAngle1[] PROGMEM = { 255 | cRRInitCoxaAngle1, cRMInitCoxaAngle1, cRFInitCoxaAngle1, cLRInitCoxaAngle1, cLMInitCoxaAngle1, cLFInitCoxaAngle1}; 256 | #endif 257 | 258 | // ////////////////// LEG START POSITIONS ////////////////// 259 | // The start positions for each leg are defined in the robot configuration. Place these initial position 260 | // values into a table for the rest of this code to access. 261 | 262 | const short cInitPosX[] PROGMEM = { 263 | cRRInitPosX, cRMInitPosX, cRFInitPosX, cLRInitPosX, cLMInitPosX, cLFInitPosX}; 264 | const short cInitPosY[] PROGMEM = { 265 | cRRInitPosY, cRMInitPosY, cRFInitPosY, cLRInitPosY, cLMInitPosY, cLFInitPosY}; 266 | const short cInitPosZ[] PROGMEM = { 267 | cRRInitPosZ, cRMInitPosZ, cRFInitPosZ, cLRInitPosZ, cLMInitPosZ, cLFInitPosZ}; 268 | 269 | // Define some globals for debug information 270 | boolean g_fShowDebugPrompt; 271 | boolean g_fDebugOutput; 272 | boolean g_fEnableServos = true; 273 | 274 | 275 | // /////////////////////////////////////////////////////////////////////////// 276 | // ////////////////////////// CONTROLLER INFO /////////////////////////// 277 | // /////////////////////////////////////////////////////////////////////////// 278 | #define cTravelDeadZone 4 //The deadzone for the analog input from the remote 279 | //[ANGLES] 280 | short CoxaAngle1[CNT_LEGS]; //Actual Angle of the horizontal hip, decimals = 1 281 | short FemurAngle1[CNT_LEGS]; //Actual Angle of the vertical hip, decimals = 1 282 | short TibiaAngle1[CNT_LEGS]; //Actual Angle of the knee, decimals = 1 283 | #ifdef c4DOF 284 | short TarsAngle1[CNT_LEGS]; //Actual Angle of the knee, decimals = 1 285 | #endif 286 | 287 | //[POSITIONS SINGLE LEG CONTROL] 288 | 289 | short LegPosX[CNT_LEGS]; //Actual X Posion of the Leg 290 | short LegPosY[CNT_LEGS]; //Actual Y Posion of the Leg 291 | short LegPosZ[CNT_LEGS]; //Actual Z Posion of the Leg 292 | 293 | //[OUTPUTS] 294 | boolean LedA; //Red 295 | boolean LedB; //Green 296 | boolean LedC; //Orange 297 | boolean Eyes; //Eyes output 298 | 299 | //[VARIABLES] 300 | byte Index; //Index universal used 301 | byte LegIndex; //Index used for leg Index Number 302 | 303 | //GetSinCos / ArcCos 304 | short AngleDeg1; //Input Angle in degrees, decimals = 1 305 | short sin4; //Output Sinus of the given Angle, decimals = 4 306 | short cos4; //Output Cosinus of the given Angle, decimals = 4 307 | short AngleRad4; //Output Angle in radials, decimals = 4 308 | 309 | //GetAtan2 310 | short AtanX; //Input X 311 | short AtanY; //Input Y 312 | short Atan4; //ArcTan2 output 313 | long XYhyp2; //Output presenting Hypotenuse of X and Y 314 | 315 | //Body Inverse Kinematics 316 | short PosX; //Input position of the feet X 317 | short PosZ; //Input position of the feet Z 318 | short PosY; //Input position of the feet Y 319 | long BodyFKPosX; //Output Position X of feet with Rotation 320 | long BodyFKPosY; //Output Position Y of feet with Rotation 321 | long BodyFKPosZ; //Output Position Z of feet with Rotation 322 | 323 | 324 | //Leg Inverse Kinematics 325 | long IKFeetPosX; //Input position of the Feet X 326 | long IKFeetPosY; //Input position of the Feet Y 327 | long IKFeetPosZ; //Input Position of the Feet Z 328 | boolean IKSolution; //Output true if the solution is possible 329 | boolean IKSolutionWarning; //Output true if the solution is NEARLY possible 330 | boolean IKSolutionError; //Output true if the solution is NOT possible 331 | 332 | //[TIMING] 333 | unsigned long lTimerStart; //Start time of the calculation cycles 334 | unsigned long lTimerEnd; //End time of the calculation cycles 335 | byte CycleTime; //Total Cycle time 336 | 337 | word ServoMoveTime; //Time for servo updates 338 | word PrevServoMoveTime; //Previous time for the servo updates 339 | 340 | // Define our global Input Control State object 341 | INCONTROLSTATE g_InControlState; // This is our global Input control state object... 342 | 343 | // Define our ServoWriter class 344 | ServoDriver g_ServoDriver; // our global servo driver class 345 | 346 | boolean g_fLowVoltageShutdown; // If set the bot shuts down because the input voltage is to low 347 | uint16_t Voltage; 348 | 349 | //[Balance] 350 | long TotalTransX; 351 | long TotalTransZ; 352 | long TotalTransY; 353 | long TotalYBal1; 354 | long TotalXBal1; 355 | long TotalZBal1; 356 | 357 | //[Single Leg Control] 358 | byte PrevSelectedLeg; 359 | boolean AllDown; 360 | 361 | boolean TravelRequest; // Temp to check if the gait is in motion 362 | 363 | long GaitPosX[CNT_LEGS]; // Array containing Relative X position corresponding to the Gait 364 | long GaitPosY[CNT_LEGS]; // Array containing Relative Y position corresponding to the Gait 365 | long GaitPosZ[CNT_LEGS]; // Array containing Relative Z position corresponding to the Gait 366 | long GaitRotY[CNT_LEGS]; // Array containing Relative Y rotation corresponding to the Gait 367 | 368 | 369 | boolean fWalking; // True if the robot are walking 370 | byte bExtraCycle; // Forcing some extra timed cycles for avoiding "end of gait bug" 371 | #define cGPlimit 2 // GP=GaitPos testing different limits 372 | 373 | boolean g_fRobotUpsideDown; // Is the robot upside down? 374 | boolean fRobotUpsideDownPrev; 375 | 376 | 377 | // //////////////////////////////////////////////////////////////////////////// 378 | // ////////////////////////// GAIT INFORMATION /////////////////////////// 379 | // //////////////////////////////////////////////////////////////////////////// 380 | 381 | #ifndef DEFAULT_GAIT_SPEED 382 | #define DEFAULT_GAIT_SPEED 50 383 | #define DEFAULT_SLOW_GAIT 70 384 | #endif 385 | 386 | #ifndef OVERWRITE_GAITS 387 | #ifndef QUADMODE 388 | // Speed, Steps, Lifted, Front Down, Lifted Factor, Half Height, On Ground, 389 | // { RR, RF, LR, , LF} 390 | #ifdef DISPLAY_GAIT_NAMES 391 | extern "C" { 392 | // Move the Gait Names to program space... 393 | const char s_szGN1[] PROGMEM = "Ripple 12"; 394 | const char s_szGN2[] PROGMEM = "Tripod 8"; 395 | const char s_szGN3[] PROGMEM = "Tripple 12"; 396 | const char s_szGN4[] PROGMEM = "Tripple 16"; 397 | const char s_szGN5[] PROGMEM = "Wave 24"; 398 | const char s_szGN6[] PROGMEM = "Tripod 6"; 399 | }; 400 | #endif 401 | 402 | PHOENIXGAIT APG[] = { 403 | {DEFAULT_SLOW_GAIT, 12, 3, 2, 2, 8, 3, {7, 11, 3, 1, 5, 9} GAITNAME(s_szGN1)}, // Ripple 12 404 | {DEFAULT_SLOW_GAIT, 8, 3, 2, 2, 4, 3, {1, 5, 1, 5, 1, 5} GAITNAME(s_szGN2)}, //Tripod 8 steps 405 | {DEFAULT_GAIT_SPEED, 12, 3, 2, 2, 8, 3, {5, 10, 3, 11, 4, 9} GAITNAME(s_szGN3) }, //Triple Tripod 12 step 406 | {DEFAULT_GAIT_SPEED, 16, 5, 3, 4, 10, 1, {6, 13, 4, 14, 5, 12} GAITNAME(s_szGN4)}, // Triple Tripod 16 steps, use 5 lifted positions 407 | {DEFAULT_SLOW_GAIT, 24, 3, 2, 2, 20, 3, {13, 17, 21, 1, 5, 9} GAITNAME(s_szGN5)}, //Wave 24 steps 408 | {DEFAULT_GAIT_SPEED, 6, 2, 1, 2, 4, 1, {1, 4, 1, 4, 1, 4} GAITNAME(s_szGN6)} //Tripod 6 steps 409 | }; 410 | 411 | #else 412 | #ifdef DISPLAY_GAIT_NAMES 413 | extern "C" { 414 | // Move the Gait Names to program space... 415 | const char s_szGN1[] PROGMEM = "Ripple 12"; 416 | const char s_szGN2[] PROGMEM = "Tripod 8"; 417 | } 418 | #endif 419 | PHOENIXGAIT APG[] = { 420 | {DEFAULT_GAIT_SPEED, 16, 3, 2, 2, 12, 3, 2250, 3600/16, 30, true, {5, 9, 1, 13} GAITNAME(s_szGN1)}, // Wave 16 421 | {1, 28, 3, 2, 2, 24, 3, 2250, 3600/28, 30, true, {8, 15, 1, 22} GAITNAME(s_szGN2)} // Wave 28? 422 | }; 423 | 424 | #endif 425 | #endif 426 | 427 | #ifdef ADD_GAITS 428 | byte NUM_GAITS = sizeof(APG)/sizeof(APG[0]) + sizeof(APG_EXTRA)/sizeof(APG_EXTRA[0]); 429 | #else 430 | byte NUM_GAITS = sizeof(APG)/sizeof(APG[0]); 431 | #endif 432 | 433 | 434 | 435 | // ///////////////////////////////////////////////////////////////////// 436 | // ////////////////////////// FUNCTIONS /////////////////////////// 437 | // ///////////////////////////////////////////////////////////////////// 438 | extern void GaitSelect(void); 439 | extern void WriteOutputs(void); 440 | extern void SingleLegControl(void); 441 | extern void GaitSeq(void); 442 | extern void BalanceBody(void); 443 | extern void CheckAngles(); 444 | 445 | extern void PrintSystemStuff(void); 446 | 447 | extern void BalCalcOneLeg (long PosX, long PosZ, long PosY, byte BalLegNr); 448 | extern void BodyFK (short PosX, short PosZ, short PosY, short RotationY, byte BodyIKLeg) ; 449 | extern void LegIK (short IKFeetPosX, short IKFeetPosY, short IKFeetPosZ, byte LegIKLegNr); 450 | extern void Gait (byte GaitCurrentLegNr); 451 | extern void GetSinCos(short AngleDeg1); 452 | extern short GetATan2 (short AtanX, short AtanY); 453 | extern unsigned long isqrt32 (unsigned long n); 454 | 455 | extern void StartUpdateServos(void); 456 | extern boolean TerminalMonitor(void); 457 | 458 | 459 | //-------------------------------------------------------------------------- 460 | // SETUP: The main arduino setup function. 461 | //-------------------------------------------------------------------------- 462 | 463 | void setup(){ 464 | #ifdef OPT_SKETCHSETUP 465 | SketchSetup(); 466 | #endif 467 | g_fShowDebugPrompt = true; 468 | g_fDebugOutput = false; 469 | #ifdef DBGSerial 470 | DBGSerial.begin(38400); 471 | #endif 472 | // Init our ServoDriver 473 | g_ServoDriver.Init(); 474 | 475 | //Checks to see if our Servo Driver support a GP Player 476 | // DBGSerial.write("Program Start\n\r"); 477 | // debug stuff 478 | delay(10); 479 | 480 | 481 | //Turning off all the leds 482 | LedA = 0; 483 | LedB = 0; 484 | LedC = 0; 485 | Eyes = 0; 486 | 487 | // Setup Init Positions 488 | for (LegIndex= 0; LegIndex < CNT_LEGS; LegIndex++ ) 489 | { 490 | LegPosX[LegIndex] = (short)pgm_read_word(&cInitPosX[LegIndex]); //Set start positions for each leg 491 | LegPosY[LegIndex] = (short)pgm_read_word(&cInitPosY[LegIndex]); 492 | LegPosZ[LegIndex] = (short)pgm_read_word(&cInitPosZ[LegIndex]); 493 | } 494 | 495 | ResetLegInitAngles(); 496 | 497 | //Single leg control. Make sure no leg is selected 498 | #ifdef OPT_SINGLELEG 499 | g_InControlState.SelectedLeg = 255; // No Leg selected 500 | PrevSelectedLeg = 255; 501 | #endif 502 | // Body Positions 503 | g_InControlState.BodyPos.x = 0; 504 | g_InControlState.BodyPos.y = 0; 505 | g_InControlState.BodyPos.z = 0; 506 | 507 | // Body Rotations 508 | g_InControlState.BodyRot1.x = 0; 509 | g_InControlState.BodyRot1.y = 0; 510 | g_InControlState.BodyRot1.z = 0; 511 | g_InControlState.BodyRotOffset.x = 0; 512 | g_InControlState.BodyRotOffset.y = 0; //Input Y offset value to adjust centerpoint of rotation 513 | g_InControlState.BodyRotOffset.z = 0; 514 | 515 | 516 | //Gait 517 | g_InControlState.GaitType = 0; 518 | g_InControlState.BalanceMode = 0; 519 | g_InControlState.LegLiftHeight = 50; 520 | g_InControlState.ForceGaitStepCnt = 0; // added to try to adjust starting positions depending on height... 521 | g_InControlState.GaitStep = 1; 522 | GaitSelect(); 523 | 524 | #ifdef cTurretRotPin 525 | g_InControlState.TurretRotAngle1 = cTurretRotInit; // Rotation of turrent in 10ths of degree 526 | g_InControlState.TurretTiltAngle1 = cTurretTiltInit; // the tile for the turret 527 | #endif 528 | 529 | g_InputController.Init(); 530 | 531 | // Servo Driver 532 | ServoMoveTime = 150; 533 | g_InControlState.fRobotOn = 0; 534 | g_fLowVoltageShutdown = false; 535 | #ifdef DEBUG_IOPINS 536 | // pinMode(A0, OUTPUT); 537 | pinMode(A1, OUTPUT); 538 | pinMode(A2, OUTPUT); 539 | pinMode(A3, OUTPUT); 540 | pinMode(A4, OUTPUT); 541 | #endif 542 | #ifdef OPT_WALK_UPSIDE_DOWN 543 | g_fRobotUpsideDown = false; //Assume off... 544 | #ifdef DBGSerial 545 | DBGSerial.println(IsRobotUpsideDown, DEC); 546 | #endif 547 | #endif 548 | 549 | } 550 | 551 | 552 | //-------------------------------------------------------------------------- 553 | // Loop: the main arduino main Loop function 554 | //-------------------------------------------------------------------------- 555 | 556 | void loop(void) 557 | { 558 | // Start time 559 | unsigned long lTimeWaitEnd; 560 | lTimerStart = millis(); 561 | DoBackgroundProcess(); 562 | //Read input 563 | CheckVoltage(); // check our voltages... 564 | if (!g_fLowVoltageShutdown) { 565 | // DebugWrite(A0, HIGH); 566 | g_InputController.ControlInput(); 567 | // DebugWrite(A0, LOW); 568 | } 569 | WriteOutputs(); // Write Outputs 570 | 571 | #ifdef IsRobotUpsideDown 572 | if (!fWalking){// dont do this while walking 573 | g_fRobotUpsideDown = IsRobotUpsideDown; // Grab the current state of the robot... 574 | if (g_fRobotUpsideDown != fRobotUpsideDownPrev) { 575 | // Double check to make sure that it was not a one shot error 576 | g_fRobotUpsideDown = IsRobotUpsideDown; // Grab the current state of the robot... 577 | if (g_fRobotUpsideDown != fRobotUpsideDownPrev) { 578 | fRobotUpsideDownPrev = g_fRobotUpsideDown; 579 | #ifdef DGBSerial 580 | DBGSerial.println(fRobotUpsideDownPrev, DEC); 581 | #endif 582 | } 583 | } 584 | } 585 | // DBGSerial.println(analogRead(0), DEC); 586 | #endif 587 | #ifdef OPT_WALK_UPSIDE_DOWN 588 | if (g_fRobotUpsideDown){ 589 | g_InControlState.TravelLength.x = -g_InControlState.TravelLength.x; 590 | g_InControlState.BodyPos.x = -g_InControlState.BodyPos.x; 591 | g_InControlState.SLLeg.x = -g_InControlState.SLLeg.x; 592 | g_InControlState.BodyRot1.z = -g_InControlState.BodyRot1.z; 593 | } 594 | #endif 595 | 596 | #ifdef OPT_GPPLAYER 597 | //GP Player 598 | g_ServoDriver.GPPlayer(); 599 | if (g_ServoDriver.FIsGPSeqActive()) 600 | return; // go back to process the next message 601 | #endif 602 | 603 | // Single leg control 604 | SingleLegControl (); 605 | DoBackgroundProcess(); 606 | 607 | //Gait 608 | GaitSeq(); 609 | 610 | DoBackgroundProcess(); 611 | 612 | //Balance calculations 613 | TotalTransX = 0; //reset values used for calculation of balance 614 | TotalTransZ = 0; 615 | TotalTransY = 0; 616 | TotalXBal1 = 0; 617 | TotalYBal1 = 0; 618 | TotalZBal1 = 0; 619 | 620 | if (g_InControlState.BalanceMode) { 621 | #ifdef DEBUG 622 | if (g_fDebugOutput) { 623 | TravelRequest = (abs(g_InControlState.TravelLength.x)>cTravelDeadZone) || (abs(g_InControlState.TravelLength.z)>cTravelDeadZone) 624 | || (abs(g_InControlState.TravelLength.y)>cTravelDeadZone) || (g_InControlState.ForceGaitStepCnt != 0) || fWalking; 625 | 626 | DBGSerial.print("T("); 627 | DBGSerial.print(fWalking, DEC); 628 | DBGSerial.print(" "); 629 | DBGSerial.print(g_InControlState.TravelLength.x,DEC); 630 | DBGSerial.print(","); 631 | DBGSerial.print(g_InControlState.TravelLength.y,DEC); 632 | DBGSerial.print(","); 633 | DBGSerial.print(g_InControlState.TravelLength.z,DEC); 634 | DBGSerial.print(")"); 635 | } 636 | #endif 637 | for (LegIndex = 0; LegIndex < (CNT_LEGS/2); LegIndex++) { // balance calculations for all Right legs 638 | 639 | DoBackgroundProcess(); 640 | BalCalcOneLeg (-LegPosX[LegIndex]+GaitPosX[LegIndex], LegPosZ[LegIndex]+GaitPosZ[LegIndex], 641 | (LegPosY[LegIndex]-(short)pgm_read_word(&cInitPosY[LegIndex]))+GaitPosY[LegIndex], LegIndex); 642 | } 643 | 644 | for (LegIndex = (CNT_LEGS/2); LegIndex < CNT_LEGS; LegIndex++) { // balance calculations for all Right legs 645 | DoBackgroundProcess(); 646 | BalCalcOneLeg(LegPosX[LegIndex]+GaitPosX[LegIndex], LegPosZ[LegIndex]+GaitPosZ[LegIndex], 647 | (LegPosY[LegIndex]-(short)pgm_read_word(&cInitPosY[LegIndex]))+GaitPosY[LegIndex], LegIndex); 648 | } 649 | BalanceBody(); 650 | } 651 | 652 | 653 | // Reset IKsolution indicators 654 | IKSolution = 0 ; 655 | IKSolutionWarning = 0; 656 | IKSolutionError = 0 ; 657 | 658 | 659 | // ///////////////////////////////////////////////////////////////////////////////// 660 | // ////////////////////////// START IK CALCULATIONS /////////////////////////// 661 | // ///////////////////////////////////////////////////////////////////////////////// 662 | 663 | // Do IK for all Right legs 664 | #ifdef DEBUG 665 | if (g_fDebugOutput && g_InControlState.fRobotOn) { 666 | DBGSerial.print(g_InControlState.GaitStep,DEC); 667 | DBGSerial.print(":"); 668 | } 669 | #endif 670 | 671 | for (LegIndex = 0; LegIndex < (CNT_LEGS/2); LegIndex++) { 672 | DoBackgroundProcess(); 673 | BodyFK(-LegPosX[LegIndex]+g_InControlState.BodyPos.x+GaitPosX[LegIndex] - TotalTransX, 674 | LegPosZ[LegIndex]+g_InControlState.BodyPos.z+GaitPosZ[LegIndex] - TotalTransZ, 675 | LegPosY[LegIndex]+g_InControlState.BodyPos.y+GaitPosY[LegIndex] - TotalTransY, 676 | GaitRotY[LegIndex], LegIndex); 677 | 678 | LegIK (LegPosX[LegIndex]-g_InControlState.BodyPos.x+BodyFKPosX-(GaitPosX[LegIndex] - TotalTransX), 679 | LegPosY[LegIndex]+g_InControlState.BodyPos.y-BodyFKPosY+GaitPosY[LegIndex] - TotalTransY, 680 | LegPosZ[LegIndex]+g_InControlState.BodyPos.z-BodyFKPosZ+GaitPosZ[LegIndex] - TotalTransZ, LegIndex); 681 | } 682 | 683 | // Do IK for all Left legs 684 | for (LegIndex = (CNT_LEGS/2); LegIndex < CNT_LEGS; LegIndex++) { 685 | DoBackgroundProcess(); 686 | BodyFK(LegPosX[LegIndex]-g_InControlState.BodyPos.x+GaitPosX[LegIndex] - TotalTransX, 687 | LegPosZ[LegIndex]+g_InControlState.BodyPos.z+GaitPosZ[LegIndex] - TotalTransZ, 688 | LegPosY[LegIndex]+g_InControlState.BodyPos.y+GaitPosY[LegIndex] - TotalTransY, 689 | GaitRotY[LegIndex], LegIndex); 690 | LegIK (LegPosX[LegIndex]+g_InControlState.BodyPos.x-BodyFKPosX+GaitPosX[LegIndex] - TotalTransX, 691 | LegPosY[LegIndex]+g_InControlState.BodyPos.y-BodyFKPosY+GaitPosY[LegIndex] - TotalTransY, 692 | LegPosZ[LegIndex]+g_InControlState.BodyPos.z-BodyFKPosZ+GaitPosZ[LegIndex] - TotalTransZ, LegIndex); 693 | } 694 | #ifdef OPT_WALK_UPSIDE_DOWN 695 | if (g_fRobotUpsideDown){ //Need to set them back for not messing with the SmoothControl 696 | g_InControlState.BodyPos.x = -g_InControlState.BodyPos.x; 697 | g_InControlState.SLLeg.x = -g_InControlState.SLLeg.x; 698 | g_InControlState.BodyRot1.z = -g_InControlState.BodyRot1.z; 699 | } 700 | #endif 701 | // Check mechanical limits 702 | CheckAngles(); 703 | 704 | // Write IK errors to leds 705 | LedC = IKSolutionWarning; 706 | LedA = IKSolutionError; 707 | 708 | // Drive Servos 709 | if (g_InControlState.fRobotOn) { 710 | if (g_InControlState.fRobotOn && !g_InControlState.fPrev_RobotOn) { 711 | MSound(3, 60, 2000, 80, 2250, 100, 2500); 712 | #ifdef USEXBEE 713 | XBeePlaySounds(3, 60, 2000, 80, 2250, 100, 2500); 714 | #endif 715 | 716 | Eyes = 1; 717 | } 718 | 719 | //Calculate Servo Move time 720 | if ((abs(g_InControlState.TravelLength.x)>cTravelDeadZone) || (abs(g_InControlState.TravelLength.z)>cTravelDeadZone) || 721 | (abs(g_InControlState.TravelLength.y*2)>cTravelDeadZone)) { 722 | ServoMoveTime = g_InControlState.gaitCur.NomGaitSpeed + (g_InControlState.InputTimeDelay*2) + g_InControlState.SpeedControl; 723 | 724 | //Add aditional delay when Balance mode is on 725 | if (g_InControlState.BalanceMode) 726 | ServoMoveTime = ServoMoveTime + BALANCE_DELAY; 727 | } 728 | else //Movement speed excl. Walking 729 | ServoMoveTime = 200 + g_InControlState.SpeedControl; 730 | 731 | // note we broke up the servo driver into start/commit that way we can output all of the servo information 732 | // before we wait and only have the termination information to output after the wait. That way we hopefully 733 | // be more accurate with our timings... 734 | DoBackgroundProcess(); 735 | StartUpdateServos(); 736 | 737 | // See if we need to sync our processor with the servo driver while walking to ensure the prev is completed 738 | //before sending the next one 739 | 740 | 741 | // Finding any incident of GaitPos/Rot <>0: 742 | for (LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) { 743 | if ( (GaitPosX[LegIndex] > cGPlimit) || (GaitPosX[LegIndex] < -cGPlimit) 744 | || (GaitPosZ[LegIndex] > cGPlimit) || (GaitPosZ[LegIndex] < -cGPlimit) 745 | || (GaitRotY[LegIndex] > cGPlimit) || (GaitRotY[LegIndex] < -cGPlimit)) { 746 | 747 | bExtraCycle = g_InControlState.gaitCur.NrLiftedPos + 1;//For making sure that we are using timed move until all legs are down 748 | break; 749 | } 750 | } 751 | if (bExtraCycle>0){ 752 | bExtraCycle--; 753 | fWalking = !(bExtraCycle==0); 754 | 755 | //Get endtime and calculate wait time 756 | lTimeWaitEnd = lTimerStart + PrevServoMoveTime; 757 | 758 | DebugWrite(A1, HIGH); 759 | do { 760 | // Wait the appropriate time, call any background process while waiting... 761 | DoBackgroundProcess(); 762 | } 763 | while (millis() < lTimeWaitEnd); 764 | DebugWrite(A1, LOW); 765 | #ifdef DEBUG_X 766 | if (g_fDebugOutput) { 767 | 768 | DBGSerial.print("BRX:"); 769 | DBGSerial.print(g_InControlState.BodyRot1.x,DEC); 770 | DBGSerial.print("W?:"); 771 | DBGSerial.print(fWalking,DEC); 772 | DBGSerial.print(" GS:"); 773 | DBGSerial.print(g_InControlState.GaitStep,DEC); 774 | //Debug LF leg 775 | DBGSerial.print(" GPZ:"); 776 | DBGSerial.print(GaitPosZ[cLF],DEC); 777 | DBGSerial.print(" GPY:"); 778 | DBGSerial.println(GaitPosY[cLF],DEC); 779 | } 780 | #endif 781 | } 782 | #ifdef DEBUG_X 783 | if (g_fDebugOutput) { 784 | 785 | 786 | DBGSerial.print("TY:"); 787 | DBGSerial.print(TotalYBal1,DEC); 788 | DBGSerial.print(" LFZ:"); 789 | DBGSerial.println(LegPosZ[cLF],DEC); 790 | DBGSerial.flush(); // see if forcing it to output helps... 791 | } 792 | #endif 793 | // Only do commit if we are actually doing something... 794 | DebugToggle(A2); 795 | g_ServoDriver.CommitServoDriver(ServoMoveTime); 796 | 797 | 798 | } 799 | else { 800 | // Turn the bot off 801 | if (g_InControlState.fPrev_RobotOn || (AllDown= 0)) { 802 | ServoMoveTime = 600; 803 | StartUpdateServos(); 804 | g_ServoDriver.CommitServoDriver(ServoMoveTime); 805 | MSound(3, 100, 2500, 80, 2250, 60, 2000); 806 | #ifdef USEXBEE 807 | XBeePlaySounds(3, 100, 2500, 80, 2250, 60, 2000); 808 | #endif 809 | lTimeWaitEnd = millis() + 600; // setup to process background stuff while we wait... 810 | do { 811 | // Wait the appropriate time, call any background process while waiting... 812 | DoBackgroundProcess(); 813 | } 814 | while (millis() < lTimeWaitEnd); 815 | //delay(600); 816 | } 817 | else { 818 | g_ServoDriver.FreeServos(); 819 | Eyes = 0; 820 | } 821 | 822 | // Allow the Servo driver to do stuff durint our idle time 823 | g_ServoDriver.IdleTime(); 824 | 825 | // We also have a simple debug monitor that allows us to 826 | // check things. call it here.. 827 | #ifdef OPT_TERMINAL_MONITOR 828 | if (TerminalMonitor()) 829 | return; 830 | #endif 831 | delay(20); // give a pause between times we call if nothing is happening 832 | } 833 | 834 | PrevServoMoveTime = ServoMoveTime; 835 | 836 | //Store previous g_InControlState.fRobotOn State 837 | if (g_InControlState.fRobotOn) 838 | g_InControlState.fPrev_RobotOn = 1; 839 | else 840 | g_InControlState.fPrev_RobotOn = 0; 841 | } 842 | 843 | 844 | void StartUpdateServos() 845 | { 846 | byte LegIndex; 847 | 848 | // First call off to the init... 849 | g_ServoDriver.BeginServoUpdate(); // Start the update 850 | 851 | for (LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) { 852 | #ifdef c4DOF 853 | g_ServoDriver.OutputServoInfoForLeg(LegIndex, 854 | cCoxaInv[LegIndex]? -CoxaAngle1[LegIndex] : CoxaAngle1[LegIndex], 855 | cFemurInv[LegIndex]? -FemurAngle1[LegIndex] : FemurAngle1[LegIndex], 856 | cTibiaInv[LegIndex]? -TibiaAngle1[LegIndex] : TibiaAngle1[LegIndex], 857 | cTarsInv[LegIndex]? -TarsAngle1[LegIndex] : TarsAngle1[LegIndex]); 858 | #else 859 | g_ServoDriver.OutputServoInfoForLeg(LegIndex, 860 | cCoxaInv[LegIndex]? -CoxaAngle1[LegIndex] : CoxaAngle1[LegIndex], 861 | cFemurInv[LegIndex]? -FemurAngle1[LegIndex] : FemurAngle1[LegIndex], 862 | cTibiaInv[LegIndex]? -TibiaAngle1[LegIndex] : TibiaAngle1[LegIndex]); 863 | #endif 864 | } 865 | #ifdef cTurretRotPin 866 | g_ServoDriver.OutputServoInfoForTurret(g_InControlState.TurretRotAngle1, g_InControlState.TurretTiltAngle1); // fist just see if it will talk 867 | #endif 868 | } 869 | 870 | //[WriteOutputs] Updates the state of the leds 871 | 872 | void WriteOutputs(void) 873 | { 874 | #ifdef cEyesPin 875 | digitalWrite(cEyesPin, Eyes); 876 | #endif 877 | } 878 | 879 | // ////////////////// CHECK VOLTAGE ////////////////// 880 | // Read the input voltage and shuts down the bot when the power drops. This prevents damage to the battery, the controller, 881 | // the servos, or some combination of all those parts. 882 | 883 | byte s_bLVBeepCnt; 884 | boolean CheckVoltage() { 885 | #ifdef cTurnOffVol 886 | Voltage = g_ServoDriver.GetBatteryVoltage(); 887 | 888 | if (!g_fLowVoltageShutdown) { 889 | if ((Voltage < cTurnOffVol) || (Voltage >= 1999)) { 890 | #ifdef DBGSerial 891 | DBGSerial.print("Voltage went low, turn off robot "); 892 | DBGSerial.println(Voltage, DEC); 893 | #endif 894 | //Turn off 895 | g_InControlState.BodyPos.x = 0; 896 | g_InControlState.BodyPos.y = 0; 897 | g_InControlState.BodyPos.z = 0; 898 | g_InControlState.BodyRot1.x = 0; 899 | g_InControlState.BodyRot1.y = 0; 900 | g_InControlState.BodyRot1.z = 0; 901 | g_InControlState.TravelLength.x = 0; 902 | g_InControlState.TravelLength.z = 0; 903 | 904 | #ifdef OPT_SINGLELEG 905 | g_InControlState.TravelLength.y = 0; 906 | g_InControlState.SelectedLeg = 255; 907 | #endif 908 | g_fLowVoltageShutdown = 1; 909 | s_bLVBeepCnt = 0; // how many times we beeped... 910 | g_InControlState.fRobotOn = false; 911 | } 912 | #ifdef cTurnOnVol 913 | } 914 | else if ((Voltage > cTurnOnVol) && (Voltage < 1999)) { 915 | #ifdef DBGSerial 916 | DBGSerial.print(F("Voltage restored: ")); 917 | DBGSerial.println(Voltage, DEC); 918 | #endif 919 | g_fLowVoltageShutdown = 0; 920 | 921 | #endif 922 | } 923 | else { 924 | if (s_bLVBeepCnt < 5) { 925 | s_bLVBeepCnt++; 926 | #ifdef DBGSerial 927 | DBGSerial.println(Voltage, DEC); 928 | #endif 929 | MSound( 1, 45, 2000); 930 | } 931 | delay(2000); 932 | } 933 | #endif 934 | return g_fLowVoltageShutdown; 935 | } 936 | 937 | 938 | // ////////////////// SINGLE LEG CONTROL ////////////////// 939 | // In single leg control, controller inputs move, as the name implies, a single leg on the robot. 940 | 941 | void SingleLegControl(void) 942 | { 943 | #ifdef OPT_SINGLELEG 944 | 945 | // Check if all legs are down because if there are legs in the air, we do not want to lift any other legs 946 | // or else the robot could tip over. 947 | AllDown = (LegPosY[cRF]==(short)pgm_read_word(&cInitPosY[cRF])) && 948 | (LegPosY[cRR]==(short)pgm_read_word(&cInitPosY[cRR])) && 949 | (LegPosY[cLR]==(short)pgm_read_word(&cInitPosY[cLR])) && 950 | #ifndef QUADMODE 951 | (LegPosY[cRM]==(short)pgm_read_word(&cInitPosY[cRM])) && 952 | (LegPosY[cLM]==(short)pgm_read_word(&cInitPosY[cLM])) && 953 | #endif 954 | (LegPosY[cLF]==(short)pgm_read_word(&cInitPosY[cLF])); 955 | 956 | if (g_InControlState.SelectedLeg<= (CNT_LEGS-1)) { 957 | if (g_InControlState.SelectedLeg!=PrevSelectedLeg) { 958 | if (AllDown) { //Lift leg a bit when it got selected 959 | LegPosY[g_InControlState.SelectedLeg] = (short)pgm_read_word(&cInitPosY[g_InControlState.SelectedLeg])-20; 960 | 961 | // Store current status 962 | PrevSelectedLeg = g_InControlState.SelectedLeg; 963 | } 964 | else {//Return prev leg back to the init position 965 | LegPosX[PrevSelectedLeg] = (short)pgm_read_word(&cInitPosX[PrevSelectedLeg]); 966 | LegPosY[PrevSelectedLeg] = (short)pgm_read_word(&cInitPosY[PrevSelectedLeg]); 967 | LegPosZ[PrevSelectedLeg] = (short)pgm_read_word(&cInitPosZ[PrevSelectedLeg]); 968 | } 969 | } 970 | else if (!g_InControlState.fSLHold) { 971 | //LegPosY[g_InControlState.SelectedLeg] = LegPosY[g_InControlState.SelectedLeg]+g_InControlState.SLLeg.y; 972 | LegPosY[g_InControlState.SelectedLeg] = (short)pgm_read_word(&cInitPosY[g_InControlState.SelectedLeg])+g_InControlState.SLLeg.y;// Using DIY remote Zenta prefer it this way 973 | LegPosX[g_InControlState.SelectedLeg] = (short)pgm_read_word(&cInitPosX[g_InControlState.SelectedLeg])+g_InControlState.SLLeg.x; 974 | LegPosZ[g_InControlState.SelectedLeg] = (short)pgm_read_word(&cInitPosZ[g_InControlState.SelectedLeg])+g_InControlState.SLLeg.z; 975 | } 976 | } 977 | else {//All legs to init position 978 | if (!AllDown) { 979 | for(LegIndex = 0; LegIndex <= (CNT_LEGS-1);LegIndex++) { 980 | LegPosX[LegIndex] = (short)pgm_read_word(&cInitPosX[LegIndex]); 981 | LegPosY[LegIndex] = (short)pgm_read_word(&cInitPosY[LegIndex]); 982 | LegPosZ[LegIndex] = (short)pgm_read_word(&cInitPosZ[LegIndex]); 983 | } 984 | } 985 | if (PrevSelectedLeg!=255) 986 | PrevSelectedLeg = 255; 987 | } 988 | #endif 989 | } 990 | 991 | 992 | void GaitSelect(void) 993 | { 994 | //Gait selector 995 | // First pass simply use defined table, next up will allow robots to add or relace set... 996 | if (g_InControlState.GaitType < NUM_GAITS) { 997 | #ifdef ADD_GAITS 998 | if (g_InControlState.GaitType < (sizeof(APG_EXTRA)/sizeof(APG_EXTRA[0]))) 999 | g_InControlState.gaitCur = APG_EXTRA[g_InControlState.GaitType]; 1000 | else 1001 | g_InControlState.gaitCur = APG[g_InControlState.GaitType - (sizeof(APG_EXTRA)/sizeof(APG_EXTRA[0]))]; 1002 | #else 1003 | g_InControlState.gaitCur = APG[g_InControlState.GaitType]; 1004 | #endif 1005 | } 1006 | 1007 | #ifdef DBGSerial 1008 | if (g_fDebugOutput) { 1009 | DBGSerial.print(g_InControlState.GaitType, DEC); 1010 | DBGSerial.print(" {"); 1011 | DBGSerial.print(g_InControlState.gaitCur.NomGaitSpeed, DEC); 1012 | DBGSerial.print(", "); 1013 | DBGSerial.print(g_InControlState.gaitCur.StepsInGait, DEC); 1014 | DBGSerial.print(", "); 1015 | DBGSerial.print(g_InControlState.gaitCur.NrLiftedPos, DEC); 1016 | DBGSerial.print(", "); 1017 | DBGSerial.print(g_InControlState.gaitCur.FrontDownPos, DEC); 1018 | DBGSerial.print(", "); 1019 | DBGSerial.print(g_InControlState.gaitCur.LiftDivFactor, DEC); 1020 | DBGSerial.print(", "); 1021 | DBGSerial.print(g_InControlState.gaitCur.TLDivFactor, DEC); 1022 | DBGSerial.print(", "); 1023 | DBGSerial.print(g_InControlState.gaitCur.HalfLiftHeight, DEC); 1024 | DBGSerial.print(", {"); 1025 | for (int il = 0; il < CNT_LEGS; il++) { 1026 | DBGSerial.print(g_InControlState.gaitCur.GaitLegNr[il], DEC); 1027 | if (il < (CNT_LEGS-1)) 1028 | DBGSerial.print(", "); 1029 | } 1030 | DBGSerial.println("}}"); 1031 | } 1032 | #endif 1033 | 1034 | } 1035 | 1036 | 1037 | // ////////////////////////////////////////////////////////////////////////// 1038 | // ////////////////////////// GAIT SEQUENCE /////////////////////////// 1039 | // ///////////////////////////////////////////////////////////////////////// 1040 | 1041 | void GaitSeq(void) 1042 | { 1043 | // Check if the Gait is in motion - If not if we are going to start a motion try to align our Gaitstep to start with a good foot 1044 | // for the direction we are about to go. This makes for a smooth gait motion. 1045 | 1046 | if (fWalking || (g_InControlState.ForceGaitStepCnt != 0)) 1047 | TravelRequest = true; // Is walking or was walking... 1048 | else { 1049 | TravelRequest = (abs(g_InControlState.TravelLength.x)>cTravelDeadZone) 1050 | || (abs(g_InControlState.TravelLength.z)>cTravelDeadZone) 1051 | || (abs(g_InControlState.TravelLength.y)>cTravelDeadZone) ; 1052 | 1053 | if (TravelRequest) { 1054 | #ifdef QUADCODE 1055 | // Just start walking - Try to guess a good foot to start off on... 1056 | if (g_InControlState.TravelLength.z < 0) 1057 | g_InControlState.GaitStep = ((g_InControlState.TravelLength.X < 0)? g_InControlState.gaitCur.GaitLegNr[cLR] : g_InControlState.gaitCur.GaitLegNr[cRR]); 1058 | else 1059 | g_InControlState.GaitStep = ((g_InControlState.TravelLength.X < 0)? g_InControlState.gaitCur.GaitLegNr[cLF] : g_InControlState.gaitCur.GaitLegNr[cRF]); 1060 | // And lets backup a few Gaitsteps before this to allow it to start the up swing... 1061 | g_InControlState.GaitStep = ((g_InControlState.GaitStep > g_InControlState.gaitCur.FrontDownPos)? (g_InControlState.GaitStep - g_InControlState.gaitCur.FrontDownPos) : (g_InControlState.GaitStep + g_InControlState.gaitCur.StepsInGait - g_InControlState.gaitCur.FrontDownPos); 1062 | #endif 1063 | } 1064 | else { //Clear values under the cTravelDeadZone 1065 | g_InControlState.TravelLength.x=0; 1066 | g_InControlState.TravelLength.z=0; 1067 | g_InControlState.TravelLength.y=0;//Gait NOT in motion, return to home position 1068 | } 1069 | } 1070 | 1071 | //Calculate Gait sequence 1072 | for (LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) { // for all legs 1073 | Gait(LegIndex); 1074 | } // next leg 1075 | 1076 | //Advance to the next step 1077 | g_InControlState.GaitStep++; 1078 | if (g_InControlState.GaitStep>g_InControlState.gaitCur.StepsInGait) 1079 | g_InControlState.GaitStep = 1; 1080 | 1081 | // If we have a force count decrement it now... 1082 | if (g_InControlState.ForceGaitStepCnt) 1083 | g_InControlState.ForceGaitStepCnt--; 1084 | } 1085 | 1086 | 1087 | // //////////////////////////////////////////////////////////////// 1088 | // ////////////////////////// GAIT /////////////////////////// 1089 | // //////////////////////////////////////////////////////////////// 1090 | 1091 | void Gait (byte GaitCurrentLegNr) 1092 | { 1093 | 1094 | // Try to reduce the number of time we look at GaitLegnr and Gaitstep 1095 | short int LegStep = g_InControlState.GaitStep - g_InControlState.gaitCur.GaitLegNr[GaitCurrentLegNr]; 1096 | 1097 | //Leg middle up position OK 1098 | //Gait in motion 1099 | // For Lifted pos = 1, 3, 5 1100 | if ((TravelRequest && (g_InControlState.gaitCur.NrLiftedPos&1) && 1101 | LegStep==0) || (!TravelRequest && LegStep==0 && ((abs(GaitPosX[GaitCurrentLegNr])>2) || 1102 | (abs(GaitPosZ[GaitCurrentLegNr])>2) || (abs(GaitRotY[GaitCurrentLegNr])>2)))) { //Up 1103 | GaitPosX[GaitCurrentLegNr] = 0; 1104 | GaitPosY[GaitCurrentLegNr] = -g_InControlState.LegLiftHeight; 1105 | GaitPosZ[GaitCurrentLegNr] = 0; 1106 | GaitRotY[GaitCurrentLegNr] = 0; 1107 | } 1108 | //Optional Half heigth Rear (2, 3, 5 lifted positions) 1109 | else if (((g_InControlState.gaitCur.NrLiftedPos==2 && LegStep==0) || (g_InControlState.gaitCur.NrLiftedPos>=3 && 1110 | (LegStep==-1 || LegStep==(g_InControlState.gaitCur.StepsInGait-1)))) 1111 | && TravelRequest) { 1112 | GaitPosX[GaitCurrentLegNr] = -g_InControlState.TravelLength.x/g_InControlState.gaitCur.LiftDivFactor; 1113 | GaitPosY[GaitCurrentLegNr] = -3*g_InControlState.LegLiftHeight/(3+g_InControlState.gaitCur.HalfLiftHeight); //Easier to shift between div factor: /1 (3/3), /2 (3/6) and 3/4 1114 | GaitPosZ[GaitCurrentLegNr] = -g_InControlState.TravelLength.z/g_InControlState.gaitCur.LiftDivFactor; 1115 | GaitRotY[GaitCurrentLegNr] = -g_InControlState.TravelLength.y/g_InControlState.gaitCur.LiftDivFactor; 1116 | } 1117 | // _A_ 1118 | // Optional Half heigth front (2, 3, 5 lifted positions) 1119 | else if ((g_InControlState.gaitCur.NrLiftedPos>=2) && (LegStep==1 || LegStep==-(g_InControlState.gaitCur.StepsInGait-1)) && TravelRequest) { 1120 | GaitPosX[GaitCurrentLegNr] = g_InControlState.TravelLength.x/g_InControlState.gaitCur.LiftDivFactor; 1121 | GaitPosY[GaitCurrentLegNr] = -3*g_InControlState.LegLiftHeight/(3+g_InControlState.gaitCur.HalfLiftHeight); // Easier to shift between div factor: /1 (3/3), /2 (3/6) and 3/4 1122 | GaitPosZ[GaitCurrentLegNr] = g_InControlState.TravelLength.z/g_InControlState.gaitCur.LiftDivFactor; 1123 | GaitRotY[GaitCurrentLegNr] = g_InControlState.TravelLength.y/g_InControlState.gaitCur.LiftDivFactor; 1124 | } 1125 | 1126 | //Optional Half heigth Rear 5 LiftedPos (5 lifted positions) 1127 | else if (((g_InControlState.gaitCur.NrLiftedPos==5 && (LegStep==-2 ))) && TravelRequest) { 1128 | GaitPosX[GaitCurrentLegNr] = -g_InControlState.TravelLength.x/2; 1129 | GaitPosY[GaitCurrentLegNr] = -g_InControlState.LegLiftHeight/2; 1130 | GaitPosZ[GaitCurrentLegNr] = -g_InControlState.TravelLength.z/2; 1131 | GaitRotY[GaitCurrentLegNr] = -g_InControlState.TravelLength.y/2; 1132 | } 1133 | 1134 | //Optional Half heigth Front 5 LiftedPos (5 lifted positions) 1135 | else if ((g_InControlState.gaitCur.NrLiftedPos==5) && (LegStep==2 || LegStep==-(g_InControlState.gaitCur.StepsInGait-2)) && TravelRequest) { 1136 | GaitPosX[GaitCurrentLegNr] = g_InControlState.TravelLength.x/2; 1137 | GaitPosY[GaitCurrentLegNr] = -g_InControlState.LegLiftHeight/2; 1138 | GaitPosZ[GaitCurrentLegNr] = g_InControlState.TravelLength.z/2; 1139 | GaitRotY[GaitCurrentLegNr] = g_InControlState.TravelLength.y/2; 1140 | } 1141 | //_B_ 1142 | //Leg front down position //bug here? From _A_ to _B_ there should only be one gaitstep, not 2! 1143 | //For example, where is the case of LegStep==0+2 executed when NRLiftedPos=3? 1144 | else if ((LegStep==g_InControlState.gaitCur.FrontDownPos || LegStep==-(g_InControlState.gaitCur.StepsInGait-g_InControlState.gaitCur.FrontDownPos)) && GaitPosY[GaitCurrentLegNr]<0) { 1145 | GaitPosX[GaitCurrentLegNr] = g_InControlState.TravelLength.x/2; 1146 | GaitPosZ[GaitCurrentLegNr] = g_InControlState.TravelLength.z/2; 1147 | GaitRotY[GaitCurrentLegNr] = g_InControlState.TravelLength.y/2; 1148 | GaitPosY[GaitCurrentLegNr] = 0; 1149 | } 1150 | 1151 | //Move body forward 1152 | else { 1153 | GaitPosX[GaitCurrentLegNr] = GaitPosX[GaitCurrentLegNr] - (g_InControlState.TravelLength.x/(short)g_InControlState.gaitCur.TLDivFactor); 1154 | GaitPosY[GaitCurrentLegNr] = 0; 1155 | GaitPosZ[GaitCurrentLegNr] = GaitPosZ[GaitCurrentLegNr] - (g_InControlState.TravelLength.z/(short)g_InControlState.gaitCur.TLDivFactor); 1156 | GaitRotY[GaitCurrentLegNr] = GaitRotY[GaitCurrentLegNr] - (g_InControlState.TravelLength.y/(short)g_InControlState.gaitCur.TLDivFactor); 1157 | } 1158 | 1159 | } 1160 | 1161 | // ///////////////////////////////////////////////////////////////////////////////////// 1162 | // ////////////////////////// BALANCE CALCULATION - LEG /////////////////////////// 1163 | // //////////////////////////////////////////////////////////////////////////////////// 1164 | 1165 | void BalCalcOneLeg (long PosX, long PosZ, long PosY, byte BalLegNr) 1166 | { 1167 | long CPR_X; //Final X value for centerpoint of rotation 1168 | long CPR_Y; //Final Y value for centerpoint of rotation 1169 | long CPR_Z; //Final Z value for centerpoint of rotation 1170 | 1171 | 1172 | #ifdef QUADMODE 1173 | if (g_InControlState.gaitCur.COGAngleStep1 == 0) { // In Quad mode only do those for those who don't support COG Balance... 1174 | #endif 1175 | long lAtan; 1176 | //Calculating totals from center of the body to the feet 1177 | CPR_Z = (short)pgm_read_word(&cOffsetZ[BalLegNr]) + PosZ; 1178 | CPR_X = (short)pgm_read_word(&cOffsetX[BalLegNr]) + PosX; 1179 | CPR_Y = 150 + PosY; // using the value 150 to lower the centerpoint of rotation 'g_InControlState.BodyPos.y + 1180 | 1181 | TotalTransY += (long)PosY; 1182 | TotalTransZ += (long)CPR_Z; 1183 | TotalTransX += (long)CPR_X; 1184 | 1185 | lAtan = GetATan2(CPR_X, CPR_Z); 1186 | TotalYBal1 += (lAtan*1800) / 31415; 1187 | #ifdef DEBUG 1188 | if (g_fDebugOutput) { 1189 | DBGSerial.print(" "); 1190 | DBGSerial.print(CPR_X, DEC); 1191 | DBGSerial.print(":"); 1192 | DBGSerial.print(CPR_Y, DEC); 1193 | DBGSerial.print(":"); 1194 | DBGSerial.print(CPR_Z, DEC); 1195 | DBGSerial.print(":"); 1196 | DBGSerial.print(TotalYBal1, DEC); 1197 | } 1198 | #endif 1199 | 1200 | lAtan = GetATan2 (CPR_X, CPR_Y); 1201 | TotalZBal1 += ((lAtan*1800) / 31415) -900; //Rotate balance circle 90 deg 1202 | 1203 | lAtan = GetATan2 (CPR_Z, CPR_Y); 1204 | TotalXBal1 += ((lAtan*1800) / 31415) - 900; //Rotate balance circle 90 deg 1205 | 1206 | #ifdef QUADMODE 1207 | } 1208 | #endif 1209 | 1210 | } 1211 | 1212 | // ////////////////////////////////////////////////////////////////////////////////////// 1213 | // ////////////////////////// BALANCE CALCULATION - BODY /////////////////////////// 1214 | // ////////////////////////////////////////////////////////////////////////////////////// 1215 | void BalanceBody(void) 1216 | { 1217 | #ifdef QUADMODE 1218 | if (g_InControlState.gaitCur.COGAngleStep1 == 0) { // In Quad mode only do those for those who don't support COG Balance... 1219 | #endif 1220 | TotalTransZ = TotalTransZ/BalanceDivFactor ; 1221 | TotalTransX = TotalTransX/BalanceDivFactor; 1222 | TotalTransY = TotalTransY/BalanceDivFactor; 1223 | 1224 | #ifndef QUADMODE // ??? on PhantomX Hex at no movment YBal1 = 1800, on Quad = 0... Need to experiment 1225 | if (TotalYBal1 > 0) //Rotate balance circle by +/- 180 deg 1226 | TotalYBal1 -= 1800; 1227 | else 1228 | TotalYBal1 += 1800; 1229 | #endif 1230 | 1231 | if (TotalZBal1 < -1800) //Compensate for extreme balance positions that causes overflow 1232 | TotalZBal1 += 3600; 1233 | 1234 | if (TotalXBal1 < -1800) //Compensate for extreme balance positions that causes overflow 1235 | TotalXBal1 += 3600; 1236 | 1237 | //Balance rotation 1238 | TotalYBal1 = -TotalYBal1/BalanceDivFactor; 1239 | TotalXBal1 = -TotalXBal1/BalanceDivFactor; 1240 | TotalZBal1 = TotalZBal1/BalanceDivFactor; 1241 | #ifdef DEBUG 1242 | if (g_fDebugOutput) { 1243 | DBGSerial.print(" L "); 1244 | DBGSerial.print(BalanceDivFactor, DEC); 1245 | DBGSerial.print(" TTrans: "); 1246 | DBGSerial.print(TotalTransX, DEC); 1247 | DBGSerial.print(" "); 1248 | DBGSerial.print(TotalTransY, DEC); 1249 | DBGSerial.print(" "); 1250 | DBGSerial.print(TotalTransZ, DEC); 1251 | DBGSerial.print(" TBal: "); 1252 | DBGSerial.print(TotalXBal1, DEC); 1253 | DBGSerial.print(" "); 1254 | DBGSerial.print(TotalYBal1, DEC); 1255 | DBGSerial.print(" "); 1256 | DBGSerial.println(TotalZBal1, DEC); 1257 | } 1258 | #endif 1259 | #ifdef QUADMODE 1260 | } else { 1261 | // Quad mode with COG balance mode... 1262 | byte COGShiftNeeded; 1263 | byte BalCOGTransX; 1264 | byte BalCOGTransZ; 1265 | word COGAngle1; 1266 | long BalTotTravelLength; 1267 | 1268 | COGShiftNeeded = TravelRequest; 1269 | for (LegIndex = 0; LegIndex <= CNT_LEGS; LegIndex++) 1270 | { 1271 | // Check if the cog needs to be shifted (travelRequest or legs goto home.) 1272 | COGShiftNeeded = COGShiftNeeded || (abs(GaitPosX[LegIndex])>2) || (abs(GaitPosZ[LegIndex])>2) || (abs(GaitRotY[LegIndex])>2); 1273 | } 1274 | 1275 | if (COGShiftNeeded) { 1276 | if (g_InControlState.gaitCur.COGCCW) { 1277 | COGAngle1 = g_InControlState.gaitCur.COGAngleStart1 - (g_InControlState.GaitStep-1) * g_InControlState.gaitCur.COGAngleStep1; 1278 | } else { 1279 | COGAngle1 = g_InControlState.gaitCur.COGAngleStart1 + (g_InControlState.GaitStep-1) * g_InControlState.gaitCur.COGAngleStep1; 1280 | } 1281 | GetSinCos(COGAngle1); 1282 | TotalTransX = (long)g_InControlState.gaitCur.COGRadius * (long)sin4 / c4DEC; 1283 | TotalTransZ = (long)g_InControlState.gaitCur.COGRadius * (long)cos4 / c4DEC; 1284 | 1285 | #ifdef DEBUG 1286 | if (g_fDebugOutput) { 1287 | DBGSerial.print(" TotalTransX: "); 1288 | DBGSerial.print(TotalTransX, DEC); 1289 | DBGSerial.print(" TotalTransZ: "); 1290 | DBGSerial.print(TotalTransZ, DEC); 1291 | } 1292 | #endif 1293 | // Add direction variable. The body will not shift in the direction you're walking 1294 | if (((abs(g_InControlState.TravelLength.x)>cTravelDeadZone) || (abs(g_InControlState.TravelLength.z)>cTravelDeadZone)) && (abs(g_InControlState.TravelLength.y)<=cTravelDeadZone) ) { 1295 | //if(TravelRotationY = 0) then 1296 | 1297 | BalTotTravelLength = isqrt32(abs(g_InControlState.TravelLength.x * g_InControlState.TravelLength.x) + abs(g_InControlState.TravelLength.z*g_InControlState.TravelLength.z)); 1298 | BalCOGTransX = abs(g_InControlState.TravelLength.z)*c2DEC/BalTotTravelLength; 1299 | BalCOGTransZ = abs(g_InControlState.TravelLength.x)*c2DEC/BalTotTravelLength; 1300 | TotalTransX = TotalTransX*BalCOGTransX/c2DEC; 1301 | TotalTransZ = TotalTransZ*BalCOGTransZ/c2DEC; 1302 | } 1303 | 1304 | #ifdef DEBUG 1305 | if (g_fDebugOutput) { 1306 | DBGSerial.print(" COGRadius: "); 1307 | DBGSerial.print(g_InControlState.gaitCur.COGRadius, DEC); 1308 | DBGSerial.print(" TotalTransX: "); 1309 | DBGSerial.print(TotalTransX, DEC); 1310 | DBGSerial.print(" TotalTransZ: "); 1311 | DBGSerial.println(TotalTransZ, DEC); 1312 | } 1313 | #endif 1314 | } 1315 | } 1316 | #endif 1317 | } 1318 | 1319 | 1320 | // ////////////////////////////////////////////////////////////////////////// 1321 | // ////////////////////////// TRIG FUNCTIONS /////////////////////////// 1322 | // ////////////////////////////////////////////////////////////////////////// 1323 | // This section contains functions for doing all of the various trig operations needed for the 1324 | // inverse kinematics calculations. The functions use the trig tables at the top of this 1325 | // document rather than actually performing the trig functions. This saves considerably on 1326 | // computational performance. 1327 | 1328 | //[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles 1329 | //AngleDeg1 - Input Angle in degrees 1330 | //sin4 - Output Sinus of AngleDeg 1331 | //cos4 - Output Cosinus of AngleDeg 1332 | void GetSinCos(short AngleDeg1) 1333 | { 1334 | short ABSAngleDeg1; // Absolute value of the Angle in Degrees, decimals = 1 1335 | //Get the absolute value of AngleDeg 1336 | if (AngleDeg1 < 0) 1337 | ABSAngleDeg1 = AngleDeg1 *-1; 1338 | else 1339 | ABSAngleDeg1 = AngleDeg1; 1340 | 1341 | //Shift rotation to a full circle of 360 deg -> AngleDeg // 360 1342 | if (AngleDeg1 < 0) //Negative values 1343 | AngleDeg1 = 3600-(ABSAngleDeg1-(3600*(ABSAngleDeg1/3600))); 1344 | else //Positive values 1345 | AngleDeg1 = ABSAngleDeg1-(3600*(ABSAngleDeg1/3600)); 1346 | 1347 | if (AngleDeg1>=0 && AngleDeg1<=900) // 0 to 90 deg 1348 | { 1349 | sin4 = pgm_read_word(&GetSin[AngleDeg1/5]); // 5 is the presision (0.5) of the table 1350 | cos4 = pgm_read_word(&GetSin[(900-(AngleDeg1))/5]); 1351 | } 1352 | 1353 | else if (AngleDeg1>900 && AngleDeg1<=1800) // 90 to 180 deg 1354 | { 1355 | sin4 = pgm_read_word(&GetSin[(900-(AngleDeg1-900))/5]); // 5 is the presision (0.5) of the table 1356 | cos4 = -pgm_read_word(&GetSin[(AngleDeg1-900)/5]); 1357 | } 1358 | else if (AngleDeg1>1800 && AngleDeg1<=2700) // 180 to 270 deg 1359 | { 1360 | sin4 = -pgm_read_word(&GetSin[(AngleDeg1-1800)/5]); // 5 is the presision (0.5) of the table 1361 | cos4 = -pgm_read_word(&GetSin[(2700-AngleDeg1)/5]); 1362 | } 1363 | 1364 | else if(AngleDeg1>2700 && AngleDeg1<=3600) // 270 to 360 deg 1365 | { 1366 | sin4 = -pgm_read_word(&GetSin[(3600-AngleDeg1)/5]); // 5 is the presision (0.5) of the table 1367 | cos4 = pgm_read_word(&GetSin[(AngleDeg1-2700)/5]); 1368 | } 1369 | } 1370 | 1371 | 1372 | //(GETARCCOS) Get the sinus and cosinus from the angle +/- multiple circles 1373 | //cos4 - Input Cosinus 1374 | //AngleRad4 - Output Angle in AngleRad4 1375 | long GetArcCos(short cos4) 1376 | { 1377 | boolean NegativeValue/*:1*/; //If the the value is Negative 1378 | //Check for negative value 1379 | if (cos4<0) 1380 | { 1381 | cos4 = -cos4; 1382 | NegativeValue = 1; 1383 | } 1384 | else 1385 | NegativeValue = 0; 1386 | 1387 | //Limit cos4 to his maximal value 1388 | cos4 = min(cos4,c4DEC); 1389 | 1390 | if ((cos4>=0) && (cos4<9000)) 1391 | { 1392 | AngleRad4 = (byte)pgm_read_byte(&GetACos[cos4/79]); 1393 | AngleRad4 = ((long)AngleRad4*616)/c1DEC; //616=acos resolution (pi/2/255) ; 1394 | } 1395 | else if ((cos4>=9000) && (cos4<9900)) 1396 | { 1397 | AngleRad4 = (byte)pgm_read_byte(&GetACos[(cos4-9000)/8+114]); 1398 | AngleRad4 = (long)((long)AngleRad4*616)/c1DEC; //616=acos resolution (pi/2/255) 1399 | } 1400 | else if ((cos4>=9900) && (cos4<=10000)) 1401 | { 1402 | AngleRad4 = (byte)pgm_read_byte(&GetACos[(cos4-9900)/2+227]); 1403 | AngleRad4 = (long)((long)AngleRad4*616)/c1DEC; //616=acos resolution (pi/2/255) 1404 | } 1405 | 1406 | //Add negative sign 1407 | if (NegativeValue) 1408 | AngleRad4 = 31416 - AngleRad4; 1409 | 1410 | return AngleRad4; 1411 | } 1412 | 1413 | unsigned long isqrt32 (unsigned long n) // 1414 | { 1415 | unsigned long root; 1416 | unsigned long remainder; 1417 | unsigned long place; 1418 | 1419 | root = 0; 1420 | remainder = n; 1421 | place = 0x40000000; // OR place = 0x4000; OR place = 0x40; - respectively 1422 | 1423 | while (place > remainder) 1424 | place = place >> 2; 1425 | while (place) 1426 | { 1427 | if (remainder >= root + place) 1428 | { 1429 | remainder = remainder - root - place; 1430 | root = root + (place << 1); 1431 | } 1432 | root = root >> 1; 1433 | place = place >> 2; 1434 | } 1435 | return root; 1436 | } 1437 | 1438 | 1439 | //(GETATAN2) Simplyfied ArcTan2 function based on fixed point ArcCos 1440 | //ArcTanX - Input X 1441 | //ArcTanY - Input Y 1442 | //ArcTan4 - Output ARCTAN2(X/Y) 1443 | //XYhyp2 - Output presenting Hypotenuse of X and Y 1444 | short GetATan2 (short AtanX, short AtanY) 1445 | { 1446 | XYhyp2 = isqrt32(((long)AtanX*AtanX*c4DEC) + ((long)AtanY*AtanY*c4DEC)); 1447 | GetArcCos (((long)AtanX*(long)c6DEC) /(long) XYhyp2); 1448 | 1449 | if (AtanY < 0) // removed overhead... Atan4 = AngleRad4 * (AtanY/abs(AtanY)); 1450 | Atan4 = -AngleRad4; 1451 | else 1452 | Atan4 = AngleRad4; 1453 | return Atan4; 1454 | } 1455 | 1456 | // //////////////////////////////////////////////////////////////////////////////////// 1457 | // ////////////////////////// BODY INVERSE KINEMATICS //////////////////////////// 1458 | // /////////////////////////////////////////////////////////////////////////////////// 1459 | // If you want to learn more about the inverse kinematics calculations involved in controlling the hexapod robot, 1460 | // visit http://toglefritz.com/hexapod-inverse-kinematics-equations/ 1461 | 1462 | //BodyRotX - Global Input pitch of the body 1463 | //BodyRotY - Global Input rotation of the body 1464 | //BodyRotZ - Global Input roll of the body 1465 | //RotationY - Input Rotation for the gait 1466 | //PosX - Input position of the feet X 1467 | //PosZ - Input position of the feet Z 1468 | //SinB - Sin buffer for BodyRotX 1469 | //CosB - Cos buffer for BodyRotX 1470 | //SinG - Sin buffer for BodyRotZ 1471 | //CosG - Cos buffer for BodyRotZ 1472 | //BodyFKPosX - Output Position X of feet with Rotation 1473 | //BodyFKPosY - Output Position Y of feet with Rotation 1474 | //BodyFKPosZ - Output Position Z of feet with Rotation 1475 | void BodyFK (short PosX, short PosZ, short PosY, short RotationY, byte BodyIKLeg) 1476 | { 1477 | short SinA4; //Sin buffer for BodyRotX calculations 1478 | short CosA4; //Cos buffer for BodyRotX calculations 1479 | short SinB4; //Sin buffer for BodyRotX calculations 1480 | short CosB4; //Cos buffer for BodyRotX calculations 1481 | short SinG4; //Sin buffer for BodyRotZ calculations 1482 | short CosG4; //Cos buffer for BodyRotZ calculations 1483 | short CPR_X; //Final X value for centerpoint of rotation 1484 | short CPR_Y; //Final Y value for centerpoint of rotation 1485 | short CPR_Z; //Final Z value for centerpoint of rotation 1486 | 1487 | //Calculating totals from center of the body to the feet 1488 | CPR_X = (short)pgm_read_word(&cOffsetX[BodyIKLeg])+PosX + g_InControlState.BodyRotOffset.x; 1489 | CPR_Y = PosY + g_InControlState.BodyRotOffset.y; //Define centerpoint for rotation along the Y-axis 1490 | CPR_Z = (short)pgm_read_word(&cOffsetZ[BodyIKLeg]) + PosZ + g_InControlState.BodyRotOffset.z; 1491 | 1492 | //Successive global rotation matrix: 1493 | //Math shorts for rotation: Alfa [A] = Xrotate, Beta [B] = Zrotate, Gamma [G] = Yrotate 1494 | //Sinus Alfa = SinA, cosinus Alfa = cosA. and so on... 1495 | 1496 | //First calculate sinus and cosinus for each rotation: 1497 | GetSinCos (g_InControlState.BodyRot1.x+TotalXBal1); 1498 | SinG4 = sin4; 1499 | CosG4 = cos4; 1500 | 1501 | GetSinCos (g_InControlState.BodyRot1.z+TotalZBal1); 1502 | SinB4 = sin4; 1503 | CosB4 = cos4; 1504 | 1505 | #ifdef OPT_WALK_UPSIDE_DOWN 1506 | if (g_fRobotUpsideDown) 1507 | GetSinCos (-g_InControlState.BodyRot1.y+(-RotationY*c1DEC)+TotalYBal1) ; 1508 | else 1509 | GetSinCos (g_InControlState.BodyRot1.y+(RotationY*c1DEC)+TotalYBal1) ; 1510 | #else 1511 | GetSinCos (g_InControlState.BodyRot1.y+(RotationY*c1DEC)+TotalYBal1) ; 1512 | #endif 1513 | SinA4 = sin4; 1514 | CosA4 = cos4; 1515 | 1516 | //Calcualtion of rotation matrix: 1517 | BodyFKPosX = ((long)CPR_X*c2DEC - ((long)CPR_X*c2DEC*CosA4/c4DEC*CosB4/c4DEC - (long)CPR_Z*c2DEC*CosB4/c4DEC*SinA4/c4DEC 1518 | + (long)CPR_Y*c2DEC*SinB4/c4DEC ))/c2DEC; 1519 | BodyFKPosZ = ((long)CPR_Z*c2DEC - ( (long)CPR_X*c2DEC*CosG4/c4DEC*SinA4/c4DEC + (long)CPR_X*c2DEC*CosA4/c4DEC*SinB4/c4DEC*SinG4/c4DEC 1520 | + (long)CPR_Z*c2DEC*CosA4/c4DEC*CosG4/c4DEC - (long)CPR_Z*c2DEC*SinA4/c4DEC*SinB4/c4DEC*SinG4/c4DEC 1521 | - (long)CPR_Y*c2DEC*CosB4/c4DEC*SinG4/c4DEC ))/c2DEC; 1522 | BodyFKPosY = ((long)CPR_Y *c2DEC - ( (long)CPR_X*c2DEC*SinA4/c4DEC*SinG4/c4DEC - (long)CPR_X*c2DEC*CosA4/c4DEC*CosG4/c4DEC*SinB4/c4DEC 1523 | + (long)CPR_Z*c2DEC*CosA4/c4DEC*SinG4/c4DEC + (long)CPR_Z*c2DEC*CosG4/c4DEC*SinA4/c4DEC*SinB4/c4DEC 1524 | + (long)CPR_Y*c2DEC*CosB4/c4DEC*CosG4/c4DEC ))/c2DEC; 1525 | } 1526 | 1527 | 1528 | 1529 | // ///////////////////////////////////////////////////////////////////////////////////// 1530 | // ////////////////////////// LEG INVERSE KINEMATICS ////////////////////////////// 1531 | // //////////////////////////////////////////////////////////////////////////////////// 1532 | // Calculates the angles of the coxa, femur and tibia for the given position of the feet 1533 | //IKFeetPosX - Input position of the Feet X 1534 | //IKFeetPosY - Input position of the Feet Y 1535 | //IKFeetPosZ - Input Position of the Feet Z 1536 | //IKSolution - Output true if the solution is possible 1537 | //IKSolutionWarning - Output true if the solution is NEARLY possible 1538 | //IKSolutionError - Output true if the solution is NOT possible 1539 | //FemurAngle1 - Output Angle of Femur in degrees 1540 | //TibiaAngle1 - Output Angle of Tibia in degrees 1541 | //CoxaAngle1 - Output Angle of Coxa in degrees 1542 | 1543 | void LegIK (short IKFeetPosX, short IKFeetPosY, short IKFeetPosZ, byte LegIKLegNr) 1544 | { 1545 | unsigned long IKSW2; //Length between Shoulder and Wrist, decimals = 2 1546 | unsigned long IKA14; //Angle of the line S>W with respect to the ground in radians, decimals = 4 1547 | unsigned long IKA24; //Angle of the line S>W with respect to the femur in radians, decimals = 4 1548 | short IKFeetPosXZ; //Diagonal direction from Input X and Z 1549 | #ifdef c4DOF 1550 | // these were shorts... 1551 | long TarsOffsetXZ; //Vector value \ ; 1552 | long TarsOffsetY; //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors 1553 | long TarsToGroundAngle1; //Angle between tars and ground. Note: the angle are 0 when the tars are perpendicular to the ground 1554 | long TGA_A_H4; 1555 | long TGA_B_H3; 1556 | #else 1557 | #define TarsOffsetXZ 0 // Vector value 1558 | #define TarsOffsetY 0 //Vector value / The 2 DOF IK calcs (femur and tibia) are based upon these vectors 1559 | #endif 1560 | 1561 | 1562 | long Temp1; 1563 | long Temp2; 1564 | long T3; 1565 | 1566 | //Calculate IKCoxaAngle and IKFeetPosXZ 1567 | GetATan2 (IKFeetPosX, IKFeetPosZ); 1568 | CoxaAngle1[LegIKLegNr] = (((long)Atan4*180) / 3141) + (short)pgm_read_word(&cCoxaAngle1[LegIKLegNr]); 1569 | 1570 | //Length between the Coxa and tars [foot] 1571 | IKFeetPosXZ = XYhyp2/c2DEC; 1572 | #ifdef c4DOF 1573 | // Some legs may have the 4th DOF and some may not, so handle this here... 1574 | //Calc the TarsToGroundAngle1: 1575 | if ((byte)pgm_read_byte(&cTarsLength[LegIKLegNr])) { // We allow mix of 3 and 4 DOF legs... 1576 | TarsToGroundAngle1 = -cTarsConst + cTarsMulti*IKFeetPosY + ((long)(IKFeetPosXZ*cTarsFactorA))/c1DEC - ((long)(IKFeetPosXZ*IKFeetPosY)/(cTarsFactorB)); 1577 | if (IKFeetPosY < 0) //Always compensate TarsToGroundAngle1 when IKFeetPosY it goes below zero 1578 | TarsToGroundAngle1 = TarsToGroundAngle1 - ((long)(IKFeetPosY*cTarsFactorC)/c1DEC); //TGA base, overall rule 1579 | if (TarsToGroundAngle1 > 400) 1580 | TGA_B_H3 = 200 + (TarsToGroundAngle1/2); 1581 | else 1582 | TGA_B_H3 = TarsToGroundAngle1; 1583 | 1584 | if (TarsToGroundAngle1 > 300) 1585 | TGA_A_H4 = 240 + (TarsToGroundAngle1/5); 1586 | else 1587 | TGA_A_H4 = TarsToGroundAngle1; 1588 | 1589 | if (IKFeetPosY > 0) //Only compensate the TarsToGroundAngle1 when it exceed 30 deg (A, H4 PEP note) 1590 | TarsToGroundAngle1 = TGA_A_H4; 1591 | else if (((IKFeetPosY <= 0) & (IKFeetPosY > -10))) // linear transition between case H3 and H4 (from PEP: H4-K5*(H3-H4)) 1592 | TarsToGroundAngle1 = (TGA_A_H4 -(((long)IKFeetPosY*(TGA_B_H3-TGA_A_H4))/c1DEC)); 1593 | else //IKFeetPosY <= -10, Only compensate TGA1 when it exceed 40 deg 1594 | TarsToGroundAngle1 = TGA_B_H3; 1595 | 1596 | //Calc Tars Offsets: 1597 | GetSinCos(TarsToGroundAngle1); 1598 | TarsOffsetXZ = ((long)sin4*(byte)pgm_read_byte(&cTarsLength[LegIKLegNr]))/c4DEC; 1599 | TarsOffsetY = ((long)cos4*(byte)pgm_read_byte(&cTarsLength[LegIKLegNr]))/c4DEC; 1600 | } 1601 | else { 1602 | TarsOffsetXZ = 0; 1603 | TarsOffsetY = 0; 1604 | } 1605 | #endif 1606 | 1607 | //Using GetAtan2 for solving IKA1 and IKSW 1608 | //IKA14 - Angle between SW line and the ground in radians 1609 | IKA14 = GetATan2 (IKFeetPosY-TarsOffsetY, IKFeetPosXZ-(byte)pgm_read_byte(&cCoxaLength[LegIKLegNr])-TarsOffsetXZ); 1610 | 1611 | //IKSW2 - Length between femur axis and tars 1612 | IKSW2 = XYhyp2; 1613 | 1614 | //IKA2 - Angle of the line S>W with respect to the femur in radians 1615 | Temp1 = ((((long)(byte)pgm_read_byte(&cFemurLength[LegIKLegNr])*(byte)pgm_read_byte(&cFemurLength[LegIKLegNr])) - ((long)(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])*(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])))*c4DEC + ((long)IKSW2*IKSW2)); 1616 | Temp2 = (long)(2*(byte)pgm_read_byte(&cFemurLength[LegIKLegNr]))*c2DEC * (unsigned long)IKSW2; 1617 | T3 = Temp1 / (Temp2/c4DEC); 1618 | IKA24 = GetArcCos (T3 ); 1619 | #ifdef DEBUG_IK 1620 | if (g_fDebugOutput && g_InControlState.fRobotOn) { 1621 | DBGSerial.print(" "); 1622 | DBGSerial.print(Temp1, DEC); 1623 | DBGSerial.print(" "); 1624 | DBGSerial.print(Temp2, DEC); 1625 | DBGSerial.print(" "); 1626 | DBGSerial.print(T3, DEC); 1627 | DBGSerial.print(" "); 1628 | DBGSerial.print(IKSW2, DEC); 1629 | DBGSerial.print(" "); 1630 | DBGSerial.print(IKA14, DEC); 1631 | DBGSerial.print(" "); 1632 | DBGSerial.print(IKA24, DEC); 1633 | } 1634 | #endif 1635 | //IKFemurAngle 1636 | #ifdef OPT_WALK_UPSIDE_DOWN 1637 | if (g_fRobotUpsideDown) 1638 | FemurAngle1[LegIKLegNr] = (long)(IKA14 + IKA24) * 180 / 3141 - 900 + CFEMURHORNOFFSET1(LegIKLegNr);//Inverted, up side down 1639 | else 1640 | FemurAngle1[LegIKLegNr] = -(long)(IKA14 + IKA24) * 180 / 3141 + 900 + CFEMURHORNOFFSET1(LegIKLegNr);//Normal 1641 | #else 1642 | FemurAngle1[LegIKLegNr] = -(long)(IKA14 + IKA24) * 180 / 3141 + 900 + CFEMURHORNOFFSET1(LegIKLegNr);//Normal 1643 | #endif 1644 | 1645 | //IKTibiaAngle 1646 | Temp1 = ((((long)(byte)pgm_read_byte(&cFemurLength[LegIKLegNr])*(byte)pgm_read_byte(&cFemurLength[LegIKLegNr])) + ((long)(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])*(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])))*c4DEC - ((long)IKSW2*IKSW2)); 1647 | Temp2 = 2 * ((long)((byte)pgm_read_byte(&cFemurLength[LegIKLegNr]))) * (long)((byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])); 1648 | GetArcCos (Temp1 / Temp2); 1649 | #ifdef DEBUG_IK 1650 | if (g_fDebugOutput && g_InControlState.fRobotOn) { 1651 | DBGSerial.print("="); 1652 | DBGSerial.print(Temp1, DEC); 1653 | DBGSerial.print(" "); 1654 | DBGSerial.print(Temp2, DEC); 1655 | DBGSerial.print(" "); 1656 | DBGSerial.print(AngleRad4, DEC); 1657 | } 1658 | #endif 1659 | 1660 | #ifdef OPT_WALK_UPSIDE_DOWN 1661 | if (g_fRobotUpsideDown) 1662 | TibiaAngle1[LegIKLegNr] = (1800-(long)AngleRad4*180/3141 + CTIBIAHORNOFFSET1(LegIKLegNr));//Full range tibia, wrong side (up side down) 1663 | else 1664 | TibiaAngle1[LegIKLegNr] = -(1800-(long)AngleRad4*180/3141 + CTIBIAHORNOFFSET1(LegIKLegNr));//Full range tibia, right side (up side up) 1665 | #else 1666 | #ifdef PHANTOMX_V2 // BugBug:: cleaner way? 1667 | TibiaAngle1[LegIKLegNr] = -(1450-(long)AngleRad4*180/3141 + CTIBIAHORNOFFSET1(LegIKLegNr)); //!!!!!!!!!!!!145 instead of 1800 1668 | #else 1669 | TibiaAngle1[LegIKLegNr] = -(900-(long)AngleRad4*180/3141 + CTIBIAHORNOFFSET1(LegIKLegNr)); 1670 | #endif 1671 | #endif 1672 | 1673 | //Set the Solution quality 1674 | if(IKSW2 < ((word)((byte)pgm_read_byte(&cFemurLength[LegIKLegNr])+(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr])-30)*c2DEC)) 1675 | IKSolution = 1; 1676 | else 1677 | { 1678 | if(IKSW2 < ((word)((byte)pgm_read_byte(&cFemurLength[LegIKLegNr])+(byte)pgm_read_byte(&cTibiaLength[LegIKLegNr]))*c2DEC)) 1679 | IKSolutionWarning = 1; 1680 | else 1681 | IKSolutionError = 1 ; 1682 | } 1683 | #ifdef DEBUG 1684 | if (g_fDebugOutput && g_InControlState.fRobotOn) { 1685 | DBGSerial.print("("); 1686 | DBGSerial.print(IKFeetPosX, DEC); 1687 | DBGSerial.print(","); 1688 | DBGSerial.print(IKFeetPosY, DEC); 1689 | DBGSerial.print(","); 1690 | DBGSerial.print(IKFeetPosZ, DEC); 1691 | DBGSerial.print(")=<"); 1692 | DBGSerial.print(CoxaAngle1[LegIKLegNr], DEC); 1693 | DBGSerial.print(","); 1694 | DBGSerial.print(FemurAngle1[LegIKLegNr], DEC); 1695 | DBGSerial.print(","); 1696 | DBGSerial.print(TibiaAngle1[LegIKLegNr], DEC); 1697 | DBGSerial.print(">"); 1698 | DBGSerial.print((IKSolutionError<<2)+(IKSolutionWarning<<1)+IKSolution, DEC); 1699 | if (LegIKLegNr == (CNT_LEGS-1)) 1700 | DBGSerial.println(); 1701 | } 1702 | #endif 1703 | } 1704 | 1705 | 1706 | // //////////////////////////////////////////////////////////////////////// 1707 | // ////////////////////////// CHECK ANGLES //////////////////////////// 1708 | // //////////////////////////////////////////////////////////////////////// 1709 | // Check the angles of the servos against the mechanical constraints defined in the robot configuration. 1710 | 1711 | short CheckServoAngleBounds(short sID, short sVal, const short *sMin PROGMEM, const short *sMax PROGMEM) { 1712 | 1713 | // Pull into simple function as so I can report errors on debug 1714 | // Note ID is bogus, but something to let me know which one. 1715 | short s = (short)pgm_read_word(sMin); 1716 | if (sVal < s) { 1717 | #ifdef DEBUG_BOUNDS 1718 | if (g_fDebugOutput) { 1719 | DBGSerial.print(sID, DEC); 1720 | DBGSerial.print(" "); 1721 | DBGSerial.print(sVal, DEC); 1722 | DBGSerial.print("<"); 1723 | DBGSerial.println(s, DEC); 1724 | } 1725 | #endif 1726 | return s; 1727 | } 1728 | 1729 | s = (short)pgm_read_word(sMax); 1730 | if (sVal > s) { 1731 | #ifdef DEBUG_BOUNDS 1732 | if (g_fDebugOutput) { 1733 | DBGSerial.print(sID, DEC); 1734 | DBGSerial.print(" "); 1735 | DBGSerial.print(sVal, DEC); 1736 | DBGSerial.print(">"); 1737 | DBGSerial.println(s, DEC); 1738 | } 1739 | #endif 1740 | return s; 1741 | } 1742 | return sVal; 1743 | 1744 | } 1745 | 1746 | void CheckAngles(void) 1747 | { 1748 | #ifndef SERVOS_DO_MINMAX 1749 | short s = 0; // BUGBUG just some index so we can get a hint who errored out 1750 | for (LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) 1751 | { 1752 | CoxaAngle1[LegIndex] = CheckServoAngleBounds(s++, CoxaAngle1[LegIndex], &cCoxaMin1[LegIndex], &cCoxaMax1[LegIndex]); 1753 | FemurAngle1[LegIndex] = CheckServoAngleBounds(s++, FemurAngle1[LegIndex], &cFemurMin1[LegIndex], &cFemurMax1[LegIndex]); 1754 | TibiaAngle1[LegIndex] = CheckServoAngleBounds(s++, TibiaAngle1[LegIndex], &cTibiaMin1[LegIndex], &cTibiaMax1[LegIndex]); 1755 | #ifdef c4DOF 1756 | if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) { // We allow mix of 3 and 4 DOF legs... 1757 | TarsAngle1[LegIndex] = CheckServoAngleBounds(s++, TarsAngle1[LegIndex], &cTarsMin1[LegIndex], &cTarsMax1[LegIndex]); 1758 | } 1759 | #endif 1760 | } 1761 | #endif 1762 | } 1763 | 1764 | 1765 | //-------------------------------------------------------------------- 1766 | // SmoothControl (From Zenta) - This function makes the body 1767 | // rotation and translation much smoother 1768 | //-------------------------------------------------------------------- 1769 | short SmoothControl (short CtrlMoveInp, short CtrlMoveOut, byte CtrlDivider) 1770 | { 1771 | 1772 | if (CtrlMoveOut < (CtrlMoveInp - 4)) 1773 | return CtrlMoveOut + abs((CtrlMoveOut - CtrlMoveInp)/CtrlDivider); 1774 | else if (CtrlMoveOut > (CtrlMoveInp + 4)) 1775 | return CtrlMoveOut - abs((CtrlMoveOut - CtrlMoveInp)/CtrlDivider); 1776 | 1777 | return CtrlMoveInp; 1778 | } 1779 | 1780 | 1781 | //-------------------------------------------------------------------- 1782 | // GetLegsXZLength - 1783 | //-------------------------------------------------------------------- 1784 | word g_wLegsXZLength = 0xffff; 1785 | word GetLegsXZLength(void) 1786 | { 1787 | // Could save away or could do a little math on one leg... 1788 | if (g_wLegsXZLength != 0xffff) 1789 | return g_wLegsXZLength; 1790 | 1791 | return isqrt32((LegPosX[0] * LegPosX[0]) + (LegPosZ[0] * LegPosZ[0])); 1792 | } 1793 | 1794 | 1795 | //-------------------------------------------------------------------- 1796 | // AdjustLegPositions() - Will adjust the init leg positions to the 1797 | // width passed in. 1798 | //-------------------------------------------------------------------- 1799 | #ifndef MIN_XZ_LEG_ADJUST 1800 | #define MIN_XZ_LEG_ADJUST (cCoxaLength[0]) // don't go inside coxa... 1801 | #endif 1802 | 1803 | #ifndef MAX_XZ_LEG_ADJUST 1804 | #define MAX_XZ_LEG_ADJUST (cCoxaLength[0]+cTibiaLength[0] + cFemurLength[0]/4) 1805 | #endif 1806 | 1807 | void AdjustLegPositions(word XZLength1) 1808 | { 1809 | //now lets see what happens when we change the leg positions... 1810 | if (XZLength1 > MAX_XZ_LEG_ADJUST) 1811 | XZLength1 = MAX_XZ_LEG_ADJUST; 1812 | if (XZLength1 < MIN_XZ_LEG_ADJUST) 1813 | XZLength1 = MIN_XZ_LEG_ADJUST; 1814 | 1815 | // see if same length as when we came in 1816 | if (XZLength1 == g_wLegsXZLength) 1817 | return; 1818 | 1819 | g_wLegsXZLength = XZLength1; 1820 | 1821 | 1822 | for (uint8_t LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) { 1823 | #ifdef DEBUG 1824 | if (g_fDebugOutput) { 1825 | DBGSerial.print("("); 1826 | DBGSerial.print(LegPosX[LegIndex], DEC); 1827 | DBGSerial.print(","); 1828 | DBGSerial.print(LegPosZ[LegIndex], DEC); 1829 | DBGSerial.print(")->"); 1830 | } 1831 | #endif 1832 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 1833 | GetSinCos(g_InControlState.aCoxaInitAngle1[LegIndex]); 1834 | #else 1835 | #ifdef cRRInitCoxaAngle1 // We can set different angles for the legs than just where they servo horns are set... 1836 | GetSinCos((short)pgm_read_word(&cCoxaInitAngle1[LegIndex])); 1837 | #else 1838 | GetSinCos((short)pgm_read_word(&cCoxaAngle1[LegIndex])); 1839 | #endif 1840 | #endif 1841 | LegPosX[LegIndex] = ((long)((long)cos4 * XZLength1))/c4DEC; //Set start positions for each leg 1842 | LegPosZ[LegIndex] = -((long)((long)sin4 * XZLength1))/c4DEC; 1843 | #ifdef DEBUG 1844 | if (g_fDebugOutput) { 1845 | DBGSerial.print("("); 1846 | DBGSerial.print(LegPosX[LegIndex], DEC); 1847 | DBGSerial.print(","); 1848 | DBGSerial.print(LegPosZ[LegIndex], DEC); 1849 | DBGSerial.print(") "); 1850 | } 1851 | #endif 1852 | } 1853 | #ifdef DEBUG 1854 | if (g_fDebugOutput) { 1855 | DBGSerial.println(""); 1856 | } 1857 | #endif 1858 | // Make sure we cycle through one gait to have the legs all move into their new locations... 1859 | g_InControlState.ForceGaitStepCnt = g_InControlState.gaitCur.StepsInGait; 1860 | } 1861 | 1862 | //-------------------------------------------------------------------- 1863 | // ResetLegInitAngles - This is used when we allow the user to 1864 | // adjust the leg position angles. This resets to what it was when the 1865 | // the program was started. 1866 | //-------------------------------------------------------------------- 1867 | void ResetLegInitAngles(void) 1868 | { 1869 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 1870 | for (int LegIndex=0; LegIndex < CNT_LEGS; LegIndex++) { 1871 | #ifdef cRRInitCoxaAngle1 // We can set different angles for the legs than just where they servo horns are set... 1872 | g_InControlState.aCoxaInitAngle1[LegIndex] = (short)pgm_read_word(&cCoxaInitAngle1[LegIndex]); 1873 | #else 1874 | g_InControlState.aCoxaInitAngle1[LegIndex] = (short)pgm_read_word(&cCoxaAngle1[LegIndex]); 1875 | #endif 1876 | } 1877 | g_wLegsXZLength = 0xffff; 1878 | #endif 1879 | } 1880 | 1881 | //-------------------------------------------------------------------- 1882 | // ResetLegInitAngles - This is used when we allow the user to 1883 | //-------------------------------------------------------------------- 1884 | void RotateLegInitAngles (int iDeltaAngle) 1885 | { 1886 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 1887 | for (int LegIndex=0; LegIndex < CNT_LEGS; LegIndex++) { 1888 | // We will use the cCoxaAngle1 array to know which direction the legs logically are 1889 | // If the initial angle is 0 don't mess with. Hex middle legs... 1890 | if ((short)pgm_read_word(&cCoxaAngle1[LegIndex]) > 0) 1891 | g_InControlState.aCoxaInitAngle1[LegIndex] += iDeltaAngle; 1892 | else if ((short)pgm_read_word(&cCoxaAngle1[LegIndex]) < 0) 1893 | g_InControlState.aCoxaInitAngle1[LegIndex] -= iDeltaAngle; 1894 | 1895 | // Make sure we don't exceed some min/max angles. 1896 | // Right now hard coded to +-70 degrees... Should probably load 1897 | if (g_InControlState.aCoxaInitAngle1[LegIndex] > 700) 1898 | g_InControlState.aCoxaInitAngle1[LegIndex] = 700; 1899 | else if (g_InControlState.aCoxaInitAngle1[LegIndex] < -700) 1900 | g_InControlState.aCoxaInitAngle1[LegIndex] = -700; 1901 | } 1902 | g_wLegsXZLength = 0xffff; 1903 | #endif 1904 | } 1905 | 1906 | //-------------------------------------------------------------------- 1907 | // AdjustLegPositionsToBodyHeight() - Will try to adjust the position of the legs 1908 | // to be appropriate for the current y location of the body... 1909 | //-------------------------------------------------------------------- 1910 | 1911 | uint8_t g_iLegInitIndex = 0x00; // remember which index we are currently using... 1912 | 1913 | void AdjustLegPositionsToBodyHeight() 1914 | { 1915 | #ifdef CNT_HEX_INITS 1916 | // Lets see which of our units we should use... 1917 | // Note: We will also limit our body height here... 1918 | if (g_InControlState.BodyPos.y > (short)pgm_read_byte(&g_abHexMaxBodyY[CNT_HEX_INITS-1])) 1919 | g_InControlState.BodyPos.y = (short)pgm_read_byte(&g_abHexMaxBodyY[CNT_HEX_INITS-1]); 1920 | 1921 | uint8_t i; 1922 | word XZLength1 = pgm_read_byte(&g_abHexIntXZ[CNT_HEX_INITS-1]); 1923 | for(i = 0; i < (CNT_HEX_INITS-1); i++) { // Don't need to look at last entry as we already init to assume this one... 1924 | if (g_InControlState.BodyPos.y <= (short)pgm_read_byte(&g_abHexMaxBodyY[i])) { 1925 | XZLength1 = pgm_read_byte(&g_abHexIntXZ[i]); 1926 | break; 1927 | } 1928 | } 1929 | if (i != g_iLegInitIndex) { 1930 | g_iLegInitIndex = i; // remember the current index... 1931 | 1932 | // Call off to helper function to do the work. 1933 | #ifdef DEBUG 1934 | if (g_fDebugOutput) { 1935 | DBGSerial.print("ALPTBH: "); 1936 | DBGSerial.print(g_InControlState.BodyPos.y, DEC); 1937 | DBGSerial.print(" "); 1938 | DBGSerial.print(XZLength1, DEC); 1939 | } 1940 | #endif 1941 | AdjustLegPositions(XZLength1); 1942 | } 1943 | #endif // CNT_HEX_INITS 1944 | 1945 | } 1946 | 1947 | //============================================================================== 1948 | // SoundNoTimer - Quick and dirty tone function to try to output a frequency 1949 | // to a speaker for some simple sounds. 1950 | //============================================================================== 1951 | #ifdef SOUND_PIN 1952 | void SoundNoTimer(unsigned long duration, unsigned int frequency) 1953 | { 1954 | #ifndef __MK20DX256__ 1955 | #ifdef __AVR__ 1956 | volatile uint8_t *pin_port; 1957 | volatile uint8_t pin_mask; 1958 | #else 1959 | volatile uint32_t *pin_port; 1960 | volatile uint16_t pin_mask; 1961 | #endif 1962 | long toggle_count = 0; 1963 | long lusDelayPerHalfCycle; 1964 | 1965 | // Set the pinMode as OUTPUT 1966 | pinMode(SOUND_PIN, OUTPUT); 1967 | 1968 | pin_port = portOutputRegister(digitalPinToPort(SOUND_PIN)); 1969 | pin_mask = digitalPinToBitMask(SOUND_PIN); 1970 | 1971 | toggle_count = 2 * frequency * duration / 1000; 1972 | lusDelayPerHalfCycle = 1000000L/(frequency * 2); 1973 | 1974 | // if we are using an 8 bit timer, scan through prescalars to find the best fit 1975 | while (toggle_count--) { 1976 | // toggle the pin 1977 | *pin_port ^= pin_mask; 1978 | 1979 | // delay a half cycle 1980 | delayMicroseconds(lusDelayPerHalfCycle); 1981 | } 1982 | *pin_port &= ~(pin_mask); // keep pin low after stop 1983 | #else 1984 | // The tone command does sort of work, but does not play multiple sounds smoothly 1985 | // tone(SOUND_PIN, frequency, duration); // Try the arduino library 1986 | // delay(duration); 1987 | // Try to get something working on DUE... 1988 | long toggle_count = 0; 1989 | long lusDelayPerHalfCycle; 1990 | boolean fHigh = false; 1991 | // Set the pinMode as OUTPUT 1992 | pinMode(SOUND_PIN, OUTPUT); 1993 | digitalWrite(SOUND_PIN, LOW); 1994 | toggle_count = 2 * frequency * duration / 1000; 1995 | lusDelayPerHalfCycle = 1000000L/(frequency * 2); 1996 | 1997 | // if we are using an 8 bit timer, scan through prescalars to find the best fit 1998 | while (toggle_count--) { 1999 | // toggle the pin 2000 | fHigh = !fHigh; 2001 | digitalWrite(SOUND_PIN, fHigh? LOW : HIGH); 2002 | // delay a half cycle 2003 | delayMicroseconds(lusDelayPerHalfCycle); 2004 | } 2005 | digitalWrite(SOUND_PIN, LOW); 2006 | 2007 | #endif 2008 | } 2009 | 2010 | void MSound(byte cNotes, ...) 2011 | { 2012 | va_list ap; 2013 | unsigned int uDur; 2014 | unsigned int uFreq; 2015 | va_start(ap, cNotes); 2016 | 2017 | while (cNotes > 0) { 2018 | uDur = va_arg(ap, unsigned int); 2019 | uFreq = va_arg(ap, unsigned int); 2020 | SoundNoTimer(uDur, uFreq); 2021 | cNotes--; 2022 | } 2023 | va_end(ap); 2024 | } 2025 | #else 2026 | void MSound(byte cNotes, ...) 2027 | { 2028 | }; 2029 | #endif 2030 | 2031 | #ifdef OPT_TERMINAL_MONITOR 2032 | #ifdef OPT_DUMP_EEPROM 2033 | extern void DumpEEPROMCmd(byte *pszCmdLine); 2034 | #endif 2035 | #ifdef QUADMODE 2036 | extern void UpdateGaitCmd(byte *pszCmdLine); 2037 | #endif 2038 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 2039 | extern void UpdateInitialPosAndAngCmd(byte *pszCmdLine); 2040 | #endif 2041 | 2042 | //============================================================================== 2043 | // TerminalMonitor - Simple background task checks to see if the user is asking 2044 | // us to do anything, like update debug levels ore the like. 2045 | //============================================================================== 2046 | boolean TerminalMonitor(void) 2047 | { 2048 | byte szCmdLine[20]; // currently pretty simple command lines... 2049 | byte ich; 2050 | int ch; 2051 | // See if we need to output a prompt. 2052 | if (g_fShowDebugPrompt) { 2053 | DBGSerial.println(F("Arduino Phoenix Monitor")); 2054 | DBGSerial.println(F("D - Toggle debug on or off")); 2055 | #ifdef OPT_DUMP_EEPROM 2056 | DBGSerial.println(F("E - Dump EEPROM")); 2057 | #endif 2058 | #ifdef QUADMODE 2059 | // DBGSerial.println(F("B ")); 2060 | DBGSerial.println(F("G ST NL RR RF LR LF")); 2061 | #endif 2062 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 2063 | DBGSerial.println(F("I pos ang")); 2064 | #endif 2065 | #ifdef OPT_TERMINAL_MONITOR_IC // Allow the input controller to define stuff as well 2066 | g_InputController.ShowTerminalCommandList(); 2067 | #endif 2068 | 2069 | // Let the Servo driver show it's own set of commands... 2070 | g_ServoDriver.ShowTerminalCommandList(); 2071 | g_fShowDebugPrompt = false; 2072 | } 2073 | 2074 | // First check to see if there is any characters to process. 2075 | if ((ich = DBGSerial.available())) { 2076 | ich = 0; 2077 | // For now assume we receive a packet of data from serial monitor, as the user has 2078 | // to click the send button... 2079 | for (ich=0; ich < sizeof(szCmdLine); ich++) { 2080 | ch = DBGSerial.read(); // get the next character 2081 | if ((ch == -1) || ((ch >= 10) && (ch <= 15))) 2082 | break; 2083 | szCmdLine[ich] = ch; 2084 | } 2085 | szCmdLine[ich] = '\0'; // go ahead and null terminate it... 2086 | 2087 | // Remove any extra EOL characters that may have been added 2088 | for (;;) { 2089 | ch = DBGSerial.peek(); 2090 | if ((ch >= 10) && (ch <= 15)) 2091 | DBGSerial.read(); 2092 | else 2093 | break; 2094 | } 2095 | if (ich) { 2096 | DBGSerial.print(F("Serial Cmd Line:")); 2097 | DBGSerial.write(szCmdLine, ich); 2098 | DBGSerial.println(F("")); 2099 | } 2100 | // So see what are command is. 2101 | if (!ich) { 2102 | g_fShowDebugPrompt = true; 2103 | } 2104 | else if ((ich == 1) && ((szCmdLine[0] == 'd') || (szCmdLine[0] == 'D'))) { 2105 | g_fDebugOutput = !g_fDebugOutput; 2106 | if (g_fDebugOutput) 2107 | DBGSerial.println(F("Debug is on")); 2108 | else 2109 | DBGSerial.println(F("Debug is off")); 2110 | } 2111 | #ifdef OPT_DUMP_EEPROM 2112 | else if (((szCmdLine[0] == 'e') || (szCmdLine[0] == 'E'))) { 2113 | DumpEEPROMCmd(szCmdLine); 2114 | } 2115 | #endif 2116 | #ifdef QUADMODE 2117 | else if (((szCmdLine[0] == 'g') || (szCmdLine[0] == 'G'))) { 2118 | UpdateGaitCmd(szCmdLine); 2119 | } 2120 | #endif 2121 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 2122 | else if (((szCmdLine[0] == 'i') || (szCmdLine[0] == 'I'))) { 2123 | UpdateInitialPosAndAngCmd(szCmdLine); 2124 | } 2125 | #endif 2126 | #ifdef OPT_TERMINAL_MONITOR_IC // Allow the input controller to define stuff as well 2127 | else if (g_InputController.ProcessTerminalCommand(szCmdLine, ich)) 2128 | ; // See if the Input controller has added commands... 2129 | #endif 2130 | 2131 | else 2132 | { 2133 | g_ServoDriver.ProcessTerminalCommand(szCmdLine, ich); 2134 | } 2135 | 2136 | return true; 2137 | } 2138 | return false; 2139 | } 2140 | 2141 | 2142 | //-------------------------------------------------------------------- 2143 | // DumpEEPROM 2144 | //-------------------------------------------------------------------- 2145 | #ifdef OPT_DUMP_EEPROM 2146 | byte g_bEEPromDumpMode = 0; // assume mode 0 - hex dump 2147 | word g_wEEPromDumpStart = 0; // where to start dumps from 2148 | byte g_bEEPromDumpCnt = 16; // how much to dump at a time 2149 | 2150 | void DumpEEPROM() { 2151 | byte i; 2152 | word wDumpCnt = g_bEEPromDumpCnt; 2153 | 2154 | while (wDumpCnt) { 2155 | DBGSerial.print(g_wEEPromDumpStart, HEX); 2156 | DBGSerial.print(" - "); 2157 | 2158 | // First in Hex 2159 | for (i = 0; (i < 16) && (i < wDumpCnt); i ++) { 2160 | byte b; 2161 | b = EEPROM.read(g_wEEPromDumpStart+i); 2162 | DBGSerial.print(b, HEX); 2163 | DBGSerial.print(" "); 2164 | } 2165 | // Next in Ascii 2166 | DBGSerial.print(" : "); 2167 | for (i = 0; (i < 16) && (i < wDumpCnt); i ++) { 2168 | byte b; 2169 | b = EEPROM.read(g_wEEPromDumpStart+i); 2170 | if ((b > 0x1f) && (b < 0x7f)) 2171 | DBGSerial.write(b); 2172 | else 2173 | DBGSerial.print("."); 2174 | } 2175 | DBGSerial.println(""); 2176 | g_wEEPromDumpStart += i; // how many bytes we output 2177 | wDumpCnt -= i; // How many more to go... 2178 | } 2179 | 2180 | } 2181 | #endif 2182 | 2183 | //-------------------------------------------------------------------- 2184 | // GetCmdLineNum - passed pointer to pointer so we can update... 2185 | //-------------------------------------------------------------------- 2186 | long GetCmdLineNum(byte **ppszCmdLine) { 2187 | byte *psz = *ppszCmdLine; 2188 | long iVal = 0; 2189 | int iSign = 1; 2190 | 2191 | // Ignore any blanks 2192 | while (*psz == ' ') 2193 | psz++; 2194 | 2195 | // See if Hex value passed in 2196 | if ((*psz == '0') && ((*(psz+1) == 'x') || (*(psz+1) == 'X'))) { 2197 | // Hex mode 2198 | psz += 2; // get over 0x 2199 | for (;;) { 2200 | if ((*psz >= '0') && (*psz <= '9')) 2201 | iVal = iVal * 16 + *psz++ - '0'; 2202 | else if ((*psz >= 'a') && (*psz <= 'f')) 2203 | iVal = iVal * 16 + *psz++ - 'a' + 10; 2204 | else if ((*psz >= 'A') && (*psz <= 'F')) 2205 | iVal = iVal * 16 + *psz++ - 'A' + 10; 2206 | else 2207 | break; 2208 | } 2209 | 2210 | } 2211 | else { 2212 | // decimal mode 2213 | if (*psz == '-') { 2214 | iSign = -1; 2215 | psz++; 2216 | } 2217 | 2218 | while ((*psz >= '0') && (*psz <= '9')) 2219 | iVal = iVal * 10 + *psz++ - '0'; 2220 | } 2221 | *ppszCmdLine = psz; // update command line pointer 2222 | return iSign * iVal; 2223 | 2224 | } 2225 | 2226 | #ifdef OPT_DUMP_EEPROM 2227 | //-------------------------------------------------------------------- 2228 | // DumpEEPROMCmd 2229 | //-------------------------------------------------------------------- 2230 | void DumpEEPROMCmd(byte *pszCmdLine) { 2231 | // first byte can be H for hex or W for words... 2232 | if (!*++pszCmdLine) // Need to get past the command letter first... 2233 | DumpEEPROM(); 2234 | else if ((*pszCmdLine == 'h') || (*pszCmdLine == 'H')) 2235 | g_bEEPromDumpMode = 0; 2236 | else if ((*pszCmdLine == 'w') || (*pszCmdLine == 'W')) 2237 | g_bEEPromDumpMode = 0; 2238 | 2239 | else { 2240 | // First argument should be the start location to dump 2241 | g_wEEPromDumpStart = GetCmdLineNum(&pszCmdLine); 2242 | 2243 | // If the next byte is an "=" may try to do updates... 2244 | if (*pszCmdLine == '=') { 2245 | // make sure we don't get stuck in a loop... 2246 | byte *psz = pszCmdLine; 2247 | word w; 2248 | while (*psz) { 2249 | w = GetCmdLineNum(&psz); 2250 | if (psz == pszCmdLine) 2251 | break; // not valid stuff so bail! 2252 | pszCmdLine = psz; // remember how far we got... 2253 | 2254 | EEPROM.write(g_wEEPromDumpStart++, w & 0xff); 2255 | } 2256 | } 2257 | else { 2258 | if (*pszCmdLine == ' ') { // A blank assume we have a count... 2259 | g_bEEPromDumpCnt = GetCmdLineNum(&pszCmdLine); 2260 | } 2261 | } 2262 | DumpEEPROM(); 2263 | } 2264 | } 2265 | #endif 2266 | 2267 | #ifdef QUADMODE 2268 | //-------------------------------------------------------------------- 2269 | // UpdateGaitCmd 2270 | //-------------------------------------------------------------------- 2271 | void UpdateGaitCmd(byte *pszCmdLine) { 2272 | // If no other parameters, show current state 2273 | if (!*++pszCmdLine) { // Need to get past the command letter first... 2274 | DBGSerial.print("St: "); 2275 | DBGSerial.print(g_InControlState.gaitCur.StepsInGait, DEC); 2276 | DBGSerial.print(" "); 2277 | DBGSerial.print(g_InControlState.gaitCur.NrLiftedPos, DEC); 2278 | DBGSerial.print(" "); 2279 | DBGSerial.print(g_InControlState.gaitCur.GaitLegNr[cRR], DEC); 2280 | DBGSerial.print(" "); 2281 | DBGSerial.print(g_InControlState.gaitCur.GaitLegNr[cRF], DEC); 2282 | DBGSerial.print(" "); 2283 | DBGSerial.print(g_InControlState.gaitCur.GaitLegNr[cLR], DEC); 2284 | DBGSerial.print(" "); 2285 | DBGSerial.println(g_InControlState.gaitCur.GaitLegNr[cLF], DEC); 2286 | } 2287 | else { 2288 | //Argument should be New percentage 2289 | word wStepsInGait = GetCmdLineNum(&pszCmdLine); 2290 | word wLifted = GetCmdLineNum(&pszCmdLine); 2291 | 2292 | // first pass only pass in number of steps and maybe Lifted pos 2293 | if (wStepsInGait) { 2294 | if (wLifted) { 2295 | // UPdated the lifted so lets update some of the gait properties 2296 | g_InControlState.gaitCur.NrLiftedPos = wLifted; 2297 | g_InControlState.gaitCur.FrontDownPos = (wLifted+1)/2; 2298 | g_InControlState.gaitCur.LiftDivFactor = (wLifted > 4)? 4 : 2; 2299 | } 2300 | 2301 | // Assume the ordering of the gait legs here and equal spaced 2302 | g_InControlState.gaitCur.StepsInGait = wStepsInGait; 2303 | g_InControlState.gaitCur.TLDivFactor = g_InControlState.gaitCur.StepsInGait-g_InControlState.gaitCur.NrLiftedPos; 2304 | 2305 | // See if user did pass in leg positions... 2306 | g_InControlState.gaitCur.GaitLegNr[cRR] = GetCmdLineNum(&pszCmdLine); 2307 | if (g_InControlState.gaitCur.GaitLegNr[cRR]) { 2308 | g_InControlState.gaitCur.GaitLegNr[cRF] = GetCmdLineNum(&pszCmdLine); 2309 | g_InControlState.gaitCur.GaitLegNr[cLR] = GetCmdLineNum(&pszCmdLine); 2310 | g_InControlState.gaitCur.GaitLegNr[cLF] = GetCmdLineNum(&pszCmdLine); 2311 | } 2312 | else { 2313 | wStepsInGait /= 4; // equal spacing. 2314 | g_InControlState.gaitCur.GaitLegNr[cRR] = wStepsInGait / 2; 2315 | g_InControlState.gaitCur.GaitLegNr[cRF] = g_InControlState.gaitCur.GaitLegNr[cRR] + wStepsInGait; 2316 | g_InControlState.gaitCur.GaitLegNr[cLR] = g_InControlState.gaitCur.GaitLegNr[cRF] + wStepsInGait; 2317 | g_InControlState.gaitCur.GaitLegNr[cLF] = g_InControlState.gaitCur.GaitLegNr[cLR] + wStepsInGait; 2318 | } 2319 | 2320 | //g_InControlState.gaitCur.HalfLiftHeight = 3; 2321 | //g_InControlState.gaitCur.NomGaitSpeed = DEFAULT_GAIT_SPEED; 2322 | } 2323 | } 2324 | } 2325 | #endif //Quad Mode 2326 | 2327 | //-------------------------------------------------------------------- 2328 | // UpdateGaitCmd 2329 | //-------------------------------------------------------------------- 2330 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 2331 | void UpdateInitialPosAndAngCmd(byte *pszCmdLine) { 2332 | // If no other parameters, show current state 2333 | if (!*++pszCmdLine) { // Need to get past the command letter first... 2334 | DBGSerial.print("Len: "); 2335 | DBGSerial.print(GetLegsXZLength() , DEC); 2336 | DBGSerial.print(" Angs: "); 2337 | for(int LegIndex=0; LegIndex < CNT_LEGS; LegIndex++) { 2338 | DBGSerial.print(g_InControlState.aCoxaInitAngle1[LegIndex], DEC); 2339 | DBGSerial.print(" "); 2340 | } 2341 | DBGSerial.println(); 2342 | } 2343 | else { 2344 | // Get the new leg positions 2345 | word wNewLegsXZPos = GetCmdLineNum(&pszCmdLine); 2346 | if (*pszCmdLine) { 2347 | int iDeltaAngle = GetCmdLineNum(&pszCmdLine); 2348 | RotateLegInitAngles(iDeltaAngle); 2349 | } 2350 | AdjustLegPositions(wNewLegsXZPos); 2351 | 2352 | } 2353 | } 2354 | #endif 2355 | 2356 | #endif 2357 | -------------------------------------------------------------------------------- /Capers_II_PS2_SSC32/Capers_II_Driver_SSC32.h: -------------------------------------------------------------------------------- 1 | // Project: Capers II Hexapod 2 | // Author: Toglefritz 3 | // 4 | // Servo Driver - This version is setup to use the SSC-32 to control the servos. 5 | 6 | //Servo Pin numbers - May be SSC-32 or actual pins on main controller, depending on configuration. 7 | #ifdef QUADMODE 8 | const byte cCoxaPin[] PROGMEM = { 9 | cRRCoxaPin, cRFCoxaPin, cLRCoxaPin, cLFCoxaPin}; 10 | const byte cFemurPin[] PROGMEM = { 11 | cRRFemurPin, cRFFemurPin, cLRFemurPin, cLFFemurPin}; 12 | const byte cTibiaPin[] PROGMEM = { 13 | cRRTibiaPin, cRFTibiaPin, cLRTibiaPin, cLFTibiaPin}; 14 | #ifdef c4DOF 15 | const byte cTarsPin[] PROGMEM = { 16 | cRRTarsPin, cRFTarsPin, cLRTarsPin, cLFTarsPin}; 17 | #endif 18 | #else 19 | const byte cCoxaPin[] PROGMEM = { 20 | cRRCoxaPin, cRMCoxaPin, cRFCoxaPin, cLRCoxaPin, cLMCoxaPin, cLFCoxaPin}; 21 | const byte cFemurPin[] PROGMEM = { 22 | cRRFemurPin, cRMFemurPin, cRFFemurPin, cLRFemurPin, cLMFemurPin, cLFFemurPin}; 23 | const byte cTibiaPin[] PROGMEM = { 24 | cRRTibiaPin, cRMTibiaPin, cRFTibiaPin, cLRTibiaPin, cLMTibiaPin, cLFTibiaPin}; 25 | #ifdef c4DOF 26 | const byte cTarsPin[] PROGMEM = { 27 | cRRTarsPin, cRMTarsPin, cRFTarsPin, cLRTarsPin, cLMTarsPin, cLFTarsPin}; 28 | #endif 29 | #endif 30 | 31 | 32 | // Add support for running on non-mega Arduino boards as well. 33 | #ifdef __AVR__ 34 | #if not defined(UBRR1H) 35 | #if cSSC_IN == 0 36 | #define SSCSerial Serial 37 | #else 38 | SoftwareSerial SSCSerial(cSSC_IN, cSSC_OUT); 39 | #endif 40 | #endif 41 | #endif 42 | 43 | //============================================================================= 44 | // Global - Local to this file only... 45 | //============================================================================= 46 | 47 | // definition of some helper functions 48 | extern int SSCRead (byte* pb, int cb, word wTimeout, word wEOL); 49 | 50 | 51 | //-------------------------------------------------------------------- 52 | //Init 53 | //-------------------------------------------------------------------- 54 | void ServoDriver::Init(void) { 55 | SSCSerial.begin(cSSC_BAUD); 56 | 57 | // Lets do the check for GP Enabled here... 58 | #ifdef OPT_GPPLAYER 59 | char abT[4]; // give a nice large buffer. 60 | byte cbRead; 61 | 62 | _fGPEnabled = false; // starts off assuming that it is not enabled... 63 | _fGPActive = false; 64 | 65 | #ifdef __AVR__ 66 | #if not defined(UBRR1H) 67 | #if cSSC_IN != 0 68 | SSCSerial.listen(); 69 | #endif 70 | #endif 71 | #endif 72 | // Instead of hard checking version numbers instead ask it for 73 | // status of one of the players. If we do not get a response... 74 | // probably does not support 75 | SSCSerial.println(F("QPL0")); 76 | cbRead = SSCRead((byte*)abT, 4, 25000, (word)-1); 77 | 78 | #ifdef DBGSerial 79 | DBGSerial.print(F("Check GP Enable: ")); 80 | DBGSerial.println(cbRead, DEC); 81 | #endif 82 | if (cbRead == 4) 83 | _fGPEnabled = true; // starts off assuming that it is not enabled... 84 | else 85 | MSound (2, 40, 2500, 40, 2500); 86 | #endif 87 | #ifdef cVoltagePin 88 | // Prime the voltage values... 89 | for (byte i=0; i < 8; i++) 90 | GetBatteryVoltage(); 91 | 92 | #endif 93 | } 94 | //-------------------------------------------------------------------- 95 | //GetBatteryVoltage - Maybe should try to minimize when this is called 96 | // as it uses the serial port... Maybe only when we are not interpolating 97 | // or if maybe some minimum time has elapsed... 98 | //-------------------------------------------------------------------- 99 | 100 | #ifdef cVoltagePin 101 | #ifndef CVADR1 102 | #define CVADR1 30 // VD Resistor 1 - reduced as only need ratio... 30K and 10K 103 | #define CVADR2 10 // VD Resistor 2 104 | #endif 105 | 106 | uint16_t g_awVoltages[8]={ 107 | 0,0,0,0,0,0,0,0}; 108 | uint16_t g_wVoltageSum = 0; 109 | byte g_iVoltages = 0; 110 | 111 | uint16_t ServoDriver::GetBatteryVoltage(void) { 112 | g_iVoltages = (++g_iVoltages)&0x7; // setup index to our array... 113 | g_wVoltageSum -= g_awVoltages[g_iVoltages]; 114 | g_awVoltages[g_iVoltages] = analogRead(cVoltagePin); 115 | g_wVoltageSum += g_awVoltages[g_iVoltages]; 116 | 117 | #ifdef CVREF 118 | return ((long)((long)g_wVoltageSum*CVREF*(CVADR1+CVADR2))/(long)(8192*(long)CVADR2)); 119 | #else 120 | return ((long)((long)g_wVoltageSum*125*(CVADR1+CVADR2))/(long)(2048*(long)CVADR2)); 121 | #endif 122 | 123 | } 124 | #endif 125 | 126 | 127 | //============================================================================== 128 | // Quick and dirty helper function to read so many bytes in from the SSC with a timeout and an end of character marker... 129 | //============================================================================== 130 | int SSCRead (byte* pb, int cb, word wTimeout, word wEOL) 131 | { 132 | int ich; 133 | byte* pbIn = pb; 134 | unsigned long ulTimeLastChar = micros(); 135 | while (cb) { 136 | while (!SSCSerial.available()) { 137 | // check for timeout 138 | if ((word)(micros()-ulTimeLastChar) > wTimeout) { 139 | return (int)(pb-pbIn); 140 | } 141 | } 142 | ich = SSCSerial.read(); 143 | *pb++ = (byte)ich; 144 | cb--; 145 | 146 | if ((word)ich == wEOL) 147 | break; // we matched so get out of here. 148 | ulTimeLastChar = micros(); // update to say we received something 149 | } 150 | 151 | return (int)(pb-pbIn); 152 | } 153 | 154 | 155 | 156 | //-------------------------------------------------------------------- 157 | //[GP PLAYER] 158 | //-------------------------------------------------------------------- 159 | #ifdef OPT_GPPLAYER 160 | uint8_t g_bGPCntSteps; 161 | uint8_t g_bGPCurStep; 162 | boolean g_fGPSMChanged; 163 | //-------------------------------------------------------------------- 164 | //[FIsGPSeqDefined] 165 | //-------------------------------------------------------------------- 166 | boolean ServoDriver::FIsGPSeqDefined(uint8_t iSeq) 167 | { 168 | word wGPSeqPtr; 169 | 170 | // See if we can see if this sequence is defined 171 | SSCSerial.print(F("EER -")); 172 | SSCSerial.print(iSeq*2, DEC); 173 | SSCSerial.println(F(";2")); 174 | if ((SSCRead((byte*)&wGPSeqPtr, sizeof(wGPSeqPtr), 1000, 0xffff) == sizeof(wGPSeqPtr)) && (wGPSeqPtr != 0) && (wGPSeqPtr != 0xffff)) { 175 | return true; 176 | } 177 | return false; // nope return error 178 | } 179 | 180 | 181 | //-------------------------------------------------------------------- 182 | // Setup to start sequence number... 183 | //-------------------------------------------------------------------- 184 | void ServoDriver::GPStartSeq(uint8_t iSeq) 185 | { 186 | if (!_fGPActive && (iSeq != 0xff)) { 187 | _fGPActive = true; 188 | _iSeq = iSeq; 189 | g_bGPCntSteps = 0xff; 190 | g_fGPSMChanged = false; 191 | } 192 | else { 193 | _iSeq = iSeq; // Signal for abort 194 | } 195 | 196 | } 197 | 198 | //-------------------------------------------------------------------- 199 | //[GP PLAYER] 200 | //-------------------------------------------------------------------- 201 | void ServoDriver::GPPlayer(void) 202 | { 203 | byte abStat[4]; 204 | byte cbRead; 205 | 206 | if (_fGPActive) { 207 | if (g_bGPCntSteps == 0xff) { 208 | // We have not init yet... 209 | g_bGPCntSteps = GPNumSteps(); // so get the number of steps. 210 | if (g_bGPCntSteps == 0xff) { 211 | _fGPActive = false; // error so bail out of here... 212 | } 213 | else { 214 | g_InputController.AllowControllerInterrupts(false); 215 | 216 | // Since we are monitoring GP Sequence we no longer tell it to only run once... 217 | SSCSerial.print(F("PL0SQ")); 218 | SSCSerial.println(_iSeq, DEC); 219 | delay(20); 220 | while (SSCSerial.read() != -1) // remove anything that was queued up. 221 | ; 222 | g_InputController.AllowControllerInterrupts(true); 223 | } 224 | } 225 | else { 226 | // Player was started up, so lets see what the state is... 227 | g_InputController.AllowControllerInterrupts(false); 228 | if (_iSeq == 0xff) { // User told us to abort 229 | SSCSerial.println(F("PL0")); 230 | _fGPActive=false; 231 | } 232 | else { 233 | SSCSerial.print(F("QPL0\r")); 234 | cbRead = SSCRead((byte*)abStat, sizeof(abStat), 10000, (word)-1); // [GPStatSeq, GPStatFromStep, GPStatToStep, GPStatTime] 235 | 236 | g_bGPCurStep = abStat[1]; 237 | if ((g_bGPCurStep == (g_bGPCntSteps-1)) && (abStat[3] == 0)) { 238 | // We are done 239 | SSCSerial.println(F("PL0")); 240 | _fGPActive=false; 241 | } 242 | else if (g_fGPSMChanged) { 243 | g_fGPSMChanged = false; 244 | SSCSerial.print(F("PL0SM")); 245 | SSCSerial.println(_sGPSM, DEC); 246 | } 247 | } 248 | g_InputController.AllowControllerInterrupts(true); // Ok to process hserial again... 249 | } 250 | } 251 | } 252 | 253 | //------------------------------------------------------------------------------------------ 254 | //------------------------------------------------------------------------------------------ 255 | uint8_t ServoDriver::GPNumSteps(void) // How many steps does the current sequence have 256 | { 257 | word wSeqStart; 258 | uint8_t bGPCntSteps = 0xff; // assume an error 259 | byte cbRead; 260 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 261 | 262 | // Output command to ssc 263 | SSCSerial.print(F("EER -")); 264 | SSCSerial.print(_iSeq, DEC); 265 | SSCSerial.println(F(";2")); 266 | cbRead = SSCRead((byte*)&wSeqStart, sizeof(wSeqStart), 10000, (word)-1); // Try to get the pointer to sequence 267 | 268 | if ((cbRead == sizeof(wSeqStart)) && (wSeqStart != 0) && (wSeqStart != 0xffff)) { 269 | // Now try to read in the count of steps from the start of the sequence. 270 | // Output command to ssc 271 | SSCSerial.print(F("EER -")); 272 | SSCSerial.print(wSeqStart, DEC); 273 | SSCSerial.println(F(";1")); 274 | cbRead = SSCRead((byte*)bGPCntSteps, sizeof(bGPCntSteps), 10000, (word)-1); // Try to get the pointer to sequence 275 | } 276 | return bGPCntSteps; 277 | } 278 | 279 | //------------------------------------------------------------------------------------------ 280 | //------------------------------------------------------------------------------------------ 281 | uint8_t ServoDriver::GPCurStep(void) // Return which step currently on... 282 | { 283 | return _fGPActive ? g_bGPCurStep : 0xff; 284 | } 285 | 286 | //------------------------------------------------------------------------------------------ 287 | //------------------------------------------------------------------------------------------ 288 | void ServoDriver::GPSetSpeedMultiplyer(short sm) // Set the Speed multiplier (100 is default) 289 | { 290 | if (_sGPSM != sm) { 291 | _sGPSM = sm; 292 | g_fGPSMChanged = true; 293 | } 294 | } 295 | 296 | 297 | #endif // OPT_GPPLAYER 298 | 299 | //------------------------------------------------------------------------------------------ 300 | //[BeginServoUpdate] Does whatever preperation that is needed to starrt a move of our servos 301 | //------------------------------------------------------------------------------------------ 302 | void ServoDriver::BeginServoUpdate(void) // Start the update 303 | { 304 | } 305 | 306 | //------------------------------------------------------------------------------------------ 307 | //[OutputServoInfoForLeg] Do the output to the SSC-32 for the servos associated with 308 | // the Leg number passed in. 309 | //------------------------------------------------------------------------------------------ 310 | #define cPwmDiv 991 //old 1059; 311 | #define cPFConst 592 //old 650 ; 900*(1000/cPwmDiv)+cPFConst must always be 1500 312 | // A PWM/deg factor of 10,09 give cPwmDiv = 991 and cPFConst = 592 313 | // For a modified 5645 (to 180 deg travel): cPwmDiv = 1500 and cPFConst = 900. 314 | #ifdef c4DOF 315 | void ServoDriver::OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1, short sTarsAngle1) 316 | #else 317 | void ServoDriver::OutputServoInfoForLeg(byte LegIndex, short sCoxaAngle1, short sFemurAngle1, short sTibiaAngle1) 318 | #endif 319 | { 320 | word wCoxaSSCV; // Coxa value in SSC units 321 | word wFemurSSCV; // 322 | word wTibiaSSCV; // 323 | #ifdef c4DOF 324 | word wTarsSSCV; // 325 | #endif 326 | 327 | // The Main code now takes care of the inversion before calling. 328 | wCoxaSSCV = ((long)(sCoxaAngle1 +900))*1000/cPwmDiv+cPFConst; 329 | wFemurSSCV = ((long)((long)(sFemurAngle1+900))*1000/cPwmDiv+cPFConst); 330 | wTibiaSSCV = ((long)(sTibiaAngle1+900))*1000/cPwmDiv+cPFConst; 331 | #ifdef c4DOF 332 | wTarsSSCV = ((long)(sTarsAngle1+900))*1000/cPwmDiv+cPFConst; 333 | #endif 334 | 335 | #ifdef cSSC_BINARYMODE 336 | SSCSerial.write(pgm_read_byte(&cCoxaPin[LegIndex]) + 0x80); 337 | SSCSerial.write(wCoxaSSCV >> 8); 338 | SSCSerial.write(wCoxaSSCV & 0xff); 339 | SSCSerial.write(pgm_read_byte(&cFemurPin[LegIndex]) + 0x80); 340 | SSCSerial.write(wFemurSSCV >> 8); 341 | SSCSerial.write(wFemurSSCV & 0xff); 342 | SSCSerial.write(pgm_read_byte(&cTibiaPin[LegIndex]) + 0x80); 343 | SSCSerial.write(wTibiaSSCV >> 8); 344 | SSCSerial.write(wTibiaSSCV & 0xff); 345 | #ifdef c4DOF 346 | if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) { // We allow mix of 3 and 4 DOF legs... 347 | SSCSerial.write(pgm_read_byte(&cTarsPin[LegIndex]) + 0x80); 348 | SSCSerial.write(wTarsSSCV >> 8); 349 | SSCSerial.write(wTarsSSCV & 0xff); 350 | } 351 | #endif 352 | #else 353 | SSCSerial.print("#"); 354 | SSCSerial.print(pgm_read_byte(&cCoxaPin[LegIndex]), DEC); 355 | SSCSerial.print("P"); 356 | SSCSerial.print(wCoxaSSCV, DEC); 357 | SSCSerial.print("#"); 358 | SSCSerial.print(pgm_read_byte(&cFemurPin[LegIndex]), DEC); 359 | SSCSerial.print("P"); 360 | SSCSerial.print(wFemurSSCV, DEC); 361 | SSCSerial.print("#"); 362 | SSCSerial.print(pgm_read_byte(&cTibiaPin[LegIndex]), DEC); 363 | SSCSerial.print("P"); 364 | SSCSerial.print(wTibiaSSCV, DEC); 365 | #ifdef c4DOF 366 | if ((byte)pgm_read_byte(&cTarsLength[LegIndex])) { 367 | SSCSerial.print("#"); 368 | SSCSerial.print(pgm_read_byte(&cTarsPin[LegIndex]), DEC); 369 | SSCSerial.print("P"); 370 | SSCSerial.print(wTarsSSCV, DEC); 371 | } 372 | #endif 373 | #endif 374 | g_InputController.AllowControllerInterrupts(true); // Ok for hserial again... 375 | } 376 | 377 | 378 | //-------------------------------------------------------------------- 379 | //[CommitServoDriver Updates the positions of the servos - This outputs 380 | // as much of the command as we can without committing it. This 381 | // allows us to once the previous update was completed to quickly 382 | // get the next command to start 383 | //-------------------------------------------------------------------- 384 | void ServoDriver::CommitServoDriver(word wMoveTime) 385 | { 386 | #ifdef cSSC_BINARYMODE 387 | byte abOut[3]; 388 | #endif 389 | 390 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 391 | 392 | #ifdef cSSC_BINARYMODE 393 | abOut[0] = 0xA1; 394 | abOut[1] = wMoveTime >> 8; 395 | abOut[2] = wMoveTime & 0xff; 396 | SSCSerial.write(abOut, 3); 397 | #else 398 | //Send 399 | SSCSerial.print("T"); 400 | SSCSerial.println(wMoveTime, DEC); 401 | #endif 402 | 403 | g_InputController.AllowControllerInterrupts(true); 404 | 405 | } 406 | 407 | //-------------------------------------------------------------------- 408 | //[FREE SERVOS] Frees all the servos 409 | //-------------------------------------------------------------------- 410 | void ServoDriver::FreeServos(void) 411 | { 412 | g_InputController.AllowControllerInterrupts(false); // If on xbee on hserial tell hserial to not processess... 413 | for (byte LegIndex = 0; LegIndex < 32; LegIndex++) { 414 | SSCSerial.print("#"); 415 | SSCSerial.print(LegIndex, DEC); 416 | SSCSerial.print("P0"); 417 | } 418 | SSCSerial.print("T200\r"); 419 | g_InputController.AllowControllerInterrupts(true); 420 | } 421 | 422 | //-------------------------------------------------------------------- 423 | //Function that gets called from the main loop if the robot is not logically 424 | // on. Gives us a chance to play some... 425 | //-------------------------------------------------------------------- 426 | void ServoDriver::IdleTime(void) 427 | { 428 | } 429 | 430 | #ifdef OPT_TERMINAL_MONITOR 431 | extern void FindServoOffsets(void); 432 | extern void SSCForwarder(void); 433 | 434 | //============================================================================== 435 | // ShowTerminalCommandList: Allow the Terminal monitor to call the servo driver 436 | // to allow it to display any additional commands it may have. 437 | //============================================================================== 438 | void ServoDriver::ShowTerminalCommandList(void) 439 | { 440 | #ifdef OPT_FIND_SERVO_OFFSETS 441 | DBGSerial.println(F("O - Adjust servo offsets (servo offset mode)")); 442 | #endif 443 | #ifdef OPT_SSC_FORWARDER 444 | DBGSerial.println(F("S - SSC Forwarder")); 445 | #endif 446 | } 447 | 448 | //============================================================================== 449 | // ProcessTerminalCommand: The terminal monitor will call this to see if the 450 | // command the user entered was one added by the servo driver. 451 | //============================================================================== 452 | boolean ServoDriver::ProcessTerminalCommand(byte *psz, byte bLen) 453 | { 454 | #ifdef OPT_FIND_SERVO_OFFSETS 455 | if ((bLen == 1) && ((*psz == 'o') || (*psz == 'O'))) { 456 | FindServoOffsets(); 457 | } 458 | #endif 459 | #ifdef OPT_SSC_FORWARDER 460 | if ((bLen == 1) && ((*psz == 's') || (*psz == 'S'))) { 461 | SSCForwarder(); 462 | } 463 | #endif 464 | return true; // Currently not using the return value 465 | 466 | } 467 | 468 | //============================================================================== 469 | // SSC Forwarder - used to allow things like Lynxterm to talk to the SSC-32 470 | // through the Arduino... Will see if it is fast enough... 471 | //============================================================================== 472 | #ifdef OPT_SSC_FORWARDER 473 | void SSCForwarder(void) 474 | { 475 | int sChar; 476 | int sPrevChar; 477 | DBGSerial.println("SSC Forwarder mode - Enter $ to exit"); 478 | 479 | for (;;) { 480 | if ((sChar = DBGSerial.read()) != -1) { 481 | SSCSerial.write(sChar & 0xff); 482 | if (((sChar == '\n') || (sChar == '\r')) && (sPrevChar == '$')) 483 | break; // exit out of the loop 484 | sPrevChar = sChar; 485 | } 486 | 487 | 488 | if ((sChar = SSCSerial.read()) != -1) { 489 | DBGSerial.write(sChar & 0xff); 490 | } 491 | } 492 | DBGSerial.println("Exited SSC Forwarder mode"); 493 | } 494 | #endif // OPT_SSC_FORWARDER 495 | //============================================================================== 496 | // FindServoOffsets - Find the zero points for each of our servos... 497 | // Will use the new servo function to set the actual pwm rate and see 498 | // how well that works... 499 | //============================================================================== 500 | #ifdef OPT_FIND_SERVO_OFFSETS 501 | #ifndef NUMSERVOSPERLEG 502 | #define NUMSERVOSPERLEG 3 503 | #endif 504 | 505 | void FindServoOffsets() 506 | { 507 | byte abSSCServoNum[NUMSERVOSPERLEG*CNT_LEGS]; // array of servos... 508 | signed short asOffsets[NUMSERVOSPERLEG*CNT_LEGS]; // we have 18 servos to find/set offsets for... 509 | signed char asOffsetsRead[NUMSERVOSPERLEG*CNT_LEGS]; // array for our read in servos... 510 | 511 | static char *apszLegs[] = { 512 | #ifdef QUADMODE 513 | "RR","RF", "LR", "LF" }; // Leg Order 514 | #else 515 | "RR","RM","RF", "LR", "LM", "LF" }; // Leg Order 516 | #endif 517 | static char *apszLJoints[] = { 518 | " Coxa", " Femur", " Tibia", " tArs" }; // which joint on the leg... 519 | 520 | byte szTemp[5]; 521 | byte cbRead; 522 | 523 | int data; 524 | short sSN ; // which servo number 525 | boolean fNew = true; // is this a new servo to work with? 526 | boolean fExit = false; // when to exit 527 | 528 | if (CheckVoltage()) { 529 | // Voltage is low... 530 | Serial.println("Low Voltage: fix or hit $ to abort"); 531 | while (CheckVoltage()) { 532 | if (Serial.read() == '$') return; 533 | } 534 | } 535 | 536 | // Fill in array of SSC-32 servo numbers 537 | for (sSN=0; sSN < CNT_LEGS; sSN++) { // Make sure all of our servos initialize to 0 offset from saved. 538 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 0] = pgm_read_byte(&cCoxaPin[sSN]); 539 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 1] = pgm_read_byte(&cFemurPin[sSN]); 540 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 2] = pgm_read_byte(&cTibiaPin[sSN]); 541 | #ifdef c4DOF 542 | abSSCServoNum[sSN*NUMSERVOSPERLEG + 3] = pgm_read_byte(&cTarsPin[sSN]); 543 | #endif 544 | } 545 | // now lets loop through and get information and set servos to 1500 546 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++ ) { 547 | asOffsets[sSN] = 0; 548 | asOffsetsRead[sSN] = 0; 549 | 550 | SSCSerial.print("R"); 551 | SSCSerial.println(32+abSSCServoNum[sSN], DEC); 552 | // now read in the current value... Maybe should use atoi... 553 | cbRead = SSCRead((byte*)szTemp, sizeof(szTemp), 10000, 13); 554 | if (cbRead > 0) 555 | asOffsetsRead[sSN] = atoi((const char *)szTemp); 556 | 557 | SSCSerial.print("#"); 558 | SSCSerial.print(abSSCServoNum[sSN], DEC); 559 | SSCSerial.println("P1500"); 560 | } 561 | 562 | // OK lets move all of the servos to their zero point. 563 | Serial.println("Adjust the zero position of each servo."); 564 | Serial.println("First choose a leg: 0-5 chooses a leg; C-Coxa, F-Femur, T-Tibia chooses a servo"); 565 | Serial.println("Leg order:\n 0: RR \n 1: RM \n 2: RF \n 3: LR \n 4: LM \n 5: LF"); 566 | Serial.println("Commands: $ exit; +, - changes zero position; * change servo"); 567 | 568 | sSN = true; 569 | while(!fExit) { 570 | if (fNew) { 571 | Serial.print("Servo: "); 572 | Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]); 573 | Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]); 574 | Serial.print("("); 575 | Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 576 | Serial.println(")"); 577 | 578 | // Now lets wiggle the servo 579 | SSCSerial.print("#"); 580 | SSCSerial.print(abSSCServoNum[sSN], DEC); 581 | SSCSerial.print("P"); 582 | SSCSerial.print(1500+asOffsets[sSN]+250, DEC); 583 | SSCSerial.println("T250"); 584 | delay(250); 585 | 586 | SSCSerial.print("#"); 587 | SSCSerial.print(abSSCServoNum[sSN], DEC); 588 | SSCSerial.print("P"); 589 | SSCSerial.print(1500+asOffsets[sSN]-250, DEC); 590 | SSCSerial.println("T500"); 591 | delay(500); 592 | 593 | SSCSerial.print("#"); 594 | SSCSerial.print(abSSCServoNum[sSN], DEC); 595 | SSCSerial.print("P"); 596 | SSCSerial.print(1500+asOffsets[sSN], DEC); 597 | SSCSerial.println("T250"); 598 | delay(250); 599 | 600 | fNew = false; 601 | } 602 | 603 | //get user entered data 604 | data = Serial.read(); 605 | //if data received 606 | if (data !=-1) { 607 | if (data == '$') 608 | fExit = true; // not sure how the keypad will map so give NL, CR, LF... all implies exit 609 | 610 | else if ((data == '+') || (data == '-')) { 611 | if (data == '+') 612 | asOffsets[sSN] += 5; // increment by 5us 613 | else 614 | asOffsets[sSN] -= 5; // increment by 5us 615 | 616 | Serial.print(" "); 617 | Serial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 618 | 619 | SSCSerial.print("#"); 620 | SSCSerial.print(abSSCServoNum[sSN], DEC); 621 | SSCSerial.print("P"); 622 | SSCSerial.print(1500+asOffsets[sSN], DEC); 623 | SSCSerial.println("T100"); 624 | } 625 | else if ((data >= '0') && (data <= '5')) { 626 | // direct enter of which servo to change 627 | fNew = true; 628 | sSN = (sSN % NUMSERVOSPERLEG) + (data - '0')*NUMSERVOSPERLEG; 629 | } 630 | else if ((data == 'c') || (data == 'C')) { 631 | fNew = true; 632 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 0; 633 | } 634 | else if ((data == 'f') || (data == 'F')) { 635 | fNew = true; 636 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 1; 637 | } 638 | else if ((data == 't') || (data == 'T')) { 639 | // direct enter of which servo to change 640 | fNew = true; 641 | sSN = (sSN / NUMSERVOSPERLEG) * NUMSERVOSPERLEG + 2; 642 | } 643 | else if (data == '*') { 644 | // direct enter of which servo to change 645 | fNew = true; 646 | sSN++; 647 | if (sSN == CNT_LEGS*NUMSERVOSPERLEG) 648 | sSN = 0; 649 | } 650 | } 651 | } 652 | Serial.print("Find Servo exit "); 653 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++){ 654 | Serial.print("Servo: "); 655 | Serial.print(apszLegs[sSN/NUMSERVOSPERLEG]); 656 | Serial.print(apszLJoints[sSN%NUMSERVOSPERLEG]); 657 | Serial.print("("); 658 | Serial.print(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 659 | Serial.println(")"); 660 | } 661 | 662 | Serial.print("\nSave Changes? Y/N: "); 663 | 664 | //get user entered data 665 | while (((data = Serial.read()) == -1) || ((data >= 10) && (data <= 15))) 666 | ; 667 | 668 | if ((data == 'Y') || (data == 'y')) { 669 | // Ok they asked for the data to be saved. We will store the data with a 670 | // number of servos (byte)at the start, followed by a byte for a checksum...followed by our offsets array... 671 | // Currently we store these values starting at EEPROM address 0. May later change... 672 | // 673 | 674 | for (sSN=0; sSN < CNT_LEGS*NUMSERVOSPERLEG; sSN++ ) { 675 | SSCSerial.print("R"); 676 | SSCSerial.print(32+abSSCServoNum[sSN], DEC); 677 | SSCSerial.print("="); 678 | SSCSerial.println(asOffsetsRead[sSN]+asOffsets[sSN], DEC); 679 | delay(10); 680 | } 681 | 682 | // Then I need to have the SSC-32 reboot in order to use the new values. 683 | delay(10); // give it some time to write stuff out. 684 | SSCSerial.println("GOBOOT"); 685 | delay(5); // Give it a little time 686 | SSCSerial.println("g0000"); // tell it that we are done in the boot section so go run the normall SSC stuff... 687 | delay(500); // Give it some time to boot up... 688 | 689 | } 690 | else { 691 | void LoadServosConfig(); 692 | } 693 | 694 | g_ServoDriver.FreeServos(); 695 | 696 | } 697 | #endif // OPT_FIND_SERVO_OFFSETS 698 | 699 | #endif // 700 | -------------------------------------------------------------------------------- /Capers_II_PS2_SSC32/Capers_II_Input_PS2.h: -------------------------------------------------------------------------------- 1 | // Project: Capers II Hexapod 2 | // Author: Toglefritz 3 | 4 | // ////////////////////// 5 | // /// PS2 CONTROLS /// 6 | // ////////////////////// 7 | 8 | // [Common Controls] 9 | // - START = Turn on/off the bot 10 | // - L1Toggle = Shift mode 11 | // - L2Toggle = Rotate mode 12 | // - Circle = Toggle, single leg mode 13 | // - Square = Toggle, balance mode 14 | // - Triangle = Move body to 35mm from the ground (walk pos) 15 | // and back to the ground 16 | // - D-Pad up = Body up 10mm 17 | // - D-Pad down = Body down 10mm 18 | // - D-Pad left = decrease speed with 50mS 19 | // - D-Pad right = increase speed with 50mS 20 | // 21 | // Optional: L3 button down, Left stick can adjust leg positions... 22 | // or if OPT_SINGLELEG is not defined may try using Circle 23 | 24 | // [Walk Controls] 25 | // - SELECT = Switch gaits 26 | // - Left Stick = (Walk mode 1) Walk/Strafe 27 | // (Walk mode 2) Disable 28 | // - Right Stick = (Walk mode 1) Rotate, 29 | // (Walk mode 2) Walk/Rotate 30 | // - R1 = Toggle, double gait travel speed 31 | // - R2 = Toggle, double gait travel length 32 | // 33 | // [Shift Controls] 34 | // - Left Stick = Shift body X/Z 35 | // - Right Stick = Shift body Y and rotate body Y 36 | // 37 | // [Rotate Controls] 38 | // - Left Stick = Rotate body X/Z 39 | // - Right Stick = Rotate body Y 40 | // 41 | // [Single leg Controls] 42 | // - SELECT = Switch legs 43 | // - Left Stick = Move Leg X/Z (relative) 44 | // - Right Stick = Move Leg Y (absolute) 45 | // - R2 = Hold/release leg position 46 | // 47 | // [GP Player Controls] 48 | // - SELECT = Switch Sequences 49 | // - R2 = Start Sequence 50 | // 51 | //==================================================================== 52 | // [Include files] 53 | #if ARDUINO>99 54 | #include // Arduino 1.0 55 | #else 56 | #include // Arduino 0022 57 | #endif 58 | #include 59 | 60 | //[CONSTANTS] 61 | #define WALKMODE 0 62 | #define TRANSLATEMODE 1 63 | #define ROTATEMODE 2 64 | #define SINGLELEGMODE 3 65 | #define GPPLAYERMODE 4 66 | 67 | 68 | #define cTravelDeadZone 4 //The deadzone for the analog input from the remote 69 | #define MAXPS2ERRORCNT 5 // How many times through the loop will we go before shutting off robot? 70 | 71 | #ifndef MAX_BODY_Y 72 | #define MAX_BODY_Y 100 73 | #endif 74 | 75 | //============================================================================= 76 | // Global - Local to this file only... 77 | //============================================================================= 78 | PS2X ps2x; // create PS2 Controller Class 79 | 80 | 81 | // Define an instance of the Input Controller... 82 | InputController g_InputController; // Our Input controller 83 | 84 | 85 | static short g_BodyYOffset; 86 | static short g_sPS2ErrorCnt; 87 | static short g_BodyYShift; 88 | static byte ControlMode; 89 | static bool DoubleHeightOn; 90 | static bool DoubleTravelOn; 91 | static bool WalkMethod; 92 | byte GPSeq; //Number of the sequence 93 | short g_sGPSMController; // What GPSM value have we calculated. 0xff - Not used yet 94 | 95 | // some external or forward function references. 96 | extern void PS2TurnRobotOff(void); 97 | 98 | //============================================================================== 99 | // This is The function that is called by the Main program to initialize 100 | //the input controller, which in this case is the PS2 controller 101 | //process any commands. 102 | //============================================================================== 103 | 104 | // If both PS2 and XBee are defined then we will become secondary to the xbee 105 | void InputController::Init(void) 106 | { 107 | int error; 108 | 109 | //error = ps2x.config_gamepad(57, 55, 56, 54); // Setup gamepad (clock, command, attention, data) pins 110 | error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT); // Setup gamepad (clock, command, attention, data) pins 111 | 112 | #ifdef DBGSerial 113 | DBGSerial.print("PS2 Init: "); 114 | DBGSerial.println(error, DEC); 115 | #endif 116 | g_BodyYOffset = 0; 117 | g_BodyYShift = 0; 118 | g_sPS2ErrorCnt = 0; // error count 119 | 120 | ControlMode = WALKMODE; 121 | DoubleHeightOn = false; 122 | DoubleTravelOn = false; 123 | WalkMethod = false; 124 | 125 | g_InControlState.SpeedControl = 100; // Sort of migrate stuff in from Devon. 126 | } 127 | 128 | //============================================================================== 129 | // This function is called by the main code to tell us when it is about to 130 | // do a lot of bit-bang outputs and it would like us to minimize any interrupts 131 | // that we do while it is active... 132 | //============================================================================== 133 | void InputController::AllowControllerInterrupts(boolean fAllow) 134 | { 135 | // We don't need to do anything... 136 | } 137 | 138 | //============================================================================== 139 | // This is The main code to input function to read inputs from the PS2 and then 140 | //process any commands. 141 | //============================================================================== 142 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 143 | boolean g_fDynamicLegXZLength = false; // Has the user dynamically adjusted the Leg XZ init pos (width) 144 | #endif 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 | 172 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 173 | boolean fAdjustLegPositions = false; 174 | short sLegInitXZAdjust = 0; 175 | short sLegInitAngleAdjust = 0; 176 | #endif 177 | // In an analog mode so should be OK... 178 | g_sPS2ErrorCnt = 0; // clear out error count... 179 | 180 | if (ps2x.ButtonPressed(PSB_START)) {// OK lets press start button 181 | if (g_InControlState.fRobotOn) { 182 | PS2TurnRobotOff(); 183 | } 184 | else { 185 | //Turn on 186 | g_InControlState.fRobotOn = 1; 187 | fAdjustLegPositions = true; 188 | } 189 | } 190 | 191 | if (g_InControlState.fRobotOn) { 192 | // [SWITCH MODES] 193 | 194 | //Translate mode 195 | if (ps2x.ButtonPressed(PSB_L1)) {// L1 Button Test 196 | MSound( 1, 50, 2000); 197 | if (ControlMode != TRANSLATEMODE ) 198 | ControlMode = TRANSLATEMODE; 199 | else { 200 | #ifdef OPT_SINGLELEG 201 | if (g_InControlState.SelectedLeg==255) 202 | ControlMode = WALKMODE; 203 | else 204 | #endif 205 | ControlMode = SINGLELEGMODE; 206 | } 207 | } 208 | 209 | //Rotate mode 210 | if (ps2x.ButtonPressed(PSB_L2)) { // L2 Button Test 211 | MSound( 1, 50, 2000); 212 | if (ControlMode != ROTATEMODE) 213 | ControlMode = ROTATEMODE; 214 | else { 215 | #ifdef OPT_SINGLELEG 216 | if (g_InControlState.SelectedLeg == 255) 217 | ControlMode = WALKMODE; 218 | else 219 | #endif 220 | ControlMode = SINGLELEGMODE; 221 | } 222 | } 223 | 224 | //Single leg mode fNO 225 | #ifdef OPT_SINGLELEG 226 | if (ps2x.ButtonPressed(PSB_CIRCLE)) {// O - Circle Button Test 227 | if (abs(g_InControlState.TravelLength.x)0) 269 | g_BodyYOffset = 0; 270 | else 271 | g_BodyYOffset = 35; 272 | fAdjustLegPositions = true; 273 | } 274 | 275 | if (ps2x.ButtonPressed(PSB_PAD_UP)) {// D-Up - Button Test 276 | g_BodyYOffset += 10; 277 | 278 | // And see if the legs should adjust... 279 | fAdjustLegPositions = true; 280 | if (g_BodyYOffset > MAX_BODY_Y) 281 | g_BodyYOffset = MAX_BODY_Y; 282 | } 283 | 284 | if (ps2x.ButtonPressed(PSB_PAD_DOWN) && g_BodyYOffset) {// D-Down - Button Test 285 | if (g_BodyYOffset > 10) 286 | g_BodyYOffset -= 10; 287 | else 288 | g_BodyYOffset = 0; // constrain don't go less than zero. 289 | 290 | // And see if the legs should adjust... 291 | fAdjustLegPositions = true; 292 | } 293 | 294 | if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) { // D-Right - Button Test 295 | if (g_InControlState.SpeedControl>0) { 296 | g_InControlState.SpeedControl = g_InControlState.SpeedControl - 50; 297 | MSound( 1, 50, 2000); 298 | } 299 | } 300 | 301 | if (ps2x.ButtonPressed(PSB_PAD_LEFT)) { // D-Left - Button Test 302 | if (g_InControlState.SpeedControl<2000 ) { 303 | g_InControlState.SpeedControl = g_InControlState.SpeedControl + 50; 304 | MSound( 1, 50, 2000); 305 | } 306 | } 307 | 308 | // We are optionally going to allow the user to modify the Initial Leg positions, when they 309 | // press the L3 button. 310 | byte lx = ps2x.Analog(PSS_LX); 311 | byte ly = ps2x.Analog(PSS_LY); 312 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 313 | #ifdef OPT_SINGLELEG 314 | if (ps2x.Button(PSB_L3)) { // L3 pressed, use this to modify leg positions. 315 | #else 316 | if (ps2x.Button(PSB_CIRCLE)) {// O - Circle Button Test 317 | #endif 318 | sLegInitXZAdjust = ((int)lx-128)/10; // play with this. 319 | sLegInitAngleAdjust = ((int)ly-128)/8; 320 | lx = 0; 321 | ly = 0; 322 | } 323 | #endif 324 | 325 | //[Walk functions] 326 | if (ControlMode == WALKMODE) { 327 | //Switch gates 328 | if (ps2x.ButtonPressed(PSB_SELECT) // Select Button Test 329 | && abs(g_InControlState.TravelLength.x) (128+16)) || (ps2x.Analog(PSS_RY) < (128-16))) 432 | { 433 | // We are in speed modify mode... 434 | short sNewGPSM = map(ps2x.Analog(PSS_RY), 0, 255, -200, 200); 435 | if (sNewGPSM != g_sGPSMController) { 436 | g_sGPSMController = sNewGPSM; 437 | g_ServoDriver.GPSetSpeedMultiplyer(g_sGPSMController); 438 | } 439 | 440 | } 441 | } 442 | 443 | //Switch between sequences 444 | if (ps2x.ButtonPressed(PSB_SELECT)) { // Select Button Test 445 | if (!g_ServoDriver.FIsGPSeqActive() ) { 446 | if (GPSeq < 5) { //Max sequence 447 | MSound(1, 50, 1500); 448 | GPSeq = GPSeq+1; 449 | } 450 | else { 451 | MSound(2, 50, 2000, 50, 2250); 452 | GPSeq=0; 453 | } 454 | } 455 | } 456 | //Start Sequence 457 | if (ps2x.ButtonPressed(PSB_R2))// R2 Button Test 458 | if (!g_ServoDriver.FIsGPSeqActive() ) { 459 | g_ServoDriver.GPStartSeq(GPSeq); 460 | g_sGPSMController = 32767; // Say that we are not in Speed modify mode yet... valid ranges are 50-200 (both postive and negative... 461 | } 462 | else { 463 | g_ServoDriver.GPStartSeq(0xff); // tell the GP system to abort if possible... 464 | MSound (2, 50, 2000, 50, 2000); 465 | } 466 | 467 | 468 | } 469 | #endif // OPT_GPPLAYER 470 | 471 | //Calculate walking time delay 472 | g_InControlState.InputTimeDelay = 128 - max(max(abs(lx - 128), abs(ly - 128)), abs(ps2x.Analog(PSS_RX) - 128)); 473 | } 474 | 475 | //Calculate g_InControlState.BodyPos.y 476 | g_InControlState.BodyPos.y = min(max(g_BodyYOffset + g_BodyYShift, 0), MAX_BODY_Y); 477 | 478 | #ifdef OPT_DYNAMIC_ADJUST_LEGS 479 | if (sLegInitXZAdjust || sLegInitAngleAdjust) { 480 | // User asked for manual leg adjustment - only do when we have finished any previous adjustment 481 | 482 | if (!g_InControlState.ForceGaitStepCnt) { 483 | if (sLegInitXZAdjust) 484 | g_fDynamicLegXZLength = true; 485 | 486 | sLegInitXZAdjust += GetLegsXZLength(); // Add on current length to our adjustment... 487 | // Handle maybe change angles... 488 | if (sLegInitAngleAdjust) 489 | RotateLegInitAngles(sLegInitAngleAdjust); 490 | // Give system time to process previous calls 491 | AdjustLegPositions(sLegInitXZAdjust); 492 | } 493 | } 494 | #endif 495 | 496 | if (fAdjustLegPositions) 497 | AdjustLegPositionsToBodyHeight(); // Put main workings into main program file 498 | } 499 | else { 500 | // We may have lost the PS2... See what we can do to recover... 501 | if (g_sPS2ErrorCnt < MAXPS2ERRORCNT) 502 | g_sPS2ErrorCnt++; // Increment the error count and if to many errors, turn off the robot. 503 | else if (g_InControlState.fRobotOn) 504 | PS2TurnRobotOff(); 505 | ps2x.reconfig_gamepad(); 506 | } 507 | } 508 | 509 | //============================================================================== 510 | // PS2TurnRobotOff - code used couple of places so save a little room... 511 | //============================================================================== 512 | void PS2TurnRobotOff(void) 513 | { 514 | //Turn off 515 | g_InControlState.BodyPos.x = 0; 516 | g_InControlState.BodyPos.y = 0; 517 | g_InControlState.BodyPos.z = 0; 518 | g_InControlState.BodyRot1.x = 0; 519 | g_InControlState.BodyRot1.y = 0; 520 | g_InControlState.BodyRot1.z = 0; 521 | g_InControlState.TravelLength.x = 0; 522 | g_InControlState.TravelLength.z = 0; 523 | g_InControlState.TravelLength.y = 0; 524 | g_BodyYOffset = 0; 525 | g_BodyYShift = 0; 526 | #ifdef OPT_SINGLELEG 527 | g_InControlState.SelectedLeg = 255; 528 | #endif 529 | g_InControlState.fRobotOn = 0; 530 | AdjustLegPositionsToBodyHeight(); // Put main workings into main program file 531 | } 532 | 533 | 534 | 535 | 536 | -------------------------------------------------------------------------------- /Capers_II_PS2_SSC32/Capers_II_PS2_SSC32.ino: -------------------------------------------------------------------------------- 1 | // Project: Capers II Hexapod 2 | // Author: Toglefritz 3 | 4 | 5 | #define DEFINE_HEX_GLOBALS 6 | 7 | // Include some of the basic libraries 8 | #include 9 | #include 10 | #include // Install the PS2X library Forked here: https://github.com/Toglefritz/Arduino-PS2X 11 | 12 | // ////////////////// COMPILATION OPTIONS ////////////////// 13 | 14 | #define OPT_TERMINAL_MONITOR 15 | 16 | #ifdef OPT_TERMINAL_MONITOR // Turning off terminal monitor will turn these off as well... 17 | //#define OPT_SSC_FORWARDER // Only useful if terminal monitor is enabled 18 | #define OPT_FIND_SERVO_OFFSETS // Only useful if terminal monitor is enabled 19 | #endif 20 | #define OPT_GPPLAYER 21 | 22 | // Which type of control(s) do you want to compile in 23 | #define DBGSerial Serial 24 | //#define DEBUG_IOPINS 25 | 26 | #if defined(UBRR1H) 27 | #define SSCSerial Serial1 28 | #else 29 | #endif 30 | 31 | #define USEPS2 32 | 33 | #define USE_SSC32 34 | 35 | #define cSSC_BAUD 38400 //SSC32 BAUD rate 36 | 37 | // ////////////////////////// CONFIGURATION /////////////////////////// 38 | 39 | // ____________________________(((((((((((((((____________________________ 40 | // ____________________________(((((((((((((((((___________________________ 41 | // ____________________________(((((((((((((((((___________________________ 42 | // ___________________________((((((((((((((((((#__________________________ 43 | // ___________________________(((((((((((((((((((__________________________ 44 | // ________________________&(((((((((((((((((((((((________________________ 45 | // _______&((((((_______#(((((((((((((((((((((((((((((#_______((((((_______ 46 | // ______#((((((((((((((((((((((((/,.......,/((((((((((((((((((((((((#_____ 47 | // _____(((((((((((((((((((((,...................*(((((((((((((((((((((____ 48 | // ____(((((((((((((((((((,.........................*(((((((((((((((((((___ 49 | // ___((((((((((((((((((...............................((((((((((((((((((__ 50 | // _%(((((((((((((((((,.........(#############(........./(((((((((((((((((_ 51 | // __((((((((((((((((........(###################(........#(((((((((((((((_ 52 | // ____(((((((((((((......./########(.....(########,.......##(((((((((((___ 53 | // ______((((((((((.......(######............,######/.......###(((((((_____ 54 | // ________(((((((/....../#####/...............(#####,......#####((#_______ 55 | // _________((((((.......#####(.................######......,######________ 56 | // _________((((((.......#####...................#####.......######________ 57 | // _________((((((.......#####...................#####*******######________ 58 | // _________((((((.......#####/.................(#####******/######________ 59 | // ________(((((((/......##### ............... #####/******########_______ 60 | // ______#(((((((((.......#######.............######(*******#########%_____ 61 | // ____(((((((((((((.......(########*...../########(*******#############___ 62 | // __((((((((((((((((........#####################********################_ 63 | // _&(((((((((((((((((..........###############*********/#################_ 64 | // ___((((((((((((((((((.........**********************##################__ 65 | // ____(((((((((((((((((((......********************/###################___ 66 | // _____((((((((((((((((((((#..******************/#####################____ 67 | // ______(((((((((((((((((((((####/*********(#########################_____ 68 | // _______%((((((_______((((((((#######################______%######_______ 69 | // _________________________((((((#################________________________ 70 | // ___________________________((((((#############__________________________ 71 | // ___________________________((((((((###########__________________________ 72 | // ____________________________((((((((#########___________________________ 73 | // ____________________________(((((((((((######___________________________ 74 | // _____________________________((((((((((((###____________________________ 75 | 76 | // ////////////////////////// CONFIGURATION /////////////////////////// 77 | 78 | // ///////////////////////////////// 79 | // /// Botboarduino Pin Numbers /// 80 | // ///////////////////////////////// 81 | 82 | #define SOUND_PIN 5 // Botboarduino JR pin number 83 | #define PS2_DAT 6 84 | #define PS2_CMD 7 85 | #define PS2_SEL 8 86 | #define PS2_CLK 9 87 | // If we are using a SSC-32 then: 88 | // If were are running on an Arduino Mega we will use one of the hardware serial port, default to Serial1 above. 89 | // If on Non mega, if the IO pins are set to 0, we will overload the hardware Serial port 90 | // Else we will user SoftwareSerial to talk to the SSC-32 91 | #define cSSC_OUT 12 //Output pin for (SSC32 RX) on BotBoard (Yellow) 92 | #define cSSC_IN 13 //Input pin for (SSC32 TX) on BotBoard (Blue) 93 | 94 | // ////////////////////////// 95 | // /// SSC-32 PIN NUMBERS /// 96 | // ////////////////////////// 97 | // This section defines which servos are connected to which pins on the SSC32 98 | #define cRRCoxaPin 0 //Rear Right leg Hip Horizontal 99 | #define cRRFemurPin 1 //Rear Right leg Hip Vertical 100 | #define cRRTibiaPin 2 //Rear Right leg Knee 101 | 102 | #define cRMCoxaPin 4 //Middle Right leg Hip Horizontal 103 | #define cRMFemurPin 5 //Middle Right leg Hip Vertical 104 | #define cRMTibiaPin 6 //Middle Right leg Knee 105 | 106 | #define cRFCoxaPin 8 //Front Right leg Hip Horizontal 107 | #define cRFFemurPin 9 //Front Right leg Hip Vertical 108 | #define cRFTibiaPin 10 //Front Right leg Knee 109 | 110 | #define cLRCoxaPin 16 //Rear Left leg Hip Horizontal 111 | #define cLRFemurPin 17 //Rear Left leg Hip Vertical 112 | #define cLRTibiaPin 18 //Rear Left leg Knee 113 | 114 | #define cLMCoxaPin 20 //Middle Left leg Hip Horizontal 115 | #define cLMFemurPin 21 //Middle Left leg Hip Vertical 116 | #define cLMTibiaPin 22 //Middle Left leg Knee 117 | 118 | #define cLFCoxaPin 24 //Front Left leg Hip Horizontal 119 | #define cLFFemurPin 25 //Front Left leg Hip Vertical 120 | #define cLFTibiaPin 26 //Front Left leg Knee 121 | 122 | // /////////////////////// 123 | // /// MIN/MAX ANGLES /// 124 | // /////////////////////// 125 | // This section defines the mechanical limits of the legs as a safety mechanism that 126 | // prevents the servos from being driven too far and damaging themselves or other 127 | // parts of the hexapod. 128 | 129 | // Mechanical limits of the Right Rear Leg, decimals = 1 130 | #define cRRCoxaMin1 -260 131 | #define cRRCoxaMax1 740 132 | #define cRRFemurMin1 -1010 133 | #define cRRFemurMax1 950 134 | #define cRRTibiaMin1 -1060 135 | #define cRRTibiaMax1 770 136 | 137 | // Mechanical limits of the Right Middle Leg, decimals = 1 138 | #define cRMCoxaMin1 -530 139 | #define cRMCoxaMax1 530 140 | #define cRMFemurMin1 -1010 141 | #define cRMFemurMax1 950 142 | #define cRMTibiaMin1 -1060 143 | #define cRMTibiaMax1 770 144 | 145 | // Mechanical limits of the Right Front Leg, decimals = 1 146 | #define cRFCoxaMin1 -580 147 | #define cRFCoxaMax1 740 148 | #define cRFFemurMin1 -1010 149 | #define cRFFemurMax1 950 150 | #define cRFTibiaMin1 -1060 151 | #define cRFTibiaMax1 770 152 | 153 | // Mechanical limits of the Left Rear Leg, decimals = 1 154 | #define cLRCoxaMin1 -740 155 | #define cLRCoxaMax1 260 156 | #define cLRFemurMin1 -950 157 | #define cLRFemurMax1 1010 158 | #define cLRTibiaMin1 -770 159 | #define cLRTibiaMax1 1060 160 | 161 | // Mechanical limits of the Left Middle Leg, decimals = 1 162 | #define cLMCoxaMin1 -530 163 | #define cLMCoxaMax1 530 164 | #define cLMFemurMin1 -950 165 | #define cLMFemurMax1 1010 166 | #define cLMTibiaMin1 -770 167 | #define cLMTibiaMax1 1060 168 | 169 | // Mechanical limits of the Left Front Leg, decimals = 1 170 | #define cLFCoxaMin1 -740 171 | #define cLFCoxaMax1 580 172 | #define cLFFemurMin1 -950 173 | #define cLFFemurMax1 1010 174 | #define cLFTibiaMin1 -770 175 | #define cLFTibiaMax1 1060 176 | 177 | // /////////////////////// 178 | // /// LEG DIMENSIONS /// 179 | // /////////////////////// 180 | // This section defines the length of each section of each leg. The dimensions are measured from the center 181 | // of one servo horn to the center of the adjacent servo horn. 182 | 183 | // There are two ways this section can be configured. First, if all the sections of all the legs are 184 | // the same length, universal dimensions can be used. These are the dimensions with XX in the beginning 185 | // part. The dimensions are millimeters. 186 | #define cXXCoxaLength 29 187 | #define cXXFemurLength 85 188 | #define cXXTibiaLength 125 189 | 190 | // If the lengths of each leg are not the same, the legs that differ from the universal lengths can be 191 | // set individually in th sections below. 192 | // Right Rear (RR) leg 193 | #define cRRCoxaLength cXXCoxaLength 194 | #define cRRFemurLength cXXFemurLength 195 | #define cRRTibiaLength cXXTibiaLength 196 | 197 | // Right middle (RM) leg 198 | #define cRMCoxaLength cXXCoxaLength 199 | #define cRMFemurLength cXXFemurLength 200 | #define cRMTibiaLength cXXTibiaLength 201 | 202 | // Rigth front (RF) leg 203 | #define cRFCoxaLength cXXCoxaLength 204 | #define cRFFemurLength cXXFemurLength 205 | #define cRFTibiaLength cXXTibiaLength 206 | 207 | // Left Rear (LR) leg 208 | #define cLRCoxaLength cXXCoxaLength 209 | #define cLRFemurLength cXXFemurLength 210 | #define cLRTibiaLength cXXTibiaLength 211 | 212 | // Left middle (LM) leg 213 | #define cLMCoxaLength cXXCoxaLength 214 | #define cLMFemurLength cXXFemurLength 215 | #define cLMTibiaLength cXXTibiaLength 216 | 217 | // Left front (LF) leg 218 | #define cLFCoxaLength cXXCoxaLength 219 | #define cLFFemurLength cXXFemurLength 220 | #define cLFTibiaLength cXXTibiaLength 221 | 222 | 223 | // /////////////////////// 224 | // /// BODY DIMENSIONS /// 225 | // /////////////////////// 226 | // This section defines the size of the robot's body. 227 | 228 | #define cRRCoxaAngle1 -600 // Default Coxa setup angle, decimals = 1 229 | #define cRMCoxaAngle1 0 // Default Coxa setup angle, decimals = 1 230 | #define cRFCoxaAngle1 600 // Default Coxa setup angle, decimals = 1 231 | #define cLRCoxaAngle1 -600 // Default Coxa setup angle, decimals = 1 232 | #define cLMCoxaAngle1 0 // Default Coxa setup angle, decimals = 1 233 | #define cLFCoxaAngle1 600 // Default Coxa setup angle, decimals = 1 234 | 235 | #define cRROffsetX -39 // Distance X from center of the body to the Right Rear coxa 236 | #define cRROffsetZ 75 // Distance Z from center of the body to the Right Rear coxa 237 | #define cRMOffsetX -64 // Distance X from center of the body to the Right Middle coxa 238 | #define cRMOffsetZ 0 // Distance Z from center of the body to the Right Middle coxa 239 | #define cRFOffsetX -39 // Distance X from center of the body to the Right Front coxa 240 | #define cRFOffsetZ -75 // Distance Z from center of the body to the Right Front coxa 241 | 242 | #define cLROffsetX 39 // Distance X from center of the body to the Left Rear coxa 243 | #define cLROffsetZ 75 // Distance Z from center of the body to the Left Rear coxa 244 | #define cLMOffsetX 64 // Distance X from center of the body to the Left Middle coxa 245 | #define cLMOffsetZ 0 // Distance Z from center of the body to the Left Middle coxa 246 | #define cLFOffsetX 39 // Distance X from center of the body to the Left Front coxa 247 | #define cLFOffsetZ -75 // Distance Z from center of the body to the Left Front coxa 248 | 249 | // ///////////////////////////// 250 | // /// START POSITIONS FEET /// 251 | // ///////////////////////////// 252 | // This section specifies the starting positions of the feet. The robot will assume a 253 | // position with the feet at these starting positions on startup. 254 | 255 | #define cHexInitXZ 105 256 | #define CHexInitXZCos60 53 // COS(60) = .5 257 | #define CHexInitXZSin60 91 // sin(60) = .866 258 | #define CHexInitY 25 259 | 260 | #define cRRInitPosX CHexInitXZCos60 // Start positions of the Right Rear leg 261 | #define cRRInitPosY CHexInitY 262 | #define cRRInitPosZ CHexInitXZSin60 263 | 264 | #define cRMInitPosX cHexInitXZ // Start positions of the Right Middle leg 265 | #define cRMInitPosY CHexInitY 266 | #define cRMInitPosZ 0 267 | 268 | #define cRFInitPosX CHexInitXZCos60 // Start positions of the Right Front leg 269 | #define cRFInitPosY CHexInitY 270 | #define cRFInitPosZ -CHexInitXZSin60 271 | 272 | #define cLRInitPosX CHexInitXZCos60 // Start positions of the Left Rear leg 273 | #define cLRInitPosY CHexInitY 274 | #define cLRInitPosZ CHexInitXZSin60 275 | 276 | #define cLMInitPosX cHexInitXZ // Start positions of the Left Middle leg 277 | #define cLMInitPosY CHexInitY 278 | #define cLMInitPosZ 0 279 | 280 | #define cLFInitPosX CHexInitXZCos60 // Start positions of the Left Front leg 281 | #define cLFInitPosY CHexInitY 282 | #define cLFInitPosZ -CHexInitXZSin60 283 | 284 | // Include the other parts of the Capers II code base 285 | 286 | #include "Capers_II.h" 287 | #include "Capers_II_Input_PS2.h" 288 | #include "Capers_II_Driver_SSC32.h" 289 | #include "Capers_II_Code.h" 290 | 291 | // ////////////////////// 292 | // /// PS2 CONTROLS /// 293 | // ////////////////////// 294 | 295 | // [Common Controls] 296 | // - START = Turn on/off the bot 297 | // - L1Toggle = Shift mode 298 | // - L2Toggle = Rotate mode 299 | // - Circle = Toggle, single leg mode 300 | // - Square = Toggle, balance mode 301 | // - Triangle = Move body to 35mm from the ground (walk pos) 302 | // and back to the ground 303 | // - D-Pad up = Body up 10mm 304 | // - D-Pad down = Body down 10mm 305 | // - D-Pad left = decrease speed with 50mS 306 | // - D-Pad right = increase speed with 50mS 307 | // 308 | // Optional: L3 button down, Left stick can adjust leg positions... 309 | // or if OPT_SINGLELEG is not defined may try using Circle 310 | 311 | // [Walk Controls] 312 | // - SELECT = Switch gaits 313 | // - Left Stick = (Walk mode 1) Walk/Strafe 314 | // (Walk mode 2) Disable 315 | // - Right Stick = (Walk mode 1) Rotate, 316 | // (Walk mode 2) Walk/Rotate 317 | // - R1 = Toggle, double gait travel speed 318 | // - R2 = Toggle, double gait travel length 319 | // 320 | // [Shift Controls] 321 | // - Left Stick = Shift body X/Z 322 | // - Right Stick = Shift body Y and rotate body Y 323 | // 324 | // [Rotate Controls] 325 | // - Left Stick = Rotate body X/Z 326 | // - Right Stick = Rotate body Y 327 | // 328 | // [Single leg Controls] 329 | // - SELECT = Switch legs 330 | // - Left Stick = Move Leg X/Z (relative) 331 | // - Right Stick = Move Leg Y (absolute) 332 | // - R2 = Hold/release leg position 333 | // 334 | // [GP Player Controls] 335 | // - SELECT = Switch Sequences 336 | // - R2 = Start Sequence 337 | // 338 | 339 | 340 | 341 | // Attribution: 342 | // Programmer: Jeroen Janssen [aka Xan] 343 | // Kurt Eckhardt(KurtE) converted to C and Arduino 344 | // Kare Halvorsen aka Zenta - Makes everything work correctly! 345 | 346 | -------------------------------------------------------------------------------- /Controls/blank controller.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/blank controller.psd -------------------------------------------------------------------------------- /Controls/common controls.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/common controls.png -------------------------------------------------------------------------------- /Controls/common controls.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/common controls.psd -------------------------------------------------------------------------------- /Controls/single leg controls.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/single leg controls.png -------------------------------------------------------------------------------- /Controls/single leg controls.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/single leg controls.psd -------------------------------------------------------------------------------- /Controls/walk controls.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/walk controls.png -------------------------------------------------------------------------------- /Controls/walk controls.psd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Controls/walk controls.psd -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Capers II, a Hexapod Robot 2 | 3 | ## Project Description 4 | 5 | Capers II is a hexapod robot. The code in this repository is designed to control Capers II. Hexapod robots are robots with six legs. Capers II 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 six legs and three servos per leg, the robot is driven by 18 servos in total. 6 | 7 | The parts necessary to build the robot's frame can be found in the *frame* directory of this repository. 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 hexapod robot's electronics system has three main subsystems: the microcontroller, servo controller, and control input. 17 | 18 | This code assumes that the hexapod 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 | Hexapod robots use a total of 18 servos. Since the robot itself can be quite heavy depending upon the construction, it is important to equip 25 | 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 26 | recommended. 27 | 28 | The robot's frame is constructed from CNC cut aluminum which is strong but also light weight. The design files for the frame can be found in the *frame* directory of this repository. It is possible to find a range of ready-made frames from various online vendors if you do not wish to fabricate your own. 29 | 30 | ## Installation 31 | 32 | To use this code you need to copy each of the folders inside the *libraries* directory into your Arduino Library directory. 33 | 34 | In the Arduino IDE, open **Capers_II_PS2_SSC32.ino**. If the frame on your robot differs from the design in the *frame* directory, the .ino file contains robot configuration options for inputting the geometry of your frame. 35 | 36 | ## Build Instructions 37 | 38 | Instructions for building the robot can be found on Instructables [Instructables](https://www.instructables.com/id/Capers-II-a-Hexapod-Robot). 39 | -------------------------------------------------------------------------------- /Wiring diagrams/Controller connections.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Controller connections.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/5V VS jumpers 2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/5V VS jumpers 2.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/5V VS jumpers.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/5V VS jumpers.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/RT TX power jumper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/RT TX power jumper.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/VS VL jumper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/VS VL jumper.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/button jumpers.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/button jumpers.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/ext usb jumper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/ext usb jumper.jpg -------------------------------------------------------------------------------- /Wiring diagrams/Jumper Diagrams/speaker jumper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/Jumper Diagrams/speaker jumper.jpg -------------------------------------------------------------------------------- /Wiring diagrams/electronics wiring.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Toglefritz/Capers_II/05a4a5b57bf8a2fe120a5faadfab0a312bc9d1bd/Wiring diagrams/electronics wiring.jpg -------------------------------------------------------------------------------- /servo_center/servo_center.ino: -------------------------------------------------------------------------------- 1 | // Author: Toglefritz 2 | // Description: This simple sketch centers a servo connected to pin 2 on the Botboarduino. 3 | // To program the Botboarduino, select "Arduino Duemilanove or Diecimilia" from the 4 | // IDE Tools menu. Center a servo by plugging it into the servo connector on pin 2. 5 | 6 | #include 7 | 8 | Servo servo; // Create servo object to control a servo 9 | 10 | int centerValue = 90; // Almost all standard servos have positions from 0 to 180 degrees, making 11 | // 90 the center point. 12 | 13 | void setup() { 14 | servo.attach(2); // Attaches the servo on pin 2 to the servo object 15 | } 16 | 17 | void loop() { 18 | servo.write(centerValue); // Sets the servo position according to the center value above 19 | delay(100); // Waits for the servo to get there 20 | } 21 | 22 | --------------------------------------------------------------------------------