HI,
Attached is a slightly better pic of the board with the exposure bumped up a bit. Also attached are images of the wall packs.
Q:“did you use the servo to logic power jumper?”
A: I think so. see pic
Basically I followed the tutorial on the website. The wireless control from Lynxmotion arrived today. The stutter went away.
However, the controller or the board or some combination seems to time out- stop moving and/or respond to controller commands sporadically (on not at all). I usually restart the board and then the controller. If they sync up then I may be good for a period of time and then… Also the controls are very temperamental in general. I’m wondering if this is par for the course with a botboardarduino and a LynxmotionPS2 or is something awry?
As this will be for kids, I’m hoping to make the controls even more simple via 12 arcade buttons(one for each direction). Aside from being more durable than the controller, they’ll make using the servos a more visibly concerted effort. _if you want to move the arm to point B, then the kid will have to figure out which combination of servos to activate. If that makes sense.
I know already that modding the controller is a can of worms but, I do wonder if in the long run for our purposes it’s the way to go.
Any help in a timely fashion is greatly appreciated.
Thanks in Advance
-Andy
Below is the code generously and expeditiously posted by Devon on GitHub.
[code]#include <Servo.h>
#include <PS2X_lib.h>
//#define DEBUG
//comment to disable the Force Sensitive Resister on the gripper
//#define FSRG
//Select which arm by uncommenting the corresponding line
//#define AL5A
//#define AL5B
#define AL5D
//FSRG pin Must be analog!!
#define FSRG_pin A1
//uncomment for digital servos in the Shoulder and Elbow
//that use a range of 900ms to 2100ms
//#define DIGITAL_RANGE
#ifdef AL5A
const float A = 3.75;
const float B = 4.25;
#elif defined AL5B
const float A = 4.75;
const float B = 5.00;
#elif defined AL5D
const float A = 5.75;
const float B = 7.375;
#endif
//PS2 pins
#define DAT 6
#define CMD 7
#define ATT 8
#define CLK 9
//PS2 analog joystick Deadzone
#define Deadzone 4
//Arm Servo pins
#define Base_pin 2
#define Shoulder_pin 3
#define Elbow_pin 4
#define Wrist_pin 10
#define Gripper_pin 11
#define WristR_pin 12
//Onboard Speaker
#define Speaker_pin 5
//Radians to Degrees constant
const float rtod = 57.295779;
//Arm Speed Variables
float Speed = 1.0;
int sps = 3;
//Servo Objects
Servo Elb;
Servo Shldr;
Servo Wrist;
Servo Base;
Servo WristR;
Servo Gripper;
//Arm Current Pos
float X = 4;
float Y = 4;
int Z = 90;
int G = 90;
int WR = 90;
float WA = 0;
//Arm temp pos
float tmpx = 4;
float tmpy = 4;
int tmpz = 90;
int tmpg = 90;
int tmpwr = 90;
float tmpwa = 0;
//PS2X Variables
PS2X ps2x;
int error = 0;
byte type = 0;
boolean mode = true;
void setup()
{
#ifdef DEBUG
Serial.begin(115200);
#endif
Base.attach(Base_pin);
Shldr.attach(Shoulder_pin);
Elb.attach(Elbow_pin);
Wrist.attach(Wrist_pin);
Gripper.attach(Gripper_pin);
WristR.attach(WristR_pin);
pinMode(7, INPUT);
if(!digitalRead(7))
{
Base.write(90);
Shldr.write(90);
Elb.write(90);
Wrist.write(90);
Gripper.write(90);
WristR.write(90);
delay(2000);
while(digitalRead(7));
}
error = ps2x.config_gamepad(CLK,CMD,ATT,DAT, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
#ifdef DEBUG
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;
}
#endif
}
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((yy)+(xx));
if(M <= 0)
return 1;
float A1 = atan(y/x);
if(x <= 0)
return 1;
float A2 = acos((AA-BB+MM)/((A2)M));
float Elbow = acos((AA+BB-MM)/((A*2)*B));
float Shoulder = A1 + A2;
Elbow = Elbow * rtod;
Shoulder = Shoulder * rtod;
if((int)Elbow <= 0 || (int)Shoulder <= 0)
return 1;
float Wris = abs(wa - Elbow - Shoulder) - 90;
#ifdef DIGITAL_RANGE
Elb.writeMicroseconds(map(180 - Elbow, 0, 180, 900, 2100 ));
Shldr.writeMicroseconds(map(Shoulder, 0, 180, 900, 2100));
#else
Elb.write(180 - Elbow);
Shldr.write(Shoulder);
#endif
Wrist.write(180 - Wris);
Base.write(z);
WristR.write(wr);
#ifndef FSRG
Gripper.write(g);
#endif
Y = tmpy;
X = tmpx;
Z = tmpz;
WA = tmpwa;
#ifndef FSRG
G = tmpg;
#endif
WR = tmpwr;
return 0;
}
void loop()
{
ps2x.read_gamepad(); //update the ps2 controller
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 = max(Y + RSY/1000.0*Speed, -1);
if(RSX > Deadzone || RSX < -Deadzone)
tmpx = max(X + RSX/1000.0*Speed, -0.5);
if(LSY > Deadzone || LSY < -Deadzone)
tmpwa = constrain(WA + LSY/100.0*Speed, 0, 180);
if(LSX > Deadzone || LSX < -Deadzone)
tmpz = constrain(Z + LSX/100.0*Speed, 0, 180);
if(ps2x.Button(PSB_R1))
{
#ifdef FSRG
while(analogRead(FSRG_pin) < 400)
{
Gripper.write(min(Gripper.read() + 2, 170));
if(Gripper.read() == 170)
break;
#ifdef DEBUG
Serial.print(analogRead(FSRG_pin));
Serial.print(" ");
Serial.println(Gripper.read());
#endif
delay(10);
}
#else
tmpg = min(G + 5*Speed, 170);
#endif
}
if(ps2x.Button(PSB_R2))
{
#ifdef FSRG
while(Gripper.read() > 90)
{
Gripper.write(max(Gripper.read() - 2, 90));
#ifdef DEBUG
Serial.println(Gripper.read());
#endif
delay(10);
}
#else
tmpg = max(G - 5*Speed, 10);
#endif
}
if(ps2x.Button(PSB_L1))
tmpwr = max(WR + 2Speed, 0);
else if(ps2x.Button(PSB_L2))
tmpwr = min(WR - 2Speed, 180);
if(ps2x.ButtonPressed(PSB_PAD_UP))
{
sps = min(sps + 1, 5);
tone(Speaker_pin, sps500, 200);
}
else if(ps2x.ButtonPressed(PSB_PAD_DOWN))
{
sps = max(sps - 1, 1);
tone(Speaker_pin, sps500, 200);
}
Speed = sps*0.20 + 0.60;
if(Arm(tmpx, tmpy, tmpz, tmpg, tmpwa, tmpwr))
{
#ifdef DEBUG
Serial.print(“NONREAL Answer”);
#endif
}
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);
}
delay(30);
}[/code]
