Merge pull request #1 from KautzA/Testing

Update Master from Testing

HexapodCodeV3/HexapodCodeV3.ino

@@ -43,8 +43,8 @@
        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
-       5 -GaitMode        - Selected Gait                         - 0:MathGait,1:SwerveSteer,2:RuleGait
+       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:WalkPeriodic,1:CrawlPeriodic,2:SwerveSteer,3:RuleGait
        6 -
        7 -
        8 -

HexapodCodeV3/a_presetup.ino

@@ -22,6 +22,8 @@
 #include<ax12Serial.h>   //KurtE's bioloid library https://github.com/KurtE/BioloidSerial
 #include<BioloidSerial.h> //KurtE's bioloid library https://github.com/KurtE/BioloidSerial
 
+#include<math.h> //enable use of cosf and sinf to use FPU
+
 #include <i2c_t3.h> //Teensy3.x I2C library to use hardware i2c
 
 #if defined(GIMBAL_ENABLE)//Gimbal Library
@@ -53,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;
@@ -85,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;

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();

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

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
@@ -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

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}
   };
 

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
   }

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;

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;

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*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
 
-  
+
   float pitch = (1.5708 + femur + tibia);
   UserSerial.println(pitch);
-  
-  
+
+
   float XZCorr[3][3] = {//Matrix to rotate point to be in proper loaction for sphericalcoords
-   { cos(pitch),0,sin(pitch)},
-   {          0,1,          0},
-   {-sin(pitch),0,cos(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}
+  };
 
-  LegDynamixels[leg][3] = atan2(DesVect2[1],DesVect2[0]);//Tibia Rotation in Degrees
-  LegDynamixels[leg][4] = -(acos(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
 }
+
+
+

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);
@@ -40,7 +44,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];