Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
/*
e-Gizmo e-BOT 4x4 EGRA with PS2 Controller
(with 2ch tiny motor driver)
UHF EX and STD compatible
Using PBOT board with ATMGE644P
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
3 - Signal Output (Servo1 - For Gripper)
5 - Signal Output (Servo2)
6 - Signal Output (Servo3)
7 - Signal Output (Servo 4 - For Base)
//**********PS Controller Functions **********
- Control pad
- Up = Move Forward
- Right = Turn Right
- Down = Move Backward
- Left = Turn Left
- Left Joystick 0,c128,255
- LEFT Y = Servo 3 controls
- LEFT X = Servo 4 - for base controls
- Right Joystick 0,c128,255
- RIGHT Y = Servo 2 controls
- RIGHT X = Servo 1 - for gripper controls
- Shape buttons
- Triangle = Move Forward
- Circle = Turn Right
- Cross = Move Forward
- Square = Turn Left
- 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
*/
#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;
Servo myservo3;
Servo myservo4; //Base
int G_pos = 0;
int S2_pos = 0;
int S3_pos = 0;
int B_pos = 0;
int val0;
int val1;
int val2;
int val3;
int valx0;
int valx1;
int valy0;
int valy1;
//Motor Pin assignments
#define m2dir 8
#define m2run 9
#define m1dir 11
#define m1run 10
void setup() {
// Serial 0 is used in this proto. Change if so desired
Serial.begin(9600);
myservo1.attach(3);
myservo2.attach(5);
myservo3.attach(6);
myservo4.attach(7);
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 eBOT (with E-GRA)");
}
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 <<<
//RIGHT JOYSTICK
//Gripper
/* valx0 = controller.rightx;
val0 = map(valx0, 128, 255, 90, 160);
val0++;
myservo1.write(val0); //D3
//Servo2
valy0 = controller.righty;
val1 = map(valy0, 0, 255, 160, 90);
val1++;
myservo2.write(val1); //D5
//LEFT JOYSTICK
//Servo3
valy1 = controller.lefty;
val2 = map(valy1, 0, 255, 160,0);
val2++;
myservo3.write(val2); //D6
//Servo3
valx1 = controller.leftx;
val3 = map(valx1, 0, 255, 180,0);
val3++;
myservo4.write(val3); //D7*/
/// >>>>> +CONTROL PAD <<<
if(controller.up==true) //Move Forward
{
runfwd();
delay(15);
motorbrk();
STOP();
}
else if(controller.right==true){ // Turn Right
turnright();
delay(15);
motorbrk();
STOP();
}
else if(controller.down==true){ //Reverse
runbwd();
delay(15);
motorbrk();
STOP();
}
else if(controller.left==true){ // Turn Left
turnleft();
delay(15);
motorbrk();
STOP();
}
/// >>>>> SHAPE BUTTONS <<<
else if(controller.triangle==true && S2_pos<180){ //up
//add codes here
S2_pos+=10;
myservo2.write(S2_pos);
delay(1);
}
else if(controller.circle==true && G_pos<180){ //grabbing
//add codes here
G_pos+=10;
myservo1.write(G_pos);
delay(1);
}
else if(controller.cross==true && S2_pos>0){//down
//add codes here
S2_pos-=10;
myservo2.write(S2_pos);
delay(1);
}
else if(controller.square==true && G_pos>0){//releasing
//add codes here
G_pos-=10;
myservo1.write(G_pos);
delay(1);
}
//RIGHT/LEFT 1 & 2
if(controller.left1==true && S3_pos<180){ //servo motor 3 up
//add codes here
S3_pos+=10;
myservo3.write(S3_pos);
delay(1);
}
else if(controller.left2==true && B_pos<180){ // turn left
//add codes here
B_pos+=10;
myservo4.write(B_pos);
delay(1);
}
else if(controller.right1 == true && S3_pos>0){//servo motor 3down
//add codes here
S3_pos-=10;
myservo3.write(S3_pos);
delay(1);
}
else if(controller.right2 == true && B_pos>0){//turn right
//add codes here
B_pos-=10;
myservo4.write(B_pos);
delay(1);
}
//other available buttons
// if(controller.start==true) Serial.println("SELECT");
}
}
/*
MOTOR CONTROLS
*/
void runfwd(){
//Forward
digitalWrite(m2dir, 0);
digitalWrite(m2run, 1);
digitalWrite(m1run, 0);
digitalWrite(m1dir, 1);
delay(100);
}
void runbwd(){
//Reverse
digitalWrite(m2dir, 1);
digitalWrite(m2run, 0);
digitalWrite(m1run, 1);
digitalWrite(m1dir, 0);
delay(100);
}
void turnleft(){
//Turning Left
digitalWrite(m2dir,1);
digitalWrite(m2run, 0);
digitalWrite(m1run, 0);
digitalWrite(m1dir, 1);
delay(100);
}
void turnright(){
//Turning Right
digitalWrite(m2dir, 0);
digitalWrite(m2run, 1);
digitalWrite(m1run, 1);
digitalWrite(m1dir, 0);
delay(100);
}
void STOP()
{
//disable motor
digitalWrite(m2dir,0);
digitalWrite(m2run, 0);
digitalWrite(m1run, 0);
digitalWrite(m1dir, 0);
}
void motorbrk(){
//break
digitalWrite(m2dir,0);
digitalWrite(m2run, 0);
digitalWrite(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);
}
Owner
e-Gizmo commented Dec 2, 2016 edited

Revision 2 --- 12-02-16
Rewritten by : Roma

  • Control pad functions (MOTOR)
    UP: Move forward
    RIGHT: Turn Right
    DOWN: Reverse
    LEFT: Turn Left
  • Shape buttons (EGRA)
    TRIANGLE: up (Servo2)
    CIRCLE: grabbing (Gripper)
    CROSS: down (Servo2)
    SQUARE: releasing (Gripper)
  • Right/Left 1 & 2 (EGRA)
    LEFT1: up (Servo3)
    LEFT2: turn left (Base)
    RIGHT1: down (Servo3)
    RIGHT2: turn right (Base)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment