Hardware components | ||||||
 |
| × | 1 | |||
| × | 1 | ||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
In this project, I am going show you how to use PHPoC to control Pan-Tilt via hand gesture. Through this way, controlling robot really becomes more fun!
Demo- Stack PHPoC- Grove Expansion board and PHPoC- PWM & Sensor Expansion board to PHPoC Blue
- Connect Gesture Sensor to PHPoC Blue via I2C interface
- Connect two Pan-Tilt motors to PHPoC Blue via HT0 and HT1 connectors
- Connect external source (battery holder) to PHPoC- PWM & Sensor Expansion board
PHPoC and gesture sensor
PHPoC Blue, gesture sensor and Pan-Tilt
With battery holder
The sensor and servo motors can be directly wired to PHPoC Blue, but here I used PHPoC expansion boards for neat-looking wiring.
Data FlowGesture sensor ---> PHPoC ---> Pan-Tilt motors
The data flow is simple: PHPoC continuously reads data from gesture sensor and sends the command to Pan-Tilt motors.
For demonstration in this project, I only retrieve four different hand gestures (right, left, up, down). Note that the sensor allows more gestures to be recognized, so it could be much more fun if you can use other gestures to control robot. Based on detected gestures, PHPoC will send the command to control the servo motors on Pan-Tilt, makes the direction of Pan-Tilt changes accordingly.
Source CodeSource code includes two files:
- gesture_PAJ7620.php: This file contains library for Grove gesture sensor.
- task0.php: This file contains main program for PHPoC.
<?php
include_once "/lib/sd_340.php";
// PHPoC I2C ID
define ("I2C_ID", 0);
// Define command value for exchanging data with PHPoC
define("CMD_LEFT", 1);
define("CMD_RIGHT", 2);
define("CMD_DOWN", 3);
define("CMD_UP", 4);
define ("PAJ7620_ADDR_BASE", 0x00);
define ("PAJ7620_REGITER_BANK_SEL", (PAJ7620_ADDR_BASE + 0xEF));
// Device ID
define ("PAJ7620_ID", 0x73);
// Register bank 0
define ("PAJ7620_ADDR_SUSPEND_CMD", (PAJ7620_ADDR_BASE + 0x3));
define ("PAJ7620_ADDR_GES_PS_DET_MASK_0", (PAJ7620_ADDR_BASE + 0x41));
define ("PAJ7620_ADDR_GES_PS_DET_MASK_1", (PAJ7620_ADDR_BASE + 0x42));
define ("PAJ7620_ADDR_GES_PS_DET_FLAG_0", (PAJ7620_ADDR_BASE + 0x43));
define ("PAJ7620_ADDR_GES_PS_DET_FLAG_1", (PAJ7620_ADDR_BASE + 0x44));
define ("PAJ7620_ADDR_STATE_INDICATOR", (PAJ7620_ADDR_BASE + 0x45));
define ("PAJ7620_ADDR_PS_HIGH_THRESHOLD", (PAJ7620_ADDR_BASE + 0x69));
define ("PAJ7620_ADDR_PS_LOW_THRESHOLD", (PAJ7620_ADDR_BASE + 0x6A));
define ("PAJ7620_ADDR_PS_APPROACH_STATE", (PAJ7620_ADDR_BASE + 0x6B));
define ("PAJ7620_ADDR_PS_RAW_DATA", (PAJ7620_ADDR_BASE + 0x6C));
// Register bank 1
define ("PAJ7620_ADDR_PS_GAIN", (PAJ7620_ADDR_BASE + 0x44));
define ("PAJ7620_ADDR_IDLE_S1_STEP_0", (PAJ7620_ADDR_BASE + 0x67));
define ("PAJ7620_ADDR_IDLE_S1_STEP_1", (PAJ7620_ADDR_BASE + 0x68));
define ("PAJ7620_ADDR_IDLE_S2_STEP_0", (PAJ7620_ADDR_BASE + 0x69));
define ("PAJ7620_ADDR_IDLE_S2_STEP_1", (PAJ7620_ADDR_BASE + 0x6A));
define ("PAJ7620_ADDR_OP_TO_S1_STEP_0", (PAJ7620_ADDR_BASE + 0x6B));
define ("PAJ7620_ADDR_OP_TO_S1_STEP_1", (PAJ7620_ADDR_BASE + 0x6C));
define ("PAJ7620_ADDR_OP_TO_S2_STEP_0", (PAJ7620_ADDR_BASE + 0x6D));
define ("PAJ7620_ADDR_OP_TO_S2_STEP_1", (PAJ7620_ADDR_BASE + 0x6E));
define ("PAJ7620_ADDR_OPERATION_ENABLE", (PAJ7620_ADDR_BASE + 0x72));
define ("PAJ7620_BANK0", 0x00);
define ("PAJ7620_BANK1", 0x01);
define ("PAJ7620_I2C_WAKEUP", 0x01);
define ("PAJ7620_I2C_SUSPEND", 0x00);
define ("PAJ7620_ENABLE", 0x01);
define ("PAJ7620_DISABLE", 0x00);
define ("GES_RIGHT_FLAG", 0x01);
define ("GES_LEFT_FLAG", 0x02);
define ("GES_UP_FLAG", 0x04);
define ("GES_DOWN_FLAG", 0x08);
define ("GES_FORWARD_FLAG", 0x10);
define ("GES_BACKWARD_FLAG", 0x20);
define ("GES_CLOCKWISE_FLAG", 0x40);
define ("GES_COUNT_CLOCKWISE_FLAG", 0x80);
define ("GES_WAVE_FLAG", 0x01);
define ("BANK0", 0);
define ("BANK1", 1);
define ("GES_REACTION_TIME", 500);
define ("GES_ENTRY_TIME", 800);
define ("GES_QUIT_TIME", 1000);
function paj7620_write_reg($addr, $cmd)
{
i2c_write(I2C_ID, ($addr&0xFF)|(($cmd&0xFF)<<8),2);
}
function init_register()
{
paj7620_write_reg(0xEF,0x00);
paj7620_write_reg(0x32,0x29);
paj7620_write_reg(0x33,0x01);
paj7620_write_reg(0x34,0x00);
paj7620_write_reg(0x35,0x01);
paj7620_write_reg(0x36,0x00);
paj7620_write_reg(0x37,0x07);
paj7620_write_reg(0x38,0x17);
paj7620_write_reg(0x39,0x06);
paj7620_write_reg(0x3A,0x12);
paj7620_write_reg(0x3F,0x00);
paj7620_write_reg(0x40,0x02);
paj7620_write_reg(0x41,0xFF);
paj7620_write_reg(0x42,0x01);
paj7620_write_reg(0x46,0x2D);
paj7620_write_reg(0x47,0x0F);
paj7620_write_reg(0x48,0x3C);
paj7620_write_reg(0x49,0x00);
paj7620_write_reg(0x4A,0x1E);
paj7620_write_reg(0x4B,0x00);
paj7620_write_reg(0x4C,0x20);
paj7620_write_reg(0x4D,0x00);
paj7620_write_reg(0x4E,0x1A);
paj7620_write_reg(0x4F,0x14);
paj7620_write_reg(0x50,0x00);
paj7620_write_reg(0x51,0x10);
paj7620_write_reg(0x52,0x00);
paj7620_write_reg(0x5C,0x02);
paj7620_write_reg(0x5D,0x00);
paj7620_write_reg(0x5E,0x10);
paj7620_write_reg(0x5F,0x3F);
paj7620_write_reg(0x60,0x27);
paj7620_write_reg(0x61,0x28);
paj7620_write_reg(0x62,0x00);
paj7620_write_reg(0x63,0x03);
paj7620_write_reg(0x64,0xF7);
paj7620_write_reg(0x65,0x03);
paj7620_write_reg(0x66,0xD9);
paj7620_write_reg(0x67,0x03);
paj7620_write_reg(0x68,0x01);
paj7620_write_reg(0x69,0xC8);
paj7620_write_reg(0x6A,0x40);
paj7620_write_reg(0x6D,0x04);
paj7620_write_reg(0x6E,0x00);
paj7620_write_reg(0x6F,0x00);
paj7620_write_reg(0x70,0x80);
paj7620_write_reg(0x71,0x00);
paj7620_write_reg(0x72,0x00);
paj7620_write_reg(0x73,0x00);
paj7620_write_reg(0x74,0xF0);
paj7620_write_reg(0x75,0x00);
paj7620_write_reg(0x80,0x42);
paj7620_write_reg(0x81,0x44);
paj7620_write_reg(0x82,0x04);
paj7620_write_reg(0x83,0x20);
paj7620_write_reg(0x84,0x20);
paj7620_write_reg(0x85,0x00);
paj7620_write_reg(0x86,0x10);
paj7620_write_reg(0x87,0x00);
paj7620_write_reg(0x88,0x05);
paj7620_write_reg(0x89,0x18);
paj7620_write_reg(0x8A,0x10);
paj7620_write_reg(0x8B,0x01);
paj7620_write_reg(0x8C,0x37);
paj7620_write_reg(0x8D,0x00);
paj7620_write_reg(0x8E,0xF0);
paj7620_write_reg(0x8F,0x81);
paj7620_write_reg(0x90,0x06);
paj7620_write_reg(0x91,0x06);
paj7620_write_reg(0x92,0x1E);
paj7620_write_reg(0x93,0x0D);
paj7620_write_reg(0x94,0x0A);
paj7620_write_reg(0x95,0x0A);
paj7620_write_reg(0x96,0x0C);
paj7620_write_reg(0x97,0x05);
paj7620_write_reg(0x98,0x0A);
paj7620_write_reg(0x99,0x41);
paj7620_write_reg(0x9A,0x14);
paj7620_write_reg(0x9B,0x0A);
paj7620_write_reg(0x9C,0x3F);
paj7620_write_reg(0x9D,0x33);
paj7620_write_reg(0x9E,0xAE);
paj7620_write_reg(0x9F,0xF9);
paj7620_write_reg(0xA0,0x48);
paj7620_write_reg(0xA1,0x13);
paj7620_write_reg(0xA2,0x10);
paj7620_write_reg(0xA3,0x08);
paj7620_write_reg(0xA4,0x30);
paj7620_write_reg(0xA5,0x19);
paj7620_write_reg(0xA6,0x10);
paj7620_write_reg(0xA7,0x08);
paj7620_write_reg(0xA8,0x24);
paj7620_write_reg(0xA9,0x04);
paj7620_write_reg(0xAA,0x1E);
paj7620_write_reg(0xAB,0x1E);
paj7620_write_reg(0xCC,0x19);
paj7620_write_reg(0xCD,0x0B);
paj7620_write_reg(0xCE,0x13);
paj7620_write_reg(0xCF,0x64);
paj7620_write_reg(0xD0,0x21);
paj7620_write_reg(0xD1,0x0F);
paj7620_write_reg(0xD2,0x88);
paj7620_write_reg(0xE0,0x01);
paj7620_write_reg(0xE1,0x04);
paj7620_write_reg(0xE2,0x41);
paj7620_write_reg(0xE3,0xD6);
paj7620_write_reg(0xE4,0x00);
paj7620_write_reg(0xE5,0x0C);
paj7620_write_reg(0xE6,0x0A);
paj7620_write_reg(0xE7,0x00);
paj7620_write_reg(0xE8,0x00);
paj7620_write_reg(0xE9,0x00);
paj7620_write_reg(0xEE,0x07);
paj7620_write_reg(0xEF,0x01);
paj7620_write_reg(0x00,0x1E);
paj7620_write_reg(0x01,0x1E);
paj7620_write_reg(0x02,0x0F);
paj7620_write_reg(0x03,0x10);
paj7620_write_reg(0x04,0x02);
paj7620_write_reg(0x05,0x00);
paj7620_write_reg(0x06,0xB0);
paj7620_write_reg(0x07,0x04);
paj7620_write_reg(0x08,0x0D);
paj7620_write_reg(0x09,0x0E);
paj7620_write_reg(0x0A,0x9C);
paj7620_write_reg(0x0B,0x04);
paj7620_write_reg(0x0C,0x05);
paj7620_write_reg(0x0D,0x0F);
paj7620_write_reg(0x0E,0x02);
paj7620_write_reg(0x0F,0x12);
paj7620_write_reg(0x10,0x02);
paj7620_write_reg(0x11,0x02);
paj7620_write_reg(0x12,0x00);
paj7620_write_reg(0x13,0x01);
paj7620_write_reg(0x14,0x05);
paj7620_write_reg(0x15,0x07);
paj7620_write_reg(0x16,0x05);
paj7620_write_reg(0x17,0x07);
paj7620_write_reg(0x18,0x01);
paj7620_write_reg(0x19,0x04);
paj7620_write_reg(0x1A,0x05);
paj7620_write_reg(0x1B,0x0C);
paj7620_write_reg(0x1C,0x2A);
paj7620_write_reg(0x1D,0x01);
paj7620_write_reg(0x1E,0x00);
paj7620_write_reg(0x21,0x00);
paj7620_write_reg(0x22,0x00);
paj7620_write_reg(0x23,0x00);
paj7620_write_reg(0x25,0x01);
paj7620_write_reg(0x26,0x00);
paj7620_write_reg(0x27,0x39);
paj7620_write_reg(0x28,0x7F);
paj7620_write_reg(0x29,0x08);
paj7620_write_reg(0x30,0x03);
paj7620_write_reg(0x31,0x00);
paj7620_write_reg(0x32,0x1A);
paj7620_write_reg(0x33,0x1A);
paj7620_write_reg(0x34,0x07);
paj7620_write_reg(0x35,0x07);
paj7620_write_reg(0x36,0x01);
paj7620_write_reg(0x37,0xFF);
paj7620_write_reg(0x38,0x36);
paj7620_write_reg(0x39,0x07);
paj7620_write_reg(0x3A,0x00);
paj7620_write_reg(0x3E,0xFF);
paj7620_write_reg(0x3F,0x00);
paj7620_write_reg(0x40,0x77);
paj7620_write_reg(0x41,0x40);
paj7620_write_reg(0x42,0x00);
paj7620_write_reg(0x43,0x30);
paj7620_write_reg(0x44,0xA0);
paj7620_write_reg(0x45,0x5C);
paj7620_write_reg(0x46,0x00);
paj7620_write_reg(0x47,0x00);
paj7620_write_reg(0x48,0x58);
paj7620_write_reg(0x4A,0x1E);
paj7620_write_reg(0x4B,0x1E);
paj7620_write_reg(0x4C,0x00);
paj7620_write_reg(0x4D,0x00);
paj7620_write_reg(0x4E,0xA0);
paj7620_write_reg(0x4F,0x80);
paj7620_write_reg(0x50,0x00);
paj7620_write_reg(0x51,0x00);
paj7620_write_reg(0x52,0x00);
paj7620_write_reg(0x53,0x00);
paj7620_write_reg(0x54,0x00);
paj7620_write_reg(0x57,0x80);
paj7620_write_reg(0x59,0x10);
paj7620_write_reg(0x5A,0x08);
paj7620_write_reg(0x5B,0x94);
paj7620_write_reg(0x5C,0xE8);
paj7620_write_reg(0x5D,0x08);
paj7620_write_reg(0x5E,0x3D);
paj7620_write_reg(0x5F,0x99);
paj7620_write_reg(0x60,0x45);
paj7620_write_reg(0x61,0x40);
paj7620_write_reg(0x63,0x2D);
paj7620_write_reg(0x64,0x02);
paj7620_write_reg(0x65,0x96);
paj7620_write_reg(0x66,0x00);
paj7620_write_reg(0x67,0x97);
paj7620_write_reg(0x68,0x01);
paj7620_write_reg(0x69,0xCD);
paj7620_write_reg(0x6A,0x01);
paj7620_write_reg(0x6B,0xB0);
paj7620_write_reg(0x6C,0x04);
paj7620_write_reg(0x6D,0x2C);
paj7620_write_reg(0x6E,0x01);
paj7620_write_reg(0x6F,0x32);
paj7620_write_reg(0x71,0x00);
paj7620_write_reg(0x72,0x01);
paj7620_write_reg(0x73,0x35);
paj7620_write_reg(0x74,0x00);
paj7620_write_reg(0x75,0x33);
paj7620_write_reg(0x76,0x31);
paj7620_write_reg(0x77,0x01);
paj7620_write_reg(0x7C,0x84);
paj7620_write_reg(0x7D,0x03);
paj7620_write_reg(0x7E,0x01);
}
function paj7620_read_reg($addr, &$data)
{
i2c_write(I2C_ID, $addr&0xFF, 1);
i2c_read(I2C_ID, $data, 1);
return 0;
}
function paj7620_select_bank($bank)
{
switch($bank){
case BANK0:
paj7620_write_reg(PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK0);
break;
case BANK1:
paj7620_write_reg(PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK1);
break;
default:
break;
}
}
function PAJ7620_init()
{
$i = 0;
$data0 = 0;
$data1 = 0;
usleep(700);
//Wait 700us for PAJ7620U2 to be stabilized
i2c_setup(I2C_ID, PAJ7620_ID);
paj7620_select_bank(BANK0);
paj7620_read_reg(0, $data0);
paj7620_read_reg(1, $data1);
if ( ($data0 != 0x20 ) || ($data1 != 0x76) )
{
return 0xff;
}
init_register();
paj7620_select_bank(BANK0);
return 0;
}
function get_gesture()
{
$data = 0;
$data1 = 0;
paj7620_read_reg(0x43, $data);
switch ($data)
{
case GES_RIGHT_FLAG:
usleep(GES_ENTRY_TIME);
paj7620_read_reg(0x43, $data);
if($data == GES_FORWARD_FLAG)
{
echo "Forward \r\n";
usleep(GES_QUIT_TIME);
}
else if($data == GES_BACKWARD_FLAG)
{
echo "Backward \r\n";
usleep(GES_QUIT_TIME);
}
else
{
echo "Right \r\n";
return CMD_RIGHT;
}
break;
case GES_LEFT_FLAG:
usleep(GES_ENTRY_TIME);
paj7620_read_reg(0x43, $data);
if($data == GES_FORWARD_FLAG)
{
echo "Forward \r\n";
usleep(GES_QUIT_TIME);
}
else if($data == GES_BACKWARD_FLAG)
{
echo "Backward \r\n";
usleep(GES_QUIT_TIME);
}
else
{
echo "Left \r\n";
return CMD_LEFT;
}
break;
case GES_UP_FLAG:
usleep(GES_ENTRY_TIME);
paj7620_read_reg(0x43, $data);
if($data == GES_FORWARD_FLAG)
{
echo "Forward \r\n";
usleep(GES_QUIT_TIME);
}
else if($data == GES_BACKWARD_FLAG)
{
echo "Backward \r\n";
usleep(GES_QUIT_TIME);
}
else
{
echo "Up \r\n";
return CMD_UP;
}
break;
case GES_DOWN_FLAG:
usleep(GES_ENTRY_TIME);
paj7620_read_reg(0x43, $data);
if($data == GES_FORWARD_FLAG)
{
echo "Forward \r\n";
usleep(GES_QUIT_TIME);
}
else if($data == GES_BACKWARD_FLAG)
{
echo "Backward \r\n";
usleep(GES_QUIT_TIME);
}
else
{
echo "Down \r\n";
return CMD_DOWN;
}
break;
default:
paj7620_read_reg(0x43, $data1);
if ($data1 == GES_WAVE_FLAG)
{
echo "Wave \r\n";
}
break;
}
return 0;
}
?>
<?php
include "gesture_PAJ7620.php";
define("PWM_PERIOD", 20000);
define("WIDTH_MIN", 771);
define("WIDTH_MAX", 2193);
function servo_set_angle($id, $angle)
{
$width = WIDTH_MIN + (int)round((WIDTH_MAX - WIDTH_MIN) * $angle / 180.0);
if(($width >= WIDTH_MIN) && ($width <= WIDTH_MAX))
ht_pwm_width($id, $width, PWM_PERIOD);
}
// Angles of two motor (degree)
$angles = array(0,0);
$angle_max = array(180, 90);
$angle_min = array( 0, 0);
$pwm = array(0, 1); // HT0 control lef_right motor, HT1 control up_down motor
$degree_step = 90;
i2c_scan(0);
PAJ7620_init();
ht_pwm_setup($pwm[0], 0, PWM_PERIOD, "us");
ht_pwm_setup($pwm[1], 0, PWM_PERIOD, "us");
servo_set_angle($pwm[0], 90);
servo_set_angle($pwm[1], 0);
while(1)
{
sleep(1);
$cmd = get_gesture();
if($cmd==0)
continue;
switch ($cmd)
{
case CMD_LEFT:
$angles[0] += $degree_step;
break;
case CMD_RIGHT:
$angles[0] -= $degree_step;
break;
case CMD_DOWN:
$angles[1] -= $degree_step;
break;
case CMD_UP:
$angles[1] += $degree_step;
break;
default:
break;
}
for($i = 0; $i <2; $i++)
{
if($angles[$i] > $angle_max[$i])
$angles[$i] = $angle_max[$i];
else if($angles[$i] < $angle_min[$i])
$angles[$i] = $angle_min[$i];
servo_set_angle($pwm[$i], $angles[$i]);
}
}
?>
Comments