Controlling a Robot using PSX (Arduino based)

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

Read More

Line Follower Robot using Arduino

I made this code for line following robot with 4 light sensors (the sensors are very powerful!!!).
Tag: Arduino, Uno, Leonardo, Line Following Robot, Programming


I'm Indonesian, so some initializations are in Indonesian
Kiri=Turn Left
Kanan=Turn Right

You can download Arduino software at www.arduino.cc
Here's the code:

Read More