e-Bot_4x4_Gripperbot_with_PS2_Controller.ino

/*

 e-Gizmo e-BOT 4x4 Gripperbot with PS2 Controller
 Sample Code
 
 UHF EX and STD compatible
 Using PBOT board with ATmega168P MCU
 
 The controller and pin assignments:
 (Summary of PS Controller Functions)
 
 I/O Usage
 - Gizduino >>> UHF EX/STD (RX)
 0   - RX  >>> TX
 1   - TX  >>> RX
 GND - GND >>> GND
 
 - motor control output (default PBOT pin)
 8  - mot2 dir as output high=fwd (right motor)
 9  - mot2 run as output
 11 - mot1 dir as output high= fwd (left motor)
 10 - mot1 run as output
 
 - Servo pin assignment
 12 - Signal Output (For Gripper)
 13 - Signal Output (For Twist)
 
 //**********PS Controller Functions **********
 -Control pad functions (MOTOR)
 UP: Move forward
 RIGHT: Turn Right
 DOWN: Reverse
 LEFT: Turn Left
 
 -Shape buttons (GRIPPER)
 TRIANGLE: grabbing(Gripper)
 CIRCLE: turning right (Twist)
 CROSS: releasing (Gripper)
 SQUARE: turning left (Twist)
 
 - Left and Right Joystick (MOTOR)
 
 - L & R Buttons (RESERVED)
 - Left 1 Debounce
 - Left 2 Debounce
 - Right 1 Latching
 - Right 2 Latching
 //******************************************
 
 March 30,2016
 Modified by Amoree
 Code by e-Gizmo Mechatronix Central
 http://www.e-gizmo.com

 Rewritten by Roma
 
 */
#include <Servo.h> 

byte serial_rxcounter=0;
byte serial_state=0;
boolean serial_available = false;  //true when data packet is available

#define SER_SYNC1 0
#define SER_SYNC2 1
#define SER_FETCHDATA 2

byte  inputData[15];

typedef  struct cont cont;
struct cont{
  boolean  up;
  boolean  down;
  boolean  left;
  boolean  right;
  boolean  triangle;
  boolean  circle;
  boolean  cross;
  boolean  square;
  boolean  left1;
  boolean  left2;
  boolean  right1;
  boolean  right2;
  boolean  start;
  byte  leftx;
  byte  lefty;
  byte  rightx;
  byte  righty;
};

cont  controller;
//Servo
Servo myservo1;//Gripper
Servo myservo2;//Twist
int G_pos = 0;
int T_pos = 0;

//Motor Pin assignments
#define m2dir 8
#define m2run 9
#define m1dir 11
#define m1run 10

int Speed=255;
boolean LEDOFF = false;
boolean L1OFF = false;
boolean L2OFF = false;

int toggle1 = 0;
int toggle2 = 0;

void setup() {
  // Serial 0 is used in this proto. Change if so desired
  Serial.begin(9600);
  myservo1.attach(12);
  myservo2.attach(13);

  pinMode(m2dir, OUTPUT);
  pinMode(m2run, OUTPUT);
  pinMode(m1dir, OUTPUT);
  pinMode(m1run, OUTPUT);
  for(int i=0;i<20;i++){
    pinMode(i,OUTPUT);
  }
  Serial.println("e-Gizmo e-BOT Gripperbot");
  Serial.println("BUTTON : STATUS : ");


}


void loop() {
  if (serial_available==true) {
    serial_available = false;
    serial_state=SER_SYNC1;

    // Put your program here

    // Example program
    // This will display the x,y values of the analog joystick
    // and whatever key is pressed
    ///                             >>>>>  CONTROL STICK <<<

    //Uncomment this if you want to use it
    //RIGHT JOYSTICK
    if(controller.rightx == 0){
      turnleft();
      delay(80);
      motorbrk();
    }
    if(controller.rightx == 255){
      turnright();
      delay(80);
      motorbrk();
    }
    if(controller.righty == 0){
      runfwd();
      delay(80);
      motorbrk();
    }
    if(controller.righty == 255){
      runbwd();
      delay(80);
      motorbrk();
    }
    //LEFT JOYSTICK
     //RIGHT JOYSTICK
    if(controller.leftx == 0){
      turnleft();
      delay(80);
      motorbrk();
    }
    if(controller.leftx == 255){
      turnright();
      delay(80);
      motorbrk();
    }
    if(controller.lefty == 0){
      runfwd();
      delay(80);
      motorbrk();
    }
    if(controller.lefty == 255){
      runbwd();
      delay(80);
      motorbrk();
    }
    //}

    ///                             >>>>>  +CONTROL PAD <<<


    if(controller.up==true) //Move Forward
    { 
      runfwd();
      delay(80);
      motorbrk();
    }
    else if(controller.right==true){  // Turn Right
      turnright();
      delay(80);
      motorbrk();
    } 
    else if(controller.down==true){ //Move Backward
      runbwd();
      delay(80);
      motorbrk();
    } 
    else if(controller.left==true){  // Turn Left
      turnleft();
      delay(80);
      motorbrk();
    } 

    ///                             >>>>>  SHAPE BUTTONS <<<
    //TWIST
    else if(controller.triangle==true){
      T_pos+=10;
      myservo2.write(T_pos);
      delay(1);
    }

    else if(controller.cross==true){
      T_pos-=10;
      myservo2.write(T_pos);
      delay(1);
    } 
    //GRIPPER
    else if(controller.circle==true){
      G_pos+=10;
      myservo1.write(G_pos);
      delay(1);
    } 

    else if(controller.square==true){
      G_pos-=10;
      myservo1.write(G_pos);
      delay(1);
    } 


    //RIGHT/LEFT 1 & 2
    else if(controller.left1==true){
      Serial.println("L1   Reserved:");
      L1OFF = true;
    }
    else if(controller.left2==true){
      Serial.println("L2   Reserved:");
      L1OFF = true;
    }
    else if(controller.right1 == true && toggle1 == 0){
      Serial.println("R1   Reserved:");
      toggle1 = 1;
    }
    else if(controller.right1 == true && toggle1 == 1){
      Serial.println("R1   Reserved:");
      toggle1 = 0;
    }
    else if(controller.right2 == true && toggle2 == 0){
      Serial.println("R2   Reserved:");
      toggle2 = 1;
    }
    else if(controller.right2 == true && toggle2 == 1){
      Serial.println("R2   Reserved:");
      toggle2 = 0;
    }
    //other available buttons
    // if(controller.start==true) Serial.println("SELECT");

    else if(L1OFF == true){
      digitalWrite(14, LOW);
    }
    else if(L2OFF == true){
      digitalWrite(15, LOW);
    }
  }
}


/*
MOTOR CONTROLS
 */

void runfwd(){
  //Forward
  digitalWrite(m2dir, 1); 
  analogWrite(m2run, Speed);
  analogWrite(m1run, Speed);
  digitalWrite(m1dir, 1);
  delay(100);
}
void runbwd(){
  //Reverse
  digitalWrite(m2dir, 0); 
  analogWrite(m2run, Speed);
  analogWrite(m1run, Speed);
  digitalWrite(m1dir, 0);
  delay(100);
}
void turnleft(){
  //Turning Left
  digitalWrite(m2dir, 0); 
  analogWrite(m2run, Speed);
  analogWrite(m1run, Speed);
  digitalWrite(m1dir, 1);
  delay(100);
}
void turnright(){
  //Turning Right
  digitalWrite(m2dir, 1); 
  analogWrite(m2run, Speed);
  analogWrite(m1run, Speed);
  digitalWrite(m1dir, 0);
  delay(100);
}
void motorbrk(){
  //break
  digitalWrite(m2dir, 0); 
  analogWrite(m2run, 0);
  analogWrite(m1run, 0);
  digitalWrite(m1dir, 0);
}


/*****************************************************************/
/*     Do not modify anything past this line
 Unless you are absolutely sure you know
 what you are doing
 */

/*
  SerialEvent occurs whenever a new data comes in the
 hardware serial RX.  This routine is run between each
 time loop() runs, so using delay inside loop can delay
 response.  Multiple bytes of data may be available.
 */


void serialEvent() {
  while (Serial.available()) {

    // get the new byte:
    byte inChar = (byte)Serial.read(); 

    switch(serial_state){
    case SER_SYNC1:
      // Look for first sync byte 0x5A
      serial_rxcounter=0;
      if(inChar==0x5A)serial_state=SER_SYNC2;
      break;

    case SER_SYNC2:
      // Verify if valid packet by checking second byte = 0xA5
      if(inChar==0xA5) 
        serial_state=SER_FETCHDATA;
      else
        serial_state=SER_SYNC1;
      break;

    case  SER_FETCHDATA:
      // Store data packet
      inputData[serial_rxcounter]= inChar;
      serial_rxcounter++;
      // Process when 10 bytes is completed
      if(serial_rxcounter>=10){
        //checksum should match

        if(checksum()==inputData[9]){
          refresh_controller();
          serial_available=true;    // data packet is ready
        }
        else{
          // bad checksum, disregard packet and wait for a new packet
          serial_state=SER_SYNC1;
          serial_available=false;
        }
      }

    }

  }
}

/* Translate raw gamepad controller data
 into something more meaningful for you
 */

void  refresh_controller(void){
  controller.up=false;
  if((inputData[3]&0x08)==0) controller.up=true;
  controller.down=false;
  if((inputData[3]&0x02)==0) controller.down=true;
  controller.left=false;
  if((inputData[3]&0x01)==0) controller.left=true;
  controller.right=false;
  if((inputData[3]&0x04)==0) controller.right=true;
  controller.triangle=false;
  if((inputData[4]&0x08)==0) controller.triangle=true;
  controller.circle=false;
  if((inputData[4]&0x04)==0) controller.circle=true;
  controller.cross=false;
  if((inputData[4]&0x02)==0) controller.cross=true;
  controller.square=false;
  if((inputData[4]&0x01)==0) controller.square=true;
  controller.left1=false;
  if((inputData[4]&0x20)==0) controller.left1=true;
  controller.left2=false;
  if((inputData[4]&0x80)==0) controller.left2=true;
  controller.right1=false;
  if((inputData[4]&0x10)==0) controller.right1=true;
  controller.right2=false;
  if((inputData[4]&0x40)==0) controller.right2=true;
  controller.start=false;
  if((inputData[3]&0x10)==0) controller.start=true;
  controller.leftx=inputData[7];
  controller.lefty=inputData[8];
  controller.rightx=inputData[5];
  controller.righty=inputData[6];
}

// calculate checksum

byte  checksum(void){
  int  i;
  byte chk= 0x5A+0xA5;
  for(i=0;i<9;i++) chk += inputData[i];
  return(chk);

}