Programming a 6dof robot arm controlled by ps2 controller/receiver using Arduino Mega. I have seen ones with custom shield/controller but programming it for the Mega and using servo.h. Below is the code, starting with 4 servo, and I'll add the last 2. But the last two should be the same functions, right? When running the program and using the PS2, the servo is not moving. Any ideas or changes I need to do to the code? Pins 22, 24, 26, and 28 are digital pins. The first part are for the 4DC motors which worked but now trying to make the servos work. The D-pad of the PS2 controls the motors (car), and the sticks and other buttons of the ps2 will control the robot arm. servo1 is the base of the arm.
// include libraries
#include <PS2X_lib.h>
#include <Servo.h>
Servo Servo1; //Declaring cradle servos, Servo1 is the base
Servo Servo2;
Servo Servo3;
Servo Servo4;
PS2X ps2x; // create PS2 Controller Class
int error = 0;
byte type = 0;
byte vibrate = 0;
int a,b,c,d;
// These are used to set the direction of the bridge driver.
#define ENA 3 //ENA
#define MOTORA_1 4 //IN3
#define MOTORA_2 5 //IN4
#define MOTORB_1 8 //IN1
#define MOTORB_2 7 //IN2
#define ENB 6 //ENB
void setup(){
// Start serial communication
Serial.begin(57600);
error = ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
// Configure output pins
pinMode(ENA, OUTPUT);
pinMode(MOTORA_1, OUTPUT);
pinMode(MOTORA_2, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(MOTORB_1, OUTPUT);
pinMode(MOTORB_2, OUTPUT);
// Disable both motors
digitalWrite(ENA,0);
digitalWrite(ENB,0);
Servo1.attach(22);
Servo2.attach(24);
Servo3.attach(26);
Servo4.attach(28);
// Check for error
if(error == 0){
Serial.println("Found Controller, configured successful");
}
else if(error == 1)
Serial.println("No controller found, check wiring or reset the Arduino");
else if(error == 2)
Serial.println("Controller found but not accepting commands");
else if(error == 3)
Serial.println("Controller refusing to enter Pressures mode, may not support it.");
// Check for the type of controller
type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type");
break;
case 1:
Serial.println("DualShock Controller Found");
break;
case 2:
Serial.println("GuitarHero Controller Found");
break;
}
}
// Main loop
void loop(){
a=Servo1.read();
b=Servo2.read();
c=Servo3.read();
d=Servo4.read();
if(error == 1) //skip loop if no controller found
return;
else { //DualShock Controller
ps2x.read_gamepad(false, vibrate); // disable vibration of the controller
// Perform movements based on D-pad buttons
// MOVE FORWARD
if(ps2x.Button(PSB_PAD_UP)) {
digitalWrite(MOTORA_1,LOW);
digitalWrite(MOTORA_2,HIGH);
digitalWrite(MOTORB_1,HIGH);
digitalWrite(MOTORB_2,LOW);
analogWrite(ENB, 1023);
analogWrite(ENA, 1023);
Serial.println("Move forward");
}
// TURN RIGHT
if(ps2x.Button(PSB_PAD_RIGHT)){
digitalWrite(MOTORA_1,HIGH);
digitalWrite(MOTORA_2,LOW);
digitalWrite(MOTORB_1,HIGH);
digitalWrite(MOTORB_2,LOW);
analogWrite(ENB, 1023);
analogWrite(ENA, 1023);
Serial.println("Turn right");
}
// TURN LEFT
if(ps2x.Button(PSB_PAD_LEFT)){
digitalWrite(MOTORA_1,LOW);
digitalWrite(MOTORA_2,HIGH);
digitalWrite(MOTORB_1,LOW);
digitalWrite(MOTORB_2,HIGH);
analogWrite(ENB, 1023);
analogWrite(ENA, 1023);
Serial.println("Turn left");
}
// MOVE BACK
if(ps2x.Button(PSB_PAD_DOWN)){
digitalWrite(MOTORA_1,HIGH);
digitalWrite(MOTORA_2,LOW);
digitalWrite(MOTORB_1,LOW);
digitalWrite(MOTORB_2,HIGH);
analogWrite(ENB, 1023);
analogWrite(ENA, 1023);
Serial.println("Move back");
}
if (!ps2x.Button(PSB_PAD_DOWN) && !ps2x.Button(PSB_PAD_UP) && !ps2x.Button(PSB_PAD_RIGHT) && ! ps2x.Button(PSB_PAD_LEFT)) {
analogWrite(ENB, 0);
analogWrite(ENA, 0);
}
//Robot Arm using Stick
//Cradle Right
if(255-ps2x.Analog(PSS_RX) < 120){ //If the stick is pointing to the right.
Serial.println("C Right"); //We read the current position of the servo (a=Servo1.read())
Serial.println(ps2x.Analog(PSS_RX)); //And we reduce "1"
a=a-1; //We then write a new position, which is a-1.
Servo1.write(a); //You can change "1" to a higher one so it turns quickly
}
//Cradle Left
if(255-ps2x.Analog(PSS_RX) > 130){
Serial.println("C Left");
Serial.println(ps2x.Analog(PSS_RX));
a=a+1;
Servo1.write(a);
}
//Cradle Up
if(255-ps2x.Analog(PSS_RY) < 120){
Serial.println("C Up");
Serial.println(ps2x.Analog(PSS_RY));
b=b+1;
Servo2.write(b);
}
//Cradle Down
if(255-ps2x.Analog(PSS_RY) > 130){
Serial.println("C Down");
Serial.println(ps2x.Analog(PSS_RY));
b=b-1;
Servo2.write(b);
}
//Cradle Up
if(255-ps2x.Analog(PSS_LX) < 120){
Serial.println("C U");
Serial.println(ps2x.Analog(PSS_LX));
c=c+1;
Servo3.write(c);
}
//Cradle Down
if(255-ps2x.Analog(PSS_LX) > 130){
Serial.println("C D");
Serial.println(ps2x.Analog(PSS_LX));
c=c-1;
Servo3.write(c);
}
//Cradle Up
if(255-ps2x.Analog(PSS_LY) < 120){
Serial.println("C R");
Serial.println(ps2x.Analog(PSS_LY));
d=d-1;
Servo4.write(d);
}
//Cradle Down
if(255-ps2x.Analog(PSS_LY) > 130){
Serial.println("C L");
Serial.println(ps2x.Analog(PSS_LY));
d=d+1;
Servo4.write(d);
}
delay(50);
}
}