Here's the code about how I control the robot using PSX and arduino Mega 2560. I make the code in such a modular way(no need but it's ok, forgive me lol)
Some command is in Bahasa Indonesia.
1. Main Program which call every tabs
/* bismillahirrohmaanirrohiim
ALLAHUAKBAR
=====================18 APRIL 2014====================================*/
/* MASTER
ChangeLog: 1. slider 2. single motor 3. otomatisasi
*/
#include
//PS2 Library
#define naik 0
#define turun 1
//initialization
PS2X ps2x;
int PS2_error = 0;
int PS2_AnalogLX = 0;
int PS2_AnalogLY = 0;
int PS2_AnalogRX = 0;
int PS2_AnalogRY = 0;
int translateX, translateY, rotateTheta;
void setup() {
Serial.begin(57600);
PS2_error = ps2x.config_gamepad(50,51,52,53, true, false); //(clock, command, attention, data, Pressures?, Rumble?)
if (PS2_error == 0) Serial.println("SIAP"); //stick detected
else if(PS2_error == 1) Serial.println("Gak ada stik, cek kabel");
else if(PS2_error == 2) Serial.println("Stik terdeteksi, tapi ga bisa nerima command");
else if(PS2_error == 3) Serial.println("Stik tidak bisa memasuki mode Pressure, hilangkan mode melalui kodingan");
slaveInit();
motorInit();
seesawInit();
gripperInit();
sliderInit();
liftingInit();
pendorongInit();
singleMotorInit();
}
void loop()
{
if(PS2_error != 0)
{
delay(500);
lifting(0,0);
return;
}
else
{
ps2x.read_gamepad(); //read controller and set large motor to spin at 'vibrate' speed
//kalo stik terlepas biar gak jalan sendiri
if(ps2x.Analog(PSS_LY) == 255 && ps2x.Analog(PSS_LX) == 255 && ps2x.Analog(PSS_RY) == 255 && ps2x.Analog(PSS_RX) == 255)
{
PS2_AnalogLY = 0;
PS2_AnalogLX = 0;
PS2_AnalogRY = 0;
PS2_AnalogRX = 0;
Serial.println("STIK TERLEPAS");
motorDrive(0,0,0);
while(1);
}
else if(ps2x.Button(PSB_L2)) // turbo speed
{
PS2_AnalogLY = ((ps2x.Analog(PSS_LY) - 128));
PS2_AnalogLX = ((ps2x.Analog(PSS_LX) - 128));
PS2_AnalogRY = ((ps2x.Analog(PSS_RY) - 128));
PS2_AnalogRX = ((ps2x.Analog(PSS_RX) - 128));
}
else if(ps2x.Button(PSB_L1)) // slow mo banget seperempat speed
{
PS2_AnalogLY = (int)((ps2x.Analog(PSS_LY) - 128)/4);
PS2_AnalogLX = (int)((ps2x.Analog(PSS_LX) - 128)/4);
PS2_AnalogRY = (int)((ps2x.Analog(PSS_RY) - 128)/4);
PS2_AnalogRX = (int)((ps2x.Analog(PSS_RX) - 128)/4);
}
else //normal speed
{
PS2_AnalogLY = (int)((ps2x.Analog(PSS_LY) - 128)/2);
PS2_AnalogLX = (int)((ps2x.Analog(PSS_LX) - 128)/2);
PS2_AnalogRY = (int)((ps2x.Analog(PSS_RY) - 128)/2);
PS2_AnalogRX = (int)((ps2x.Analog(PSS_RX) - 128)/2);
}
// Serial.print("Stick Values:");
// Serial.print(PS2_AnalogLX, DEC);
// Serial.print(" ,");
// Serial.print(PS2_AnalogLY, DEC);
// Serial.print(" ,");
// Serial.print(PS2_AnalogRX, DEC);
// Serial.print(",");
// Serial.println(PS2_AnalogRY, DEC);
translateX= PS2_AnalogLX*2;
translateY= PS2_AnalogLY*2;
rotateTheta= PS2_AnalogRX*-2;
omniDrive(translateX,translateY,rotateTheta); // kalo mau pake pwm master doang
//slaveOmniPWM(translateX,translateY,rotateTheta); // kalo mau pake pwm tp master slave
//slaveOmniRPM(translateX,translateY,rotateTheta); // kalo mau pake rpm master slave
if(ps2x.ButtonPressed(PSB_PINK))
{
push();
}
if(ps2x.ButtonPressed(PSB_RED))
{
gripping(); // grip depan
}
if(ps2x.ButtonPressed(PSB_BLUE))
{
seesaw_on(); // ayunin jungkat jungkit
}
if(ps2x.Button(PSB_PAD_RIGHT))
{
slider(120,naik);
}
if(ps2x.ButtonReleased(PSB_PAD_RIGHT))
{
slider(0,turun);
}
if(ps2x.Button(PSB_PAD_LEFT))
{
slider(120,turun);
}
if(ps2x.ButtonReleased(PSB_PAD_LEFT))
{
slider(0,naik);
}
if(ps2x.Button(PSB_PAD_UP))
{
lifting(250,turun);
}
if(ps2x.ButtonReleased(PSB_PAD_UP))
{
lifting(0,naik);
}
if(ps2x.Button(PSB_PAD_DOWN))
{
lifting(250,naik);
}
if(ps2x.ButtonReleased(PSB_PAD_DOWN))
{
lifting(0,turun);
}
if (ps2x.ButtonPressed(PSB_GREEN))
{
push();
}
if (ps2x.ButtonReleased(PSB_GREEN))
{
motor(0,100,95,1,1,0,700);
motor(0,0,0,0,0,0,100);
}
// if (ps2x.ButtonPressed(PSB_R1))
// {
//
// }
if (ps2x.Button(PSB_R1))
{
slider(130,naik);
lifting2();
}
if (ps2x.ButtonReleased(PSB_R1))
{
slider(0,turun);
lifting(0,naik);
}
if (ps2x.Button(PSB_R2))
{
slider(130,turun);
lifting3();
}
if (ps2x.ButtonReleased(PSB_R2))
{
slider(0,naik);
lifting(0,turun);
}
//
// if(ps2x.ButtonReleased(PSB_PINK))
// {
// //diisi nanti
// }
//
// if(ps2x.Button(PSB_GREEN))
// {
// //diisi nanti
// }
//
// if(ps2x.ButtonReleased(PSB_GREEN))
// {
// //diisi nanti
// }
// if(ps2x.ButtonPressed(PSB_PAD_RIGHT))
// {
// ping(1);
// }
//
// if(ps2x.ButtonPressed(PSB_PAD_UP))
// {
// ping(2);
// }
// if(ps2x.ButtonPressed(PSB_PAD_LEFT))
// {
//
// ping(3);
// }
}
}
2. Actuator's Contoller
#define GRIPPER_PIN 32
char grip;
void gripperInit()
{
pinMode(GRIPPER_PIN,OUTPUT);
digitalWrite(GRIPPER_PIN,HIGH);
}
void gripping ()
{
if(grip==1)
{
grip=0;
Serial.println("GRIPPER ACTIVATED");
}
else
{
grip=1;
Serial.println("GRIPPER DEACTIVATED");
}
digitalWrite(GRIPPER_PIN,grip);
}
#define LIFTING_PWM_PIN 2
#define LIFTING_CS_PIN 26
#define MOTOR_CAP 255
void liftingInit()
{
pinMode(LIFTING_PWM_PIN,OUTPUT);
pinMode(LIFTING_CS_PIN,OUTPUT);
digitalWrite(LIFTING_CS_PIN,LOW);
analogWrite(LIFTING_PWM_PIN,0);
attachInterrupt(3,lifting2,CHANGE);
attachInterrupt(2, lifting3, CHANGE);
}
void lifting(int speed, int arah)
{
analogWrite(LIFTING_PWM_PIN,speed);
digitalWrite(LIFTING_CS_PIN,arah);
}
void lifting2()
{
analogWrite(LIFTING_PWM_PIN,250);
digitalWrite(LIFTING_CS_PIN,1);
}
void lifting3()
{
analogWrite(LIFTING_PWM_PIN,250);
digitalWrite(LIFTING_CS_PIN,0);
}
void lifting_d(int speed, int arah,int time)
{
analogWrite(LIFTING_PWM_PIN,speed);
digitalWrite(LIFTING_CS_PIN,arah);
delay(time);
}
#define PENDORONG_PIN 31
char pendorong;
void pendorongInit()
{
pinMode(PENDORONG_PIN,OUTPUT);
digitalWrite(PENDORONG_PIN,LOW);
}
void push()
{
if(pendorong==1)
{
pendorong=0;
// Serial.println("GRIPPER ACTIVATED");
}
else
{
pendorong=1;
// Serial.println("GRIPPER DEACTIVATED");
}
digitalWrite(PENDORONG_PIN,pendorong);
}
#define SEESAW_PIN 30
char seesaw;
void seesawInit()
{
pinMode(SEESAW_PIN,OUTPUT);
digitalWrite(SEESAW_PIN,LOW);
}
void seesaw_on ()
{
if(seesaw==1)
{
seesaw=0;
Serial.println("SEESAW NAIK");
}
else
{
seesaw=1;
Serial.println("SEESAW TURUN");
}
digitalWrite(SEESAW_PIN,seesaw);
}
3. Motor Control
#define CSB 28
#define CSC 29
#define CSA 27
#define PWMB 4
#define PWMC 9
#define PWMA 3
#define CCW 1
#define CW 0
#define MOTOR_CAP 255 //max 255
int lastA = 0, lastB = 0, lastC = 0, last_delay = 0;
byte stopA = 0, stopB = 0, stopC = 0;
// MOTOR INIT
void motorInit()
{
pinMode(CSA,OUTPUT);
pinMode(CSB,OUTPUT);
pinMode(CSC,OUTPUT);
digitalWrite(CSA,CCW);
digitalWrite(CSB,CCW);
digitalWrite(CSC,CCW);
pinMode(PWMA,OUTPUT);
pinMode(PWMB,OUTPUT);
pinMode(PWMC,OUTPUT);
}
//************************************
// Motor A
//************************************
void motor_A(int pwm)
{
if(pwm>MOTOR_CAP) pwm = MOTOR_CAP;
else if(pwm<-motor_cap pwm="-MOTOR_CAP;</div">
if(pwm>0)
{
digitalWrite(CSA,CCW);
analogWrite(PWMA,pwm);
stopA = 1;
if(lastA < 0) last_delay = 1;
}
else if(pwm<0 div="">
{
digitalWrite(CSA,CW);
analogWrite(PWMA,-1*pwm);
stopA = 1;
if(lastA > 0) last_delay = 1;
}
else
{
if(stopA == 0) return;
else if(digitalRead(CSA) == CCW)
{
digitalWrite(CSA,CW);
analogWrite(PWMA,0);
stopA = 0;
last_delay = 1;
}
else if(digitalRead(CSA) == CW)
{
digitalWrite(CSA,CCW);
analogWrite(PWMA,0);
stopA = 0;
last_delay = 1;
}
}
lastA = pwm;
}
//************************************
// Motor B
//************************************
void motor_B(int pwm)
{
if(pwm>MOTOR_CAP) pwm = MOTOR_CAP;
else if(pwm<-motor_cap pwm="-MOTOR_CAP;</div">
if(pwm>0)
{
digitalWrite(CSB,CCW);
analogWrite(PWMB,pwm);
stopB = 1;
if(lastB < 0) last_delay = 1;
}
else if(pwm<0 div="">
{
digitalWrite(CSB,CW);
analogWrite(PWMB,-1*pwm);
stopB = 1;
if(lastB > 0) last_delay = 1;
}
else
{
if(stopB == 0) return;
else if(digitalRead(CSB) == CCW)
{
digitalWrite(CSB,CW);
analogWrite(PWMB,0);
stopB = 0;
last_delay = 1;
}
else if(digitalRead(CSB) == CW)
{
digitalWrite(CSB,CCW);
analogWrite(PWMB,0);
stopB = 0;
last_delay = 1;
}
}
lastB = pwm;
}
//************************************
// Motor C
//************************************
void motor_C(int pwm)
{
if(pwm>MOTOR_CAP) pwm = MOTOR_CAP;
else if(pwm<-motor_cap pwm="-MOTOR_CAP;</div">
if(pwm>0)
{
digitalWrite(CSC,CCW);
analogWrite(PWMC,pwm);
stopC = 1;
if(lastC < 0) last_delay = 1;
}
else if(pwm<0 div="">
{
digitalWrite(CSC,CW);
analogWrite(PWMC,-1*pwm);
stopC = 1;
if(lastC > 0) last_delay = 1;
}
else
{
if(stopC == 0) return;
else if(digitalRead(CSC) == CCW)
{
digitalWrite(CSC,CW);
analogWrite(PWMC,0);
stopC = 0;
last_delay = 1;
}
else if(digitalRead(CSC) == CW)
{
digitalWrite(CSC,CCW);
analogWrite(PWMC,0);
stopC = 0;
last_delay = 1;
}
}
lastC = pwm;
}
//************************************
//Motor Drive Function, -255 ~ 255
//************************************
void motorDrive(int va,int vb,int vc)
{
motor_A(va);
motor_B(vb);
motor_C(vc);
if(last_delay==1)
{
delay(100);
last_delay = 0;
Serial.println("delayed");
}
}
void omniDrive(int x, int y, int rot) //putaran positif searah jarum jam
{
int va,vb,vc;
va = 0.333*x - 0.572*y + 0.333*rot;
vb = 0.333*x + 0.572*y + 0.333*rot;
//vc = -0.667*x + 0.333*rot;
vc = -0.667*x + 0.333*rot;
motorDrive(va,vb,vc);
/*
Serial.print(x,DEC);
Serial.print("\t");
Serial.print(y,DEC);
Serial.print("\t");
Serial.println(rot,DEC);
*/
// Serial.print(va,DEC);
// Serial.print(" ");
// Serial.print(vb,DEC);
// Serial.print(" ");
// Serial.print(vc,DEC);
// Serial.println();
}
You can get the full code. It's easy just subscribe my blog and left the comment. I'll give you the link
.png)
.png)
.png)
.png)
.png)
.png){kind=logo}
