Here’s an arduino sketch that can control an AH2 hexapod with an AL5 Arm on top!
#include <PS2X_lib.h>
PS2X ps2x;
int error = 0;
byte type = 0;
#define Deadzone 10
char ArmSize = 'A'; // Must be 'A', 'B', or 'D' DO NOT REMOVE THE SEMICOLON OR THE ' '
float A;
float B;
float rtod = 57.295779;
float atop = 11.111;
float Arm_Speed = 1.0;
int Arm_sps = 3;
int Hex_Speed = 100;
int Hex_sps = 2;
int High = 2000;
int Middle = 1667;
int Low = 2000;
int Height_Setting = 1;
int Gait_Setting = 1;
boolean mode = true;
float X = 4;
float Y = 4;
int Z = 1500;
int G = 1500;
int WR = 1500;
float WA = 0;
float tmpx = 4;
float tmpy = 4;
int tmpz = 1500;
int tmpg = 1500;
int tmpwr = 1500;
float tmpwa = 0;
void setup()
{
Serial.begin(115200);
error = ps2x.config_gamepad(13,11,12,10, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
if(error == 0)
Serial.println("Found Controller, configured successful");
else if(error == 1)
Serial.println("No controller found");
else if(error == 2)
Serial.println("not accepting commands");
else if(error == 3)
Serial.println("refusing Pressures mode");
type = ps2x.readType();
switch(type)
{
case 0:
Serial.println("Unknown");
break;
case 1:
Serial.println("DualShock");
break;
case 2:
Serial.println("GuitarHero");
break;
}
delay(1000);
Serial.println("LH1000 LM1333 LL2000 RH2000 RM1667 RL1000 VS3000");
Serial.println("LF1700 LR1300 RF1300 RR1700 HT750");
switch(ArmSize)
{
case 'A':
A = 3.75;
B = 4.25;
break;
case 'B':
A = 4.75;
B = 5.00;
break;
case 'D':
A = 5.75;
B = 7.375;
break;
}
}
int Arm(float x, float y, float z, int g, float wa, int wr) //Here's all the Inverse Kinematics to control the arm
{
float M = sqrt((y*y)+(x*x));
if(M <= 0)
return 1;
float A1 = atan(y/x);
float A2 = acos((A*A-B*B+M*M)/((A*2)*M));
float Elbow = acos((A*A+B*B-M*M)/((A*2)*B));
float Shoulder = A1 + A2;
Elbow = Elbow * rtod;
Shoulder = Shoulder * rtod;
while((int)Elbow <= 0 || (int)Shoulder <= 0)
return 1;
float Wrist = abs(wa - Elbow - Shoulder) + 7.50;
Elbow = (180 - Elbow) * atop;
Shoulder = Shoulder*atop + 500;
Wrist = (180 - Wrist) * atop + 500;
Serial.print("#10 P");
Serial.println(z);
Serial.print(" #11 P");
Serial.print(Shoulder);
Serial.print(" #12 P");
Serial.print(Elbow);
Serial.print(" #13 P");
Serial.print(Wrist);
Serial.print(" #14 P");
Serial.print(g);
Serial.print(" #15 P");
Serial.print(wr);
Serial.println(" T150");
Y = tmpy;
X = tmpx;
Z = tmpz;
WA = tmpwa;
G = tmpg;
WR = tmpwr;
return 0;
}
void Arm_mode()
{
int LSY = 128 - ps2x.Analog(PSS_LY);
int LSX = ps2x.Analog(PSS_LX) - 128;
int RSY = 128 - ps2x.Analog(PSS_RY);
int RSX = ps2x.Analog(PSS_RX) - 128;
if(RSY > Deadzone || RSY < -Deadzone)
tmpy = Y + RSY/1000.0*Arm_Speed;
if(tmpy < -1)
tmpy = -1;
if(RSX > Deadzone || RSX < -Deadzone)
tmpx = X + RSX/1000.0*Arm_Speed;
if(tmpx < -0.5)
tmpx = -0.5;
if(LSY > Deadzone || LSY < -Deadzone)
tmpwa = WA + LSY/100.0*Arm_Speed;
if(tmpwa > 180)
tmpwa = 180;
else if(tmpwa < 0)
tmpwa = 0;
if(LSX > Deadzone || LSX < -Deadzone)
tmpz = Z + LSX/10.0*Arm_Speed;
if(tmpz > 2500)
tmpz = 2500;
else if(tmpz < 500)
tmpz = 500;
if(ps2x.Button(PSB_R1))
tmpg = G + 20*Arm_Speed;
if(ps2x.Button(PSB_R2))
tmpg = G - 20*Arm_Speed;
if(tmpg > 2500)
tmpg = 2500;
else if(tmpg < 500)
tmpg = 500;
if(ps2x.Button(PSB_L1))
tmpwr = WR + 20*Arm_Speed;
else if(ps2x.Button(PSB_L2))
tmpwr = WR - 20*Arm_Speed;
if(tmpwr > 2500)
tmpwr = 2500;
if(tmpwr < 500)
tmpwr = 500;
if(ps2x.ButtonPressed(PSB_PAD_UP))
{
Arm_sps = Arm_sps + 1;
if(Arm_sps > 5)
Arm_sps = 5;
tone(5, Arm_sps*500, 200);
}
else if(ps2x.ButtonPressed(PSB_PAD_DOWN))
{
Arm_sps = Arm_sps - 1;
if(Arm_sps < 1)
Arm_sps = 1;
tone(5, Arm_sps*500, 200);
}
Arm_Speed = Arm_sps*0.20 + 0.60;
if(Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr))
{
// Serial.print("ERROR");
}
if(tmpx < 2 && tmpy < 2 && RSX < 0)
{
tmpy = tmpy + 0.05;
Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);
}
else if(tmpx < 1 && tmpy < 2 && RSY < 0)
{
tmpx = tmpx + 0.05;
Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr);
}
}
void Gait_Set()
{
Serial.print("LH");
Serial.print(3000 - High);
Serial.print(" LM");
Serial.print(3000 - Middle);
Serial.print(" LL");
Serial.print(3000 - Low);
Serial.print(" RH");
Serial.print(High);
Serial.print(" RM");
Serial.print(Middle);
Serial.print(" RL");
Serial.print(Low);
Serial.println(" VS3000");
}
void Hex_mode()
{
int LSY = (256 - ps2x.Analog(PSS_LY))/256.0 * 200 - 100;
int RSY = (256 - ps2x.Analog(PSS_RY))/256.0 * 200 - 100;
if(LSY > Deadzone || LSY < -Deadzone || RSY > Deadzone || RSY < -Deadzone)
{
Serial.print("XL");
Serial.print(LSY);
Serial.print(" XR");
Serial.print(RSY);
Serial.print(" XS");
Serial.println(Hex_Speed);
}
else
Serial.println("XL0 XR0 XS0");
if(ps2x.ButtonPressed(PSB_BLUE))
{
High = 2000;
Middle = 1667;
Low = 1000;
Gait_Set();
Gait_Setting = 1;
}
else if(ps2x.ButtonPressed(PSB_RED))
{
High = 2100;
Middle = 1700;
Low = 900;
Gait_Set();
Gait_Setting = 2;
}
else if(ps2x.ButtonPressed(PSB_GREEN))
{
High = 2500;
Middle = 1000;
Low = 1000;
Gait_Set();
Gait_Setting = 3;
}
else if(ps2x.ButtonPressed(PSB_PINK))
{
High = 2500;
Middle = 2500;
Low = 500;
Gait_Set();
Gait_Setting = 4;
}
if(ps2x.ButtonPressed(PSB_PAD_UP))
{
if(Gait_Setting == 1)
{
Low = Low - 200;
if(Low < 1000)
Low = 1000;
else
{
Middle = (High - Low) * 2 / 3 + Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 2)
{
Low = Low - 200;
if(Low < 900)
Low = 900;
else
{
Middle = (High - Low) / 3 + Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 3)
{
Low = Low - 200;
if(Low < 1000)
Low = 1000;
else
{
Middle = Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 4)
{
Low = Low - 200;
if(Low < 500)
Low = 500;
else
{
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
}
if(ps2x.ButtonPressed(PSB_PAD_DOWN))
{
if(Gait_Setting == 1)
{
Low = Low + 200;
if(Low > 1800)
Low = 1800;
else
{
Middle = (High - Low) * 2 / 3 + Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 2)
{
Low = Low + 200;
if(Low > 2100)
Low = 2100;
else
{
Middle = (High - Low) / 3 + Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 3)
{
Low = Low + 200;
if(Low > 1800)
Low = 1800;
else
{
Middle = Low;
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
else if(Gait_Setting == 4)
{
Low = Low + 200;
if(Low > 2300)
Low = 2000;
else
{
Gait_Set();
Serial.println("XL0 XR0 XS200");
}
}
}
if(ps2x.ButtonPressed(PSB_PAD_RIGHT))
{
Hex_sps = Hex_sps + 1;
if(Hex_sps > 5)
Hex_sps = 5;
tone(5, Hex_sps*500, 200);
}
else if(ps2x.ButtonPressed(PSB_PAD_LEFT))
{
Hex_sps = Hex_sps - 1;
if(Hex_sps < 1)
Hex_sps = 1;
tone(5, Hex_sps*500, 200);
}
Hex_Speed = Hex_sps*25 + 50;
}
void Tripod_Down()
{
Serial.print("#0 P");
Serial.print(3000 - High);
Serial.print(" #2 P");
Serial.print(3000 - High);
Serial.print(" #4 P");
Serial.print(3000 - High);
Serial.print(" #16 P");
Serial.print(High);
Serial.print(" #18 P");
Serial.print(High);
Serial.print(" #20 P");
Serial.println(High);
}
void loop()
{
ps2x.read_gamepad(); //update the ps2 controller
if(ps2x.ButtonPressed(PSB_SELECT))
{
mode = !mode;
if(!mode)
{
Serial.println("XSTOP");
Tripod_Down();
}
else if(mode)
{
Serial.println("XL0 XR0 XS200");
delay(500);
}
}
if(mode)
Hex_mode();
else
Arm_mode();
delay(30);
}
This code uses the following controls:
Hexapod:
Tank mode steering using the left and right sticks.
Speed Up : Pad Right
Slow Down : Pad Left
Ride Height lower : Pad Down
Ride Height Raise : Pad Up
Gait setting : Cross, Triangle, Square, Circle ::See picture::
Toggle Control modes : Select
Arm:
Right stick Y axis: Move arm Up/Down
Right stick X axis: Move arm In/Out
Left stick Y axis: Change wrist angle
Left stick X axis: Rotate Base
R1: Close gripper
R2: Open gripper
L1: rotate wrist CW
L2: rotate wrist CCW
Pad Up: increase speed
Pad Down: decrease speed
To select which arm you wish to use change this line of code:
char ArmSize = 'B'; // Must be 'A', 'B', or 'D' DO NOT REMOVE THE SEMICOLON OR THE ' '