Create a gist now

Instantly share code, notes, and snippets.

/*
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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment