From b24779d2b0df4932f5b75d7619977325b3583fe7 Mon Sep 17 00:00:00 2001 From: AKautz Date: Thu, 18 Oct 2018 09:17:16 -0400 Subject: [PATCH 1/6] added comment to describe swervesteer --- HexapodCodeV3/l_swerveSteer.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/HexapodCodeV3/l_swerveSteer.ino b/HexapodCodeV3/l_swerveSteer.ino index 97bef2a..9b09897 100644 --- a/HexapodCodeV3/l_swerveSteer.ino +++ b/HexapodCodeV3/l_swerveSteer.ino @@ -40,7 +40,7 @@ void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 300, int LegWheelSpherical2[i][j] = LegWheelSpherical1[i][j]; } } - + //Scale values so largest has a max of 1 if (LegWheelSpherical1[0][2] > 1){ for(int i = 0; i < 4; i++){ LegWheelSpherical2[i][2] /= LegWheelSpherical1[0][2]; From f38a494d6d034101ce0cc6d908f5d328e47d4021 Mon Sep 17 00:00:00 2001 From: AKautz Date: Thu, 18 Oct 2018 15:14:02 -0400 Subject: [PATCH 2/6] Improved Swerve Mode --- HexapodCodeV3/c_loop.ino | 5 +++++ HexapodCodeV3/l_swerveSteer.ino | 16 ++++++++++------ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/HexapodCodeV3/c_loop.ino b/HexapodCodeV3/c_loop.ino index b9e1a62..138dec0 100644 --- a/HexapodCodeV3/c_loop.ino +++ b/HexapodCodeV3/c_loop.ino @@ -17,6 +17,11 @@ void loop() { case MOVE_MODE_CRAWL_PERIODIC: GaitGen2(GaitPeriod,Tim1,GaitMoveX,GaitMoveY,GaitStepHeight,GaitMoveZrot); break; + /* + case MOVE_MODE_SWERVE + SwerveSteer(GaitMoveX, GaitMoveY, GaitMoveZrot, GaitStepHeight, GaitStepHeight) + break; + */ } BodyMod(GaitBodyX,GaitBodyY,GaitBodyZ,GaitBodyPitch,GaitBodyRoll,GaitBodyYaw); LegCoords(); diff --git a/HexapodCodeV3/l_swerveSteer.ino b/HexapodCodeV3/l_swerveSteer.ino index 9b09897..4c21dc5 100644 --- a/HexapodCodeV3/l_swerveSteer.ino +++ b/HexapodCodeV3/l_swerveSteer.ino @@ -1,13 +1,16 @@ +const int SWERVEBASEWIDTH = 300; +const int SWERVEBASELENGTH = 300; - -void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 300, int WheelLength = 300){ +void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 0, int WheelLength = 0){ - Xmove = map(Xmove,-127,127,-100,100)/100; - Ymove = map(Ymove,-127,127,-100,100)/100; - Zrot = map(Zrot,-127,127,314,-314)/100;//zrot is in radians and counterclockwise is positive + Xmove = float(map(Xmove,-127,127,-100,100))/100.0; + Ymove = float(map(Ymove,-127,127,-100,100))/100.0; + Zrot = float(map(Zrot,-127,127,314,-314))/100.0;//zrot is in radians and counterclockwise is positive + WheelWidth = map(WheelWidth,-127,127,-100,100)+SWERVEBASEWIDTH; + WheelLength = map(WheelWidth,-127,127,-100,100)+SWERVEBASELENGTH; - //calcualte the leg positions + //calcualte the leg positions and output them int LegPos[NUM_LEGS][3] = { {-WheelWidth/2,WheelLength/2,Leg0InitZ}, {WheelWidth/2,WheelLength/2,Leg1InitZ}, @@ -23,6 +26,7 @@ void SwerveSteer(float Xmove, float Ymove, float Zrot, int WheelWidth = 300, int //Code that does the swerve steering-------------------------------------------------------- + //idea taken from http://www.simbotics.org/resources/mobility/omnidirectional-drive float ModeA = Xmove - (Zrot*WheelLength/2); float ModeB = Xmove + (Zrot*WheelLength/2); float ModeC = Ymove - (Zrot*WheelWidth/2); From 0ea502b489502ba27b2af0d50ebe07f8232b8705 Mon Sep 17 00:00:00 2001 From: AKautz Date: Tue, 23 Oct 2018 11:26:34 -0400 Subject: [PATCH 3/6] updated doccumentation for leg place mode (not yet implimented) --- HexapodCodeV3/HexapodCodeV3.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/HexapodCodeV3/HexapodCodeV3.ino b/HexapodCodeV3/HexapodCodeV3.ino index 95cc901..99b77fa 100644 --- a/HexapodCodeV3/HexapodCodeV3.ino +++ b/HexapodCodeV3/HexapodCodeV3.ino @@ -43,7 +43,7 @@ 1 -Nothing - NA - no function, does not reset any values 2 -Ext1 Follows - Ext1 3 -Ext2 Follows - Ext2 - 4 -Leg Place mode - Selected leg, numbers are as above - LV->x,LH->y,WV->Z,WH->WheelAzimuth,Buttons->WheelElevation, Ext1 to wheel rotation + 4 -Leg Place mode - Selected leg, numbers are as above - X->x,Y->y,Z->Z,Rz->WheelAzimuth,Rx->WheelElevation, Ry to wheel rotation 5 -GaitMode - Selected Gait - 0:MathGait,1:SwerveSteer,2:RuleGait 6 - 7 - From f8c2960a6013c1561a7dcc02585ddb794c4e2371 Mon Sep 17 00:00:00 2001 From: AKautz Date: Wed, 24 Oct 2018 09:10:42 -0400 Subject: [PATCH 4/6] changes most (all?) calls to trig functions with trigf functions from math.h to use FPU --- HexapodCodeV3/a_presetup.ino | 2 ++ HexapodCodeV3/d0_gait1.ino | 16 ++++++++-------- HexapodCodeV3/d1_gait2.ino | 4 ++-- HexapodCodeV3/e_bodymod.ino | 12 ++++++------ HexapodCodeV3/f_IK.ino | 6 +++--- HexapodCodeV3/k_sphericalcoords.ino | 10 +++++----- 6 files changed, 26 insertions(+), 24 deletions(-) diff --git a/HexapodCodeV3/a_presetup.ino b/HexapodCodeV3/a_presetup.ino index d7be758..bb37a36 100644 --- a/HexapodCodeV3/a_presetup.ino +++ b/HexapodCodeV3/a_presetup.ino @@ -22,6 +22,8 @@ #include //KurtE's bioloid library https://github.com/KurtE/BioloidSerial #include //KurtE's bioloid library https://github.com/KurtE/BioloidSerial +#include //enable use of cosf and sinf to use FPU + #include //Teensy3.x I2C library to use hardware i2c #if defined(GIMBAL_ENABLE)//Gimbal Library diff --git a/HexapodCodeV3/d0_gait1.ino b/HexapodCodeV3/d0_gait1.ino index 809583c..19b8ec9 100644 --- a/HexapodCodeV3/d0_gait1.ino +++ b/HexapodCodeV3/d0_gait1.ino @@ -50,20 +50,20 @@ void GaitGen1(int Period, int Cycle, int Xtrans, int Ytrans, int Ztrans, float Z //Second pass does gait rotate in place int Output2[NUM_LEGS][3]; - Output2[0][0] = (Output1[0][0]*cos(Theta1)-Output1[0][1]*sin(Theta1)); - Output2[0][1] = (Output1[0][0]*sin(Theta1)+Output1[0][1]*cos(Theta1)); + Output2[0][0] = (Output1[0][0]*cosf(Theta1)-Output1[0][1]*sinf(Theta1)); + Output2[0][1] = (Output1[0][0]*sinf(Theta1)+Output1[0][1]*cosf(Theta1)); Output2[0][2] = Output1[0][2]; - Output2[1][0] = (Output1[1][0]*cos(-Theta1)-Output1[1][1]*sin(-Theta1)); - Output2[1][1] = (Output1[1][0]*sin(-Theta1)+Output1[1][1]*cos(-Theta1)); + Output2[1][0] = (Output1[1][0]*cosf(-Theta1)-Output1[1][1]*sinf(-Theta1)); + Output2[1][1] = (Output1[1][0]*sinf(-Theta1)+Output1[1][1]*cosf(-Theta1)); Output2[1][2] = Output1[1][2]; - Output2[2][0] = (Output1[2][0]*cos(Theta1)-Output1[2][1]*sin(Theta1)); - Output2[2][1] = (Output1[2][0]*sin(Theta1)+Output1[2][1]*cos(Theta1)); + Output2[2][0] = (Output1[2][0]*cosf(Theta1)-Output1[2][1]*sinf(Theta1)); + Output2[2][1] = (Output1[2][0]*sinf(Theta1)+Output1[2][1]*cosf(Theta1)); Output2[2][2] = Output1[2][2]; - Output2[3][0] = (Output1[3][0]*cos(-Theta1)-Output1[3][1]*sin(-Theta1)); - Output2[3][1] = (Output1[3][0]*sin(-Theta1)+Output1[3][1]*cos(-Theta1)); + Output2[3][0] = (Output1[3][0]*cosf(-Theta1)-Output1[3][1]*sinf(-Theta1)); + Output2[3][1] = (Output1[3][0]*sinf(-Theta1)+Output1[3][1]*cosf(-Theta1)); Output2[3][2] = Output1[3][2]; //Update GaitGenOut diff --git a/HexapodCodeV3/d1_gait2.ino b/HexapodCodeV3/d1_gait2.ino index a0c4e61..8b19156 100644 --- a/HexapodCodeV3/d1_gait2.ino +++ b/HexapodCodeV3/d1_gait2.ino @@ -43,8 +43,8 @@ int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cyc int Output2[3];//Rotate - Output2[0] = (Output1[0]*cos(Modifiers[3])-Output1[1]*sin(Modifiers[3])); - Output2[1] = (Output1[0]*sin(Modifiers[3])+Output1[1]*cos(Modifiers[3])); + Output2[0] = (Output1[0]*cosf(Modifiers[3])-Output1[1]*sinf(Modifiers[3])); + Output2[1] = (Output1[0]*sinf(Modifiers[3])+Output1[1]*cosf(Modifiers[3])); Output2[2] = Output1[2]; //Update input diff --git a/HexapodCodeV3/e_bodymod.ino b/HexapodCodeV3/e_bodymod.ino index fdef330..c117dde 100644 --- a/HexapodCodeV3/e_bodymod.ino +++ b/HexapodCodeV3/e_bodymod.ino @@ -42,17 +42,17 @@ void BodyMod(int Xlean, int Ylean, float Zheight, float pitch, float roll, float float PitchMatrix[3][3] ={ {1, 0, 0}, - {0,cos(pitch),-sin(pitch)}, - {0,sin(pitch), cos(pitch)} + {0,cosf(pitch),-sinf(pitch)}, + {0,sinf(pitch), cosf(pitch)} }; float RollMatrix[3][3] ={ - { cos(roll),0,sin(roll)}, + { cosf(roll),0,sinf(roll)}, { 0,1, 0}, - {-sin(roll),0,cos(roll)} + {-sinf(roll),0,cosf(roll)} }; float YawMatrix[3][3] ={ - {cos(yaw),-sin(yaw),0}, - {sin(yaw), cos(yaw),0}, + {cosf(yaw),-sinf(yaw),0}, + {sinf(yaw), cosf(yaw),0}, { 0, 0,1} }; diff --git a/HexapodCodeV3/f_IK.ino b/HexapodCodeV3/f_IK.ino index 6624cf3..d3d2a78 100644 --- a/HexapodCodeV3/f_IK.ino +++ b/HexapodCodeV3/f_IK.ino @@ -48,11 +48,11 @@ void LegCalculate(){ //UserSerial.print(((FemurLength*FemurLength)-((LegRadius*LegRadius)+(Zpos*Zpos))-(TibiaLength*TibiaLength))/(-2*sqrt((LegRadius*LegRadius)+(Zpos*Zpos))*TibiaLength)); //UserSerial.println(","); - Output4[i][0] = -(atan(Ypos/Xpos)); + Output4[i][0] = -(atanf(Ypos/Xpos)); //Output4[i][0] = -(atan2(-Ypos,Xpos)); ;//Coxa - Output4[i][1] = (atan2(Zpos,LegRadius) + acos(((FemurLength*FemurLength)-((LegRadius*LegRadius)+(Zpos*Zpos))-(TibiaLength*TibiaLength))/(-2*sqrt((LegRadius*LegRadius)+(Zpos*Zpos))*TibiaLength)));//Femur - Output4[i][2] = -(acos(((TibiaLength*TibiaLength)+(FemurLength*FemurLength)-((LegRadius*LegRadius)+(Zpos*Zpos)))/(-2*FemurLength*TibiaLength)));//Tibia + Output4[i][1] = (atan2f(Zpos,LegRadius) + acos(((FemurLength*FemurLength)-((LegRadius*LegRadius)+(Zpos*Zpos))-(TibiaLength*TibiaLength))/(-2*sqrt((LegRadius*LegRadius)+(Zpos*Zpos))*TibiaLength)));//Femur + Output4[i][2] = -(acosf(((TibiaLength*TibiaLength)+(FemurLength*FemurLength)-((LegRadius*LegRadius)+(Zpos*Zpos)))/(-2*FemurLength*TibiaLength)));//Tibia Output4[i][3] = (0); Output4[i][4] = (1.5707 + Output4[i][2] + Output4[i][1]);//Tharsus } diff --git a/HexapodCodeV3/k_sphericalcoords.ino b/HexapodCodeV3/k_sphericalcoords.ino index f7f73aa..e15b958 100644 --- a/HexapodCodeV3/k_sphericalcoords.ino +++ b/HexapodCodeV3/k_sphericalcoords.ino @@ -36,7 +36,7 @@ void LocalSpherical(float Angles[NUM_LEGS][2]){ void LocalServoGen(int leg,float azimuth, float elevation, float coxa, float femur, float tibia){//Calculates new TibaRot and Tarsus based on local azimuth and elevation //float IniVect[3] = {cos(coxa)*sin(femur+tibia),sin(coxa)*cos(femur+tibia),cos(femur+tibia)};//vector of the leg(x,y,z) azimuth -=coxa;//The first rotation is faster this way, no trig compared to a cartesian rotation - float DesVect1[3] = {10*cos(azimuth)*sin(elevation),10*sin(azimuth)*sin(elevation),10*cos(elevation)};//output vector(x,y,z) + float DesVect1[3] = {10*cosf(azimuth)*sinf(elevation),10*sinf(azimuth)*sinf(elevation),10*cosf(elevation)};//output vector(x,y,z) float DesVect2[3]; //desired vector @@ -46,9 +46,9 @@ void LocalServoGen(int leg,float azimuth, float elevation, float coxa, float fem float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords - { cos(pitch),0,sin(pitch)}, + { cosf(pitch),0,sinf(pitch)}, { 0,1, 0}, - {-sin(pitch),0,cos(pitch)} + {-sinf(pitch),0,cosf(pitch)} }; #if defined(UserSerialTransmit) @@ -66,8 +66,8 @@ void LocalServoGen(int leg,float azimuth, float elevation, float coxa, float fem DesVect2[1] = XZCorr[1][0]*DesVect1[0] + XZCorr[1][1]*DesVect1[1] + XZCorr[1][2]*DesVect1[2]; DesVect2[2] = XZCorr[2][0]*DesVect1[0] + XZCorr[2][1]*DesVect1[1] + XZCorr[2][2]*DesVect1[2]; - LegDynamixels[leg][3] = atan2(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees - LegDynamixels[leg][4] = -(acos(DesVect2[2]/10));//Tarsus + LegDynamixels[leg][3] = atan2f(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees + LegDynamixels[leg][4] = -(acosf(DesVect2[2]/10));//Tarsus #if defined(UserSerialTransmit) UserSerial.print("FirstShift x "); From 0b6377a2935ce5afd731ffbc157f17d9f49499a0 Mon Sep 17 00:00:00 2001 From: AKautz Date: Wed, 24 Oct 2018 11:23:18 -0400 Subject: [PATCH 5/6] started the code for leg placement mode --- HexapodCodeV3/a_presetup.ino | 3 +- HexapodCodeV3/k_sphericalcoords.ino | 116 ++++++++++++++++------------ 2 files changed, 70 insertions(+), 49 deletions(-) diff --git a/HexapodCodeV3/a_presetup.ino b/HexapodCodeV3/a_presetup.ino index bb37a36..6db3aee 100644 --- a/HexapodCodeV3/a_presetup.ino +++ b/HexapodCodeV3/a_presetup.ino @@ -55,7 +55,8 @@ #define MOVE_MODE_WALK_PERIODIC 0 #define MOVE_MODE_CRAWL_PERIODIC 1 #define MOVE_MODE_SWERVE 2 -#define MOVE_MODE_WALK_RULE 3 +#define MOVE_MODE_LEG_PLACE 3 +#define MOVE_MODE_WALK_RULE 4 //Not written yet uint8_t MoveMode = MOVE_MODE_WALK_PERIODIC; //uint8_t MoveMode = MOVE_MODE_CRAWL_PERIODIC; diff --git a/HexapodCodeV3/k_sphericalcoords.ino b/HexapodCodeV3/k_sphericalcoords.ino index e15b958..c13dd2b 100644 --- a/HexapodCodeV3/k_sphericalcoords.ino +++ b/HexapodCodeV3/k_sphericalcoords.ino @@ -2,29 +2,29 @@ /* -+Z + +Z ^ +X | / | / | / | / o------------> +Y - -X is to the front of the servo - - -*/ + + X is to the front of the servo + + + */ void LocalSpherical(float Angles[NUM_LEGS][2]){ /*float LocalLeg1[2]={Angles[0][0],Angles[0][1]}; - float LocalLeg2[2]={-Angles[1][0],-Angles[1][1]}; - float LocalLeg3[2]={-Angles[2][0],-Angles[2][1]}; - float LocalLeg3[2]={Angles[3][0],Angles[3][1]};*/ + float LocalLeg2[2]={-Angles[1][0],-Angles[1][1]}; + float LocalLeg3[2]={-Angles[2][0],-Angles[2][1]}; + float LocalLeg3[2]={Angles[3][0],Angles[3][1]};*/ Angles[1][0] *= -1; Angles[1][1] *= -1; Angles[2][0] *= -1; Angles[2][1] *= -1; - + for(int i = 0; i < NUM_LEGS; i++){ LocalServoGen(i,Angles[i][0],Angles[i][1],LegDynamixels[i][0],LegDynamixels[i][1],LegDynamixels[i][2]); } @@ -36,56 +36,76 @@ void LocalSpherical(float Angles[NUM_LEGS][2]){ void LocalServoGen(int leg,float azimuth, float elevation, float coxa, float femur, float tibia){//Calculates new TibaRot and Tarsus based on local azimuth and elevation //float IniVect[3] = {cos(coxa)*sin(femur+tibia),sin(coxa)*cos(femur+tibia),cos(femur+tibia)};//vector of the leg(x,y,z) azimuth -=coxa;//The first rotation is faster this way, no trig compared to a cartesian rotation - float DesVect1[3] = {10*cosf(azimuth)*sinf(elevation),10*sinf(azimuth)*sinf(elevation),10*cosf(elevation)};//output vector(x,y,z) + float DesVect1[3] = { + 10*cosf(azimuth)*sinf(elevation),10*sinf(azimuth)*sinf(elevation),10*cosf(elevation) };//output vector(x,y,z) float DesVect2[3]; //desired vector - + float pitch = (1.5708 + femur + tibia); UserSerial.println(pitch); - - + + float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords - { cosf(pitch),0,sinf(pitch)}, - { 0,1, 0}, - {-sinf(pitch),0,cosf(pitch)} - }; - - #if defined(UserSerialTransmit) - UserSerial.print("Angle x "); - UserSerial.println(DesVect1[0]); - UserSerial.print("Angle y "); - UserSerial.println(DesVect1[1]); - UserSerial.print("Angle z "); - UserSerial.println(DesVect1[2]); - #endif - - + {cosf(pitch),0,sinf(pitch)}, + {0,1,0}, + {-sinf(pitch),0,cosf(pitch)}}; + + +#if defined(UserSerialTransmit) + UserSerial.print("Angle x "); + UserSerial.println(DesVect1[0]); + UserSerial.print("Angle y "); + UserSerial.println(DesVect1[1]); + UserSerial.print("Angle z "); + UserSerial.println(DesVect1[2]); +#endif + + //Preform the second rotation DesVect2[0] = XZCorr[0][0]*DesVect1[0] + XZCorr[0][1]*DesVect1[1] + XZCorr[0][2]*DesVect1[2]; DesVect2[1] = XZCorr[1][0]*DesVect1[0] + XZCorr[1][1]*DesVect1[1] + XZCorr[1][2]*DesVect1[2]; DesVect2[2] = XZCorr[2][0]*DesVect1[0] + XZCorr[2][1]*DesVect1[1] + XZCorr[2][2]*DesVect1[2]; - + LegDynamixels[leg][3] = atan2f(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees LegDynamixels[leg][4] = -(acosf(DesVect2[2]/10));//Tarsus + +#if defined(UserSerialTransmit) + UserSerial.print("FirstShift x "); + UserSerial.println(DesVect2[0]); + UserSerial.print("FirstShift y "); + UserSerial.println(DesVect2[1]); + UserSerial.print("FirstShift z "); + UserSerial.println(DesVect2[2]); + + UserSerial.print("SecondShift x "); + UserSerial.println(DesVect1[0]); + UserSerial.print("SecondShift y "); + UserSerial.println(DesVect1[1]); + UserSerial.print("SecondShift z "); + UserSerial.println(DesVect1[2]); + + UserSerial.println(""); +#endif + + //other stuff +} + + +void LegPlace (uint8_t leg, int Xplace, int Yplace, int Zplace, int Xrot, int Yrot, int Zrot){ + int Output1[NUM_LEGS][3] = {//inital places + { //Leg 0 + Leg0InitX, Leg0InitY, Leg0InitZ}, + { //Leg 1 + Leg1InitX, Leg1InitY, Leg1InitZ}, + { //Leg 2 + Leg2InitX, Leg2InitY, Leg2InitZ}, + { //Leg 3 + Leg3InitX, Leg3InitY, Leg3InitZ} + }; - #if defined(UserSerialTransmit) - UserSerial.print("FirstShift x "); - UserSerial.println(DesVect2[0]); - UserSerial.print("FirstShift y "); - UserSerial.println(DesVect2[1]); - UserSerial.print("FirstShift z "); - UserSerial.println(DesVect2[2]); - - UserSerial.print("SecondShift x "); - UserSerial.println(DesVect1[0]); - UserSerial.print("SecondShift y "); - UserSerial.println(DesVect1[1]); - UserSerial.print("SecondShift z "); - UserSerial.println(DesVect1[2]); - - UserSerial.println(""); - #endif - //other stuff } + + + From 9796fb99ff1f45f0df016cf11149959aba684283 Mon Sep 17 00:00:00 2001 From: AKautz Date: Sat, 3 Nov 2018 14:45:53 -0400 Subject: [PATCH 6/6] updated use of gaits and fixed rotation of crawl gait --- HexapodCodeV3/HexapodCodeV3.ino | 2 +- HexapodCodeV3/a_presetup.ino | 2 +- HexapodCodeV3/d1_gait2.ino | 4 ++-- HexapodCodeV3/i_inputs.ino | 10 +++++++++- HexapodCodeV3/j_ControlReset.ino | 2 +- 5 files changed, 14 insertions(+), 6 deletions(-) diff --git a/HexapodCodeV3/HexapodCodeV3.ino b/HexapodCodeV3/HexapodCodeV3.ino index 99b77fa..1dd29ff 100644 --- a/HexapodCodeV3/HexapodCodeV3.ino +++ b/HexapodCodeV3/HexapodCodeV3.ino @@ -44,7 +44,7 @@ 2 -Ext1 Follows - Ext1 3 -Ext2 Follows - Ext2 4 -Leg Place mode - Selected leg, numbers are as above - X->x,Y->y,Z->Z,Rz->WheelAzimuth,Rx->WheelElevation, Ry to wheel rotation - 5 -GaitMode - Selected Gait - 0:MathGait,1:SwerveSteer,2:RuleGait + 5 -GaitMode - Selected Gait - 0:WalkPeriodic,1:CrawlPeriodic,2:SwerveSteer,3:RuleGait 6 - 7 - 8 - diff --git a/HexapodCodeV3/a_presetup.ino b/HexapodCodeV3/a_presetup.ino index 6db3aee..a1d76ac 100644 --- a/HexapodCodeV3/a_presetup.ino +++ b/HexapodCodeV3/a_presetup.ino @@ -88,7 +88,7 @@ uint8_t InputExtend2; //GaitGenControlVars -int GaitPeriod = 2500; //walk period +int GaitPeriod = 2000; //walk period int GaitMoveX = 0; int GaitMoveY = 0; int GaitStepHeight = 0; diff --git a/HexapodCodeV3/d1_gait2.ino b/HexapodCodeV3/d1_gait2.ino index 8b19156..3f600a1 100644 --- a/HexapodCodeV3/d1_gait2.ino +++ b/HexapodCodeV3/d1_gait2.ino @@ -10,7 +10,7 @@ const uint8_t GaitLegOffset[NUM_LEGS] = {0,2,1,3}; 3(3) 1(2) */ -int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cycle,int Xtrans, int Ytrans,int Ztrans, int ZTurn, int NumLegsPeriod = NUM_LEGS){//translation for gait2 +int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cycle,int Xtrans, int Ytrans,int Ztrans, float ZTurn, int NumLegsPeriod = NUM_LEGS){//translation for gait2 float Generator0 = (Cycle-((GaitLegOffset[LegNumber]*Period)/NumLegsPeriod))%Period; //generates cyclic values offeset for leg number. float Modifiers[4] = {0,0,0,0}; @@ -22,7 +22,7 @@ int GaitGen2Cycle(int InputArray[NUM_LEGS][3],int LegNumber, int Period, int Cyc Modifiers[0] = Xtrans*Generator1; Modifiers[1] = Ytrans*Generator1; Modifiers[2] = Ztrans; - Modifiers[3] = (ZTurn * 3.14 / 4 *Generator1); + Modifiers[3] = ((float)ZTurn * 3.14 / 4.0 *Generator1); } else{//Ground Leg Segment float Generator1 = ((2.0*(float)NumLegsPeriod/Period)*((-Generator0/(NumLegsPeriod-1))+(Period/(2.0*((float)NumLegsPeriod-1)))));//Periodic function that goes from 1 to -1 in NumLegs-1/NumLegs of period diff --git a/HexapodCodeV3/i_inputs.ino b/HexapodCodeV3/i_inputs.ino index bc0ded8..fdacad6 100644 --- a/HexapodCodeV3/i_inputs.ino +++ b/HexapodCodeV3/i_inputs.ino @@ -161,7 +161,15 @@ void GetInputs(){ GaitBodyZ = BODYUD; GaitBodyRoll = BODYROLL; } - + + if (bitRead(InputButtons, ButtonRight1)){ + MoveMode = MOVE_MODE_WALK_PERIODIC; + } + else if (bitRead(InputButtons, ButtonRight2)){ + MoveMode = MOVE_MODE_CRAWL_PERIODIC; + } + + switch (InputExtend1 >> 4) { case 0://Reset Exended byte values break; diff --git a/HexapodCodeV3/j_ControlReset.ino b/HexapodCodeV3/j_ControlReset.ino index 46ca48d..872524f 100644 --- a/HexapodCodeV3/j_ControlReset.ino +++ b/HexapodCodeV3/j_ControlReset.ino @@ -1,7 +1,7 @@ //Code to reset Gait gen control variables //Stops robot and sets cycle to 5 seconds and sep height to 10 cm void GaitGenControlReset(){ - GaitPeriod = 2500; + GaitPeriod = 2000; GaitMoveX = 0; GaitMoveY = 0; GaitStepHeight = 10;