Buto is my ‘real steel’ robot project. Now, BuTO has been connected to the Internet and can be controlled remotely.
Things used in this project
Hardware components |
||||||
 |
|
× | 1 | |||
 |
|
× | 1 | |||
|
× | 2 | ||||
|
× | 2 | ||||
|
× | 2 | ||||
|
× | 4 | ||||
|
× | 4 | ||||
|
× | 4 | ||||
|
× | 1 | ||||
|
× | 1 | ||||
|
× | 1 | ||||
 |
|
× | 1 | |||
|
× | 1 | ||||
 |
|
× | 24 | |||
 |
|
× | 1 | |||
Software apps and online services |
||||||
 |
|
|||||
 |
|
|||||
 |
|
|||||
Hand tools and fabrication machines |
||||||
 |
|
|||||
Story
This robot is called BuTO. This is my ‘real steel’ robot project. Buto has 4 DOF robot arm. Each DOF is controlled by a servo Dynamixel. Buto is also equipped with a ‘hand’ gripper. BuTO’s arm movement is accordance to the user’s arm movement. I use Kinect to capture the user’s body movements.
I also use 4 omni wheels (for BuTO’s acceleration) that is controlled using a Attach3 logitech joystick. Currently, to control the BuTO’s movement wirelesly, I use a wifi connection between 2 PCs. The first computer’s used as a server, and the second computer’s used as a client to receive user data.
With MKR1000, I can control the servos and omni wheels movement via the Internet network. If you are connected to the Internet, we can use BuTO to lead a meeting, become a robot receptionist, or explore a dangerous space remotely.
Story:
1. Robot BuTO 2.0 (previous versions)
– BuTO v 2.0 requires two PCs, one at the user side and the other at the robot side.
2. After getting MKR1000, I replace the PC mounted on the robot using genuino MKR1000. The first step, I tried to control dynamixel servo movement using MRK1000 wirelessly. (video: Indonesia Language)
3. After the dynamixel servo motor was successfully controlled using MKR1000, I connected it with gestures recognition application (using VB.net), and it worked!
4. Unfortunately, my MKR1000 is damaged. So, I replaced MKR1000 with PC as before. The PC is connected to the Internet network using public IP. The PC server reads joystick and Kinect data and sends it to the PC client. Then the PC client process this data and sends it to the servo Dynamixel via usb2dynamixel and arduino UNO via serial communication. Arduino Uno is used to control the motor speed and direction. (I add a streaming video using skype :D)
The following image is a block diagram of BuTO robot system:
And, this is BuTO’s photo collection:
overall appearance
electronic circuit
BuTO in action (indoor)
BuTO in action 2 (outdoor)
Custom parts and enclosures
Arm and hand robot design
Arm and hand robot design (using AutoCAD 2002).
Schematics
Arm and Hand Robot Design
Arm and hand robot design. For arm, i use 4 dynamixel servos. For hand, I use 2 Hitec servo. Robot ‘finger’ is made of balsa wood and arm frame made of stainless steel. I draw it using AutoCad 2002.
BuTO Robot Schematics
The circuit diagram of BuTO robot using Fritzing.
BuTO Robot Schematics (screenshoot)
The schematics diagram of BuTO robot (screenshoot, .JPG). I modify the arduino nano library as MKR1000 board illustration.
BuTO Robot Schematics (screenshoot)
The schematics diagram of BuTO robot (screenshoot .JPG). I modify the arduino nano library as MKR1000 board illustration.
Code
Uno for BuTO Robot
Arduino
#include <LiquidCrystal.h>
#include <Wire.h>
#include <Servo.h>
LiquidCrystal lcd(8, 9, 16, 17, 18, 19);
Servo sServo1; // create servo object to control a servo
Servo sServo2; // create servo object to control a servo
String dataTerima;
String servo1, servo2, servo3, servo4;
String dataI2C;
int speed12 = 180;
int speed34 = 270;
int led = 13;
String dataTwi = "O128P128Q0R0S";
String genggam = "0";
int dirKiri = 4;
int pwmKiri = 5;
int pwmKanan = 6;
int dirKanan = 7;
int dirDepan = 12;
int pwmDepan = 11;
int pwmBelakang = 3;
int dirBelakang = 2;
int k,l,m,n,o,p,q,r,s;
void jariMenggenggam(){
sServo1.write(125);
sServo2.write(95);
}
void jariMembuka(){
sServo1.write(155);
sServo2.write(75);
}
void rodaDepan(int x){
x = x - 128;
if (x<0) {
digitalWrite(dirDepan, LOW);
if (x<=-128) {
analogWrite(pwmDepan, 255);
}
else {
analogWrite(pwmDepan, (x*-2));
}
}
else if (x>0) {
digitalWrite(dirDepan, HIGH);
if (x>=128) {
analogWrite(pwmDepan, 0);
}
else {
analogWrite(pwmDepan, 255-(x*2));
}
}
else {
digitalWrite(dirDepan, LOW);
analogWrite(pwmDepan, 0);
}
}
void rodaBelakang(int x){
x = x - 128;
if (x<0) {
digitalWrite(dirBelakang, LOW);
if (x<=-128) {
analogWrite(pwmBelakang, 255);
}
else {
analogWrite(pwmBelakang, (x*-2));
}
}
else if (x>0) {
digitalWrite(dirBelakang, HIGH);
if (x>=128) {
analogWrite(pwmBelakang, 0);
}
else {
analogWrite(pwmBelakang, 255 - (x*2));
}
}
else {
digitalWrite(dirBelakang, LOW);
analogWrite(pwmBelakang, 0);
}
}
void rodaKiri(int x){
x = x - 128;
if (x<0) {
digitalWrite(dirKiri, LOW);
if (x<=-128) {
analogWrite(pwmKiri, 255);
}
else {
analogWrite(pwmKiri, (x*-2));
}
}
else if (x>0) {
digitalWrite(dirKiri, HIGH);
if (x>=128) {
analogWrite(pwmKiri, 0);
}
else {
analogWrite(pwmKiri, 255 - (x*2));
}
}
else {
digitalWrite(dirKiri, LOW);
analogWrite(pwmKiri, 0);
}
}
void rodaKanan(int x){
x = x - 128;
if (x<0) {
digitalWrite(dirKanan, LOW);
if (x<=-128) {
analogWrite(pwmKanan, 255);
}
else {
analogWrite(pwmKanan, (x*-2));
}
}
else if (x>0) {
digitalWrite(dirKanan, HIGH);
if (x>=128) {
analogWrite(pwmKanan, 0);
}
else {
analogWrite(pwmKanan, 255-(x*2));
}
} else {
digitalWrite(dirKanan, LOW);
analogWrite(pwmKanan, 0);
}
}
void setup(){
Serial.begin(57600);
//serialku.begin(19200);
lcd.begin(16,2);
lcd.print("BuTO Robot");
pinMode(dirKiri, OUTPUT);
pinMode(dirKanan, OUTPUT);
pinMode(dirDepan, OUTPUT);
pinMode(dirBelakang, OUTPUT);
pinMode(pwmKiri, OUTPUT);
pinMode(pwmKanan, OUTPUT);
pinMode(pwmDepan, OUTPUT);
pinMode(pwmBelakang, OUTPUT);
pinMode(led, OUTPUT);
sServo1.attach(14);
sServo2.attach(15);
jariMembuka();
Wire.begin();
}
void loop(){
if (Serial.available() > 0) {
dataTerima = Serial.readStringUntil('\n');
digitalWrite(led, HIGH);
o = dataTerima.indexOf("O");
p = dataTerima.indexOf("P");
q = dataTerima.indexOf("Q");
r = dataTerima.indexOf("R");
s = dataTerima.indexOf("S");
String mKiriKanan = dataTerima.substring(o+1,p);
String mDepanBelakang = dataTerima.substring(p+1,q);
String CW_CCW = dataTerima.substring(q+1,r);
if (CW_CCW == "1") { //CCW
rodaDepan(160);
rodaBelakang(160);
rodaKiri(160);
rodaKanan(160);
} else if (CW_CCW == "2") { //CW
rodaDepan(96);
rodaBelakang(96);
rodaKiri(96);
rodaKanan(96);
}
else {
rodaDepan(256 - mDepanBelakang.toInt());
rodaBelakang(mDepanBelakang.toInt());
rodaKiri(256 - mKiriKanan.toInt());
rodaKanan(mKiriKanan.toInt());
}
genggam = dataTerima.substring(r+1, s);
if (genggam == "0") {
jariMembuka();
} else {
jariMenggenggam();
}
lcd.setCursor(0,1);
lcd.print(mKiriKanan.toInt());
lcd.print("-");
lcd.print(mDepanBelakang.toInt());
lcd.print("-");
lcd.print(CW_CCW);
lcd.print("-");
lcd.print(genggam);
lcd.print("-");
digitalWrite(led, LOW);
}
}
This code is used to access logitech attack3 joystick.
Imports System.Runtime.InteropServices
Imports System
Imports System.Threading
Imports System.IO.Ports
Imports System.ComponentModel
Imports System.Math
Imports System.Timers
Imports System.Net
Imports System.Net.Sockets
Imports System.Text
Public Class Form2
Dim analogx As Integer
Dim analogy As Integer
Dim persenkecepatan As Integer
Dim tombol As Integer
Declare Function joyGetPosEx Lib "winmm.dll" (ByVal uJoyID As Integer, ByRef pji As JOYINFOEX) As Integer
<StructLayout(LayoutKind.Sequential)> _
Public Structure JOYINFOEX
Public dwSize As Integer
Public dwFlags As Integer
Public dwXpos As Integer
Public dwYpos As Integer
Public dwZpos As Integer
Public dwRpos As Integer
Public dwUpos As Integer
Public dwVpos As Integer
Public dwButtons As Integer
Public dwButtonNumber As Integer
Public dwPOV As Integer
Public dwReserved1 As Integer
Public dwReserved2 As Integer
End Structure
Dim myjoyEX As JOYINFOEX
Private Sub Form2_Load(sender As System.Object, e As System.EventArgs) Handles MyBase.Load
myjoyEX.dwSize = 64
myjoyEX.dwFlags = &HFF ' All information
'Timer1.Interval = 200 'Update at 5 hz
End Sub
Private Sub initializeSender()
sendingClient = New UdpClient(broadCastAddress, port)
sendingClient.EnableBroadcast = True
End Sub
Private Sub initializeReceiver()
receivingClient = New UdpClient(port)
If (String.IsNullOrEmpty(port)) Then
'messagebox.Show("port is not available")
End If
Dim start As ThreadStart = New ThreadStart(AddressOf receiver)
receivingThread = New Thread(start)
receivingThread.IsBackground = True
receivingThread.Start()
End Sub
Private Sub receiver()
Dim endpoint As IPEndPoint = New IPEndPoint(IPAddress.Any, port)
Dim messageDelegate As addMessage = AddressOf MessageReceived
While (True)
Dim Data() As Byte
Data = receivingClient.Receive(endpoint)
Dim Message As String = Encoding.ASCII.GetString(Data)
Invoke(messageDelegate, Message)
End While
End Sub
Private Sub MessageReceived(ByRef message As String)
'rtbChart.Text += message + vbNewLine
End Sub
Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick
End Sub
Private Sub Button1_Click(sender As System.Object, e As System.EventArgs) Handles Button1.Click
'SerialPort1.Open()
'Button1.Enabled = False
Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes("testing")
sendingClient.Send(datasiapkirim, datasiapkirim.Length)
End Sub
Private Sub Button2_Click(sender As System.Object, e As System.EventArgs) Handles Button2.Click
Timer2.Enabled = True
Button2.Enabled = False
End Sub
Private Sub Timer2_Tick(sender As System.Object, e As System.EventArgs) Handles Timer2.Tick
' Get the joystick information
Call joyGetPosEx(0, myjoyEX)
ListBox1.Items.Clear()
With myjoyEX
analogx = 255 / 65535 * .dwXpos
analogy = 255 - (255 / 65535 * .dwYpos)
If analogx >= 123 And analogx <= 133 Then analogx = 128
If analogy >= 123 And analogy <= 133 Then analogy = 128
persenkecepatan = (65535 - .dwZpos) / 65535 * 128
tombol = .dwButtons
ListBox1.Items.Add(.dwXpos.ToString & "-" & analogx) 'Up to six axis supported
ListBox1.Items.Add(.dwYpos.ToString & "-" & analogy)
ListBox1.Items.Add(.dwZpos.ToString & "-" & persenkecepatan)
ListBox1.Items.Add(.dwButtons.ToString) '("X")) 'Print in Hex, so can see the individual bits associated with the buttons
'ListBox1.Items.Add(.dwButtonNumber.ToString) 'number of buttons pressed at the same time
'ListBox1.Items.Add((.dwPOV / 100).ToString) 'POV hat (in 1/100ths of degrees, so divided by 100 to give degrees)
End With
Dim itombol As Integer = 255
Dim genggam As Integer = 0
Dim cwccw As Integer = 0
If tombol = 1 Then
genggam = 1
ElseIf tombol = 8 Then
cwccw = 1
ElseIf tombol = 16 Then
cwccw = 2
ElseIf tombol = 9 Then
cwccw = 1
genggam = 1
ElseIf tombol = 17 Then
cwccw = 2
genggam = 1
End If
TextBox1.Text = "O" & analogy & "P" & analogx & "Q" & cwccw & "R" & genggam & "S"
Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(TextBox1.Text)
sendingClient.Send(datasiapkirim, datasiapkirim.Length)
End Sub
End Class
This code is used to read Kinect data, process it, and then send it to client PC.
Imports System
Imports Microsoft.Kinect
Imports System.Threading
Imports System.IO.Ports
Imports System.ComponentModel
Imports System.Math
Imports System.Drawing
Imports System.Windows.Forms
Imports System.Timers
Imports System.Net
Imports System.Net.Sockets
Imports System.Text
Public Class Form1
'Inherits Form
Delegate Sub addMessage(ByRef message As String)
Private username As String = "BuTO_Server"
Const port As Integer = 2390
Dim broadCastAddress As String
Private receivingClient As UdpClient
Private sendingClient As UdpClient
Private receivingThread As Thread
'konstanta servo
'-------------------------------------------------------------------------------------------
Public TAmbilData_Delegate, TSudutA_Delegate, TSudutB_Delegate, TSudutD_Delegate As Threading.TimerCallback
Public TAmbilData, TSudutA, TSudutB, TSudutD As System.Threading.Timer
Dim nilai, nilai2, nilai3 As Integer
Dim keep1, keep2, keep3 As Integer
Public a1, a2, a3, a4 As Single
Dim konter_serial As Integer = 0
Dim lastgrip_open, lastgrip_close As Integer
'Dim WithEvents aTimer As New System.Threading.Timer(AddressOf ticktock, Nothing, 0, 500)
'Dim WithEvents bTimer As New System.Threading.Timer(AddressOf ticktock2, Nothing, 0, 1000)
Dim bahutengah_x, bahutengah_y, bahutengah_z As Integer
Dim bahutengah_dx, bahutengah_dy, bahutengah_dz As Single
Dim bahukanan_x, bahukanan_y, bahukanan_z As Integer
Dim bahukanan_dx, bahukanan_dy, bahukanan_dz As Single
Dim sikukanan_x, sikukanan_y, sikukanan_z As Integer
Dim sikukanan_dx, sikukanan_dy, sikukanan_dz As Single
Dim pergelangankanan_x, pergelangankanan_y, pergelangankanan_z As Integer
Dim tangankanan_x, tangankanan_y, tangankanan_z As Integer
Dim pergelangankanan_dx, pergelangankanan_dy, pergelangankanan_dz As Single
Dim nservo1 As Integer = 2048
Dim nservo2 As Integer = 2048
Dim nservo3 As Integer = 512
Dim nservo4 As Integer = 512
Dim skalaplus, skalaplusb, skalaplusd As Integer
Dim suduta1 As Integer = 0
Dim suduta2 As Integer = 0
Dim suduta3 As Integer = 0
Dim lastservo1 As Integer = 2048
Dim lastservo2 As Integer = 2048
Dim lastservo3 As Integer = 512
Dim lastservo4 As Integer = 512
Dim lservo1, lservo2, lservo3, lservo4 As Integer
'Complemntary Filter
Dim klowpass, klowpass2 As String
Dim khighpass, khighpass2 As String
Dim dout1, dout2, dout3, dout4 As Integer
'Konstanta Kinect
'-----------------------------------------------------------------------------------
Public TKinect_Delegate As Threading.TimerCallback
Public TKinect As System.Threading.Timer
'untuk kinect
Dim kinz As KinectSensor
Dim imagez As ColorImageFrame
Dim skeletonz As SkeletonFrame
Dim piccolor As Bitmap = New Bitmap(640, 480, Imaging.PixelFormat.Format32bppRgb)
Dim depthz As DepthImageFrame
Dim picdepth As Bitmap = New Bitmap(640, 480, Imaging.PixelFormat.Format32bppRgb)
Dim picdepth_procc As Bitmap = New Bitmap(100, 100, Imaging.PixelFormat.Format32bppRgb)
Dim picdepth_procc2 As Bitmap = New Bitmap(100, 100, Imaging.PixelFormat.Format32bppRgb)
Dim gfx As Graphics = Graphics.FromImage(piccolor)
Dim gfxd As Graphics = Graphics.FromImage(picdepth)
Dim pixmax As Integer = 35000
Dim last_servo As Integer
'tampilan grafik
Dim bmp As Bitmap
Dim big_G As Graphics
Dim pen As New Pen(Color.Red)
'operasi sudut
Dim konter As Integer = 0
Dim x1, y1, x2, y2, x3, y3, x4, y4 As Integer
'Dim a1, a2, a3 As Single
Dim smooth As TransformSmoothParameters = New TransformSmoothParameters
Private Sub Form1_Load(sender As System.Object, e As System.EventArgs) Handles MyBase.Load
'----------------------------------------------------------
Dim potentialsensor As Microsoft.Kinect.KinectSensor
broadCastAddress = TxtAddres.Text
smooth.Correction = 0.7F
smooth.JitterRadius = 0.5F
smooth.MaxDeviationRadius = 0.1F
smooth.Prediction = 0.1F
smooth.Smoothing = 0.5F
For Each potentialsensor In KinectSensor.KinectSensors
If potentialsensor.Status = KinectStatus.Connected Then
Me.kinz = potentialsensor
Exit For
End If
Next potentialsensor
'Me.kinz.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30)
'If KinectStatus.Connected = True Then
'End If
'Timerdelay.Enabled = True
klowpass = ScrBar_KLowPass.Value / 100
khighpass = 1 - klowpass
ScrBar_KHighPass.Value = khighpass * 100
klowpass2 = klowpass / 2.5
khighpass2 = 1 - klowpass2
TxtKLowPass.Text = klowpass
TxtKHighPass.Text = khighpass
initializeSender()
initializeReceiver()
'Form2.Show()
End Sub
Private Sub initializeSender()
sendingClient = New UdpClient(broadCastAddress, port)
sendingClient.EnableBroadcast = True
End Sub
Private Sub initializeReceiver()
receivingClient = New UdpClient(port)
If (String.IsNullOrEmpty(port)) Then
MessageBox.Show("port is not available")
End If
Dim start As ThreadStart = New ThreadStart(AddressOf receiver)
receivingThread = New Thread(start)
receivingThread.IsBackground = True
receivingThread.Start()
End Sub
Private Sub receiver()
Dim endpoint As IPEndPoint = New IPEndPoint(IPAddress.Any, port)
Dim messageDelegate As addMessage = AddressOf MessageReceived
While (True)
Dim Data() As Byte
Data = receivingClient.Receive(endpoint)
Dim Message As String = Encoding.ASCII.GetString(Data)
Invoke(messageDelegate, Message)
End While
End Sub
Private Sub MessageReceived(ByRef message As String)
'rtbChart.Text += message + vbNewLine
End Sub
Private Sub Form1_Leave(sender As System.Object, e As System.EventArgs) Handles MyBase.Leave
Me.kinz.Stop()
End Sub
'Complementary Filter
Private Sub ScrBar_KLowPass_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles ScrBar_KLowPass.Scroll
klowpass = ScrBar_KLowPass.Value / 100
khighpass = 1 - klowpass
ScrBar_KHighPass.Value = khighpass * 100
klowpass2 = klowpass / 2.5
khighpass2 = 1 - klowpass2
TxtKLowPass.Text = klowpass
TxtKHighPass.Text = khighpass
End Sub
'Sub servo
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Private Sub Button1_Click(sender As System.Object, e As System.EventArgs) Handles BtnBukaPort.Click
If dxl_initialize(3, 1) = 1 Then
'Set goal speed
dxl_write_word(1, P_GOAL_SPEED_L, 180) '270
dxl_write_word(2, P_GOAL_SPEED_L, 180) '270
dxl_write_word(3, P_GOAL_SPEED_L, 270) '270
dxl_write_word(4, P_GOAL_SPEED_L, 270) '270
'Set goal position
dxl_write_word(1, P_GOAL_POSITION_L, 2100)
dxl_write_word(2, P_GOAL_POSITION_L, 2150)
dxl_write_word(3, P_GOAL_POSITION_L, 512)
dxl_write_word(4, P_GOAL_POSITION_L, 512)
dxl_write_word(5, P_GOAL_POSITION_L, 512)
Else
MsgBox("cek koneksi")
End If
End Sub
Private Sub BtnDataSent_Click(sender As System.Object, e As System.EventArgs) Handles BtnDataSent.Click
'TSudutA = New System.Threading.Timer(TSudutA_Delegate, Nothing, 0, 100)
'TSudutB = New System.Threading.Timer(TSudutB_Delegate, Nothing, 0, 100)
'TSudutD = New System.Threading.Timer(TSudutD_Delegate, Nothing, 0, 100)
TimerKirimData.Enabled = True
BtnDataSent.Enabled = False
End Sub
Private Sub VScrollBar6_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles VScrollBar6.Scroll
TextBox6.Text = VScrollBar6.Value
End Sub
Private Sub VScrollBar5_Scroll(sender As System.Object, e As System.Windows.Forms.ScrollEventArgs) Handles VScrollBar5.Scroll
TextBox5.Text = VScrollBar5.Value
End Sub
'Sub Kinect
'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Private Sub colorready(ByVal sender As Object, ByVal e As ColorImageFrameReadyEventArgs)
imagez = e.OpenColorImageFrame
End Sub
Private Sub skeletonready(ByVal sender As Object, ByVal e As SkeletonFrameReadyEventArgs)
skeletonz = e.OpenSkeletonFrame
End Sub
Private Sub depthready(ByVal sender As Object, ByVal e As DepthImageFrameReadyEventArgs)
depthz = e.OpenDepthImageFrame
End Sub
'Skeleton display
Public Sub skeletonmethod()
'Dim G As Graphics
Dim idindex As Integer
Dim rad As Single = 3.14 / 180
Dim antirad As Single = 180 / 3.14
' Dim nilaiy As Integer
'Dim nilaix As Integer
Dim status1, status2, status3, status4, status5, status6 As String
Dim skeletons(-1) As Skeleton
If skeletonz IsNot Nothing Then
skeletons = New Skeleton(skeletonz.SkeletonArrayLength - 1) {}
skeletonz.CopySkeletonDataTo(skeletons)
End If
Dim penz As Pen = New Pen(Brushes.Gold, 5)
Dim penz1 As Pen = New Pen(Brushes.Blue, 3)
Dim penz2 As Pen = New Pen(Brushes.Red, 3)
Dim myfont As Font = New System.Drawing.Font("Verdana", 20)
Dim mybrust As Brush = New System.Drawing.SolidBrush(Color.Red)
Dim mybrust1 As Brush = New System.Drawing.SolidBrush(Color.Green)
gfx.Clear(Color.Black)
If skeletons.Length <> 0 Then
'If kinz.SkeletonStream.AppChoosesSkeletons = False Then kinz.SkeletonStream.AppChoosesSkeletons = True
'kinz.SkeletonStream.ChooseSkeletons(txttrackingID.Text)
status1 = "tidak aktif"
status2 = "tidak aktif"
status3 = "tidak aktif"
status4 = "tidak aktif"
status5 = "tidak aktif"
status6 = "tidak aktif"
If RadioButton1.Checked = True Then
idindex = 1
ElseIf RadioButton2.Checked = True Then
idindex = 2
ElseIf RadioButton3.Checked = True Then
idindex = 3
ElseIf RadioButton4.Checked = True Then
idindex = 4
ElseIf RadioButton5.Checked = True Then
idindex = 5
ElseIf RadioButton6.Checked = True Then
idindex = 6
End If
For Each skel As Skeleton In skeletons
konter = konter + 1
If konter = 1 And skel.Position.Z <> 0 Then
status1 = "Aktif"
End If
If konter = 2 And skel.Position.Z <> 0 Then
status2 = "Aktif"
End If
If konter = 3 And skel.Position.Z <> 0 Then
status3 = "Aktif"
End If
If konter = 4 And skel.Position.Z <> 0 Then
status4 = "Aktif"
End If
If konter = 5 And skel.Position.Z <> 0 Then
status5 = "Aktif"
End If
If konter = 6 And skel.Position.Z <> 0 Then
status6 = "Aktif"
End If
'ListBox1.Items.Add(konter & " - " & skel.Position.Z)
LblStatusTracking.Text = "ID1=" & status1 & "|ID2=" & status2 & "|ID3=" & status3 & "|ID4=" & status4 & "|ID5=" & status5 & "|ID6=" & status6
'Right Arm
Dim shoulderright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderRight).Position, DepthImageFormat.Resolution640x480Fps30)
Dim elbowright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ElbowRight).Position, DepthImageFormat.Resolution640x480Fps30)
Dim wristright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.WristRight).Position, DepthImageFormat.Resolution640x480Fps30)
Dim handright As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HandRight).Position, DepthImageFormat.Resolution640x480Fps30)
gfx.DrawLine(penz, New Point(shoulderright.X, shoulderright.Y), New Point(elbowright.X, elbowright.Y))
gfx.DrawLine(penz, New Point(elbowright.X, elbowright.Y), New Point(wristright.X, wristright.Y))
gfx.DrawLine(penz, New Point(wristright.X, wristright.Y), New Point(handright.X, handright.Y))
'gfx.DrawRectangle(penz1, New Rectangle(handright.X - 15, handright.Y - 15, 30, 30))
'Left Arm
Dim shoulderleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderLeft).Position, DepthImageFormat.Resolution640x480Fps30)
Dim elbowleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ElbowLeft).Position, DepthImageFormat.Resolution640x480Fps30)
Dim wristleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.WristLeft).Position, DepthImageFormat.Resolution640x480Fps30)
Dim handleft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HandLeft).Position, DepthImageFormat.Resolution640x480Fps30)
gfx.DrawLine(penz, New Point(shoulderleft.X, shoulderleft.Y), New Point(elbowleft.X, elbowleft.Y))
gfx.DrawLine(penz, New Point(elbowleft.X, elbowleft.Y), New Point(wristleft.X, wristleft.Y))
gfx.DrawLine(penz, New Point(wristleft.X, wristleft.Y), New Point(handleft.X, handleft.Y))
'gfx.DrawRectangle(penz1, New Rectangle(handleft.X - 15, handleft.Y - 15, 30, 30))
'Right Leg
'Dim FootRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.FootRight).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim AnkleRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.AnkleRight).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim KneeRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.KneeRight).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim HipRight As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipRight).Position, DepthImageFormat.Resolution640x480Fps30)
'gfx.DrawLine(penz, New Point(HipRight.X, HipRight.Y), New Point(KneeRight.X, KneeRight.Y))
'gfx.DrawLine(penz, New Point(KneeRight.X, KneeRight.Y), New Point(AnkleRight.X, AnkleRight.Y))
'gfx.DrawLine(penz, New Point(AnkleRight.X, AnkleRight.Y), New Point(FootRight.X, FootRight.Y))
'Left Leg
'Dim FootLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.FootLeft).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim AnkleLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.AnkleLeft).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim KneeLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.KneeLeft).Position, DepthImageFormat.Resolution640x480Fps30)
'Dim HipLeft As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipLeft).Position, DepthImageFormat.Resolution640x480Fps30)
'gfx.DrawLine(penz, New Point(HipLeft.X, HipLeft.Y), New Point(KneeLeft.X, KneeLeft.Y))
'gfx.DrawLine(penz, New Point(KneeLeft.X, KneeLeft.Y), New Point(AnkleLeft.X, AnkleLeft.Y))
'gfx.DrawLine(penz, New Point(AnkleLeft.X, AnkleLeft.Y), New Point(FootLeft.X, FootLeft.Y))
'Body
Dim head As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.Head).Position, DepthImageFormat.Resolution640x480Fps30)
Dim shouldercenter As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.ShoulderCenter).Position, DepthImageFormat.Resolution640x480Fps30)
Dim Spine As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.Spine).Position, DepthImageFormat.Resolution640x480Fps30)
Dim HipCenter As DepthImagePoint = Me.kinz.CoordinateMapper.MapSkeletonPointToDepthPoint(skel.Joints(JointType.HipCenter).Position, DepthImageFormat.Resolution640x480Fps30)
gfx.DrawLine(penz, New Point(head.X, head.Y), New Point(shouldercenter.X, shouldercenter.Y))
gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(shoulderright.X, shoulderright.Y))
gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(shoulderleft.X, shoulderleft.Y))
gfx.DrawLine(penz, New Point(shouldercenter.X, shouldercenter.Y), New Point(Spine.X, Spine.Y))
gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(Spine.X, Spine.Y))
'gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(HipRight.X, HipRight.Y))
'gfx.DrawLine(penz, New Point(HipCenter.X, HipCenter.Y), New Point(HipLeft.X, HipLeft.Y))
'gfx.DrawRectangle(penz1, New Rectangle(head.X - 15, head.Y - 15, 30, 30))
'gfx.DrawRectangle(penz1, New Rectangle(shouldercenter.X - 15, shouldercenter.Y - 15, 30, 30))
'tangankanan = handright.X & "," & handright.Y & "," & handleft.Depth
'tangankiri = handleft.X & "," & handleft.Y
'kepala = head.X & "," & head.Y
'gfx.DrawString(handright.Depth, myfont, mybrust, handright.X, handright.Y)
'gfx.DrawString(tangankiri, myfont, mybrust, handleft.X, handleft.Y)
Dim deep_shouldercenter As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, shouldercenter)
Dim deep_shoulderright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, shoulderright)
Dim deep_elbowright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, elbowright)
Dim deep_wristright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, wristright)
Dim deep_handright As SkeletonPoint = Me.kinz.CoordinateMapper.MapDepthPointToSkeletonPoint(DepthImageFormat.Resolution640x480Fps30, handright)
gfx.DrawLine(penz2, New Point(wristright.X, wristright.Y), New Point(shouldercenter.X, shouldercenter.Y))
If skel.Position.Z <> 0 Then
gfx.DrawString(konter, myfont, mybrust, head.X + 10, head.Y)
gfx.DrawString((shoulderright.Depth - shoulderleft.Depth), myfont, mybrust, shoulderright.X + 10, shoulderright.Y)
End If
If konter = idindex Then
bahutengah_x = shouldercenter.X
bahutengah_y = shouldercenter.Y
bahutengah_z = shouldercenter.Depth
bahutengah_dx = deep_shouldercenter.X
bahutengah_dy = deep_shouldercenter.Y
bahutengah_dz = deep_shouldercenter.Z
bahukanan_x = shoulderright.X
bahukanan_y = shoulderright.Y
bahukanan_z = shoulderright.Depth
bahukanan_dx = deep_shoulderright.X * 100
bahukanan_dy = deep_shoulderright.Y * 100
bahukanan_dz = deep_shoulderright.Z * 100
sikukanan_x = elbowright.X
sikukanan_y = elbowright.Y
sikukanan_z = elbowright.Depth
sikukanan_dx = deep_elbowright.X * 100
sikukanan_dy = deep_elbowright.Y * 100
sikukanan_dz = deep_elbowright.Z * 100
pergelangankanan_x = wristright.X
pergelangankanan_y = wristright.Y
pergelangankanan_z = wristright.Depth
pergelangankanan_dx = deep_wristright.X * 100
pergelangankanan_dy = deep_wristright.Y * 100
pergelangankanan_dz = deep_wristright.Z * 100
tangankanan_x = handright.X
tangankanan_y = handright.Y
olah_sudut_xyz()
End If
If konter = 6 Then konter = 0
Next skel
End If
End Sub
Private Sub olah_sudut_xyz()
Txtpundak_x.Text = bahukanan_dx
TxtPundak_y.Text = bahukanan_dy
txtpundak_z.Text = bahukanan_dz
TxtSiku_x.Text = sikukanan_dx
TxtSiku_y.Text = sikukanan_dy
Txtsiku_z.Text = sikukanan_dz
TxtGelang_x.Text = pergelangankanan_dx
TxtGelang_y.Text = pergelangankanan_dy
TxtGelang_z.Text = pergelangankanan_dz
'-----------------------------------
'a4 = a4 * (180 / 3.14)
Dim sudut1, sudut2, sudut3, sudut4 As Single
'Dim status_persamaan As String = "persamaan ..."
' LblStatus_Persamaan.Text = status_persamaan
'----------------------------------------------------------
'rumus baru
Dim X_LB As Single = pergelangankanan_dx - sikukanan_dx
Dim Y_LB As Single = sikukanan_dy - pergelangankanan_dy
Dim Z_LB As Single = sikukanan_dz - pergelangankanan_dz
Dim X_LA As Single = sikukanan_dx - (bahukanan_dx + 7)
Dim Y_LA As Single = bahukanan_dy - sikukanan_dy
Dim Z_LA As Single = bahukanan_dz - sikukanan_dz
Txt_XLA.Text = X_LA
Txt_YLA.Text = Y_LA
Txt_ZLA.Text = Z_LA
Txt_XLB.Text = X_LB
Txt_YLB.Text = Y_LB
Txt_ZLB.Text = Z_LB
Dim XY_LA As Single = Sqrt(X_LA ^ 2 + Y_LA ^ 2)
Dim ZY_LA As Single = Sqrt(Z_LA ^ 2 + Y_LA ^ 2)
Dim A1N As Single = Atan(Z_LA / Y_LA) * 180 / PI
Dim A2N As Single = Atan(X_LA / Y_LA) * 180 / PI
Dim A1 As Single = Atan(Z_LA / XY_LA) * 180 / PI
Dim A2 As Single = Atan(X_LA / ZY_LA) * 180 / PI
Dim AI2 As Single = Atan(X_LB / Y_LB) * 180 / PI
Dim LB As Single = Sqrt(X_LB ^ 2 + Y_LB ^ 2 + Z_LB ^ 2)
Dim LA As Single = Sqrt(X_LA ^ 2 + Y_LA ^ 2 + Z_LA ^ 2)
Dim X_LI1 As Single = X_LA - X_LB
Dim Y_LI1 As Single = Y_LB - Y_LA
Dim Z_LI1 As Single = Z_LB - Z_LA
Dim LI1 As Single = Sqrt(X_LI1 ^ 2 + Y_LI1 ^ 2 + Z_LI1 ^ 2)
Dim cosA4 As Single = (LA ^ 2 + LB ^ 2 - LI1 ^ 2) / (2 * LA * LB)
Dim A4 As Single = Acos(cosA4) * 180 / PI
Dim A3 As Single
If A4 <= 25 Or (AI2 > A2N - 4 And AI2 < A2N + 4) Then
A3 = 0
Else
Dim LI3 As Single = Sin(A4 * PI / 180) * LB
Dim LI4 As Single = Cos((A1 + A4) * PI / 180) * LB
Dim Z_LI2 As Single = Sin((A1 + A4) * PI / 180) * LB
Dim X_LI2 As Single = Sin(A2N * PI / 180) * LI4
Dim Y_LI2 As Single = Cos(A2N * PI / 180) * LI4
'
Dim X_LI5 As Single = X_LI2 - X_LB
Dim Y_LI5 As Single = Y_LI2 - Y_LB
Dim Z_LI5 As Single = Z_LI2 - Z_LB
'
Dim LI5 As Single = Sqrt(X_LI5 ^ 2 + Y_LI5 ^ 2 + Z_LI5 ^ 2)
Dim cosA3 As Single = (LI3 ^ 2 + LI3 ^ 2 - LI5 ^ 2) / (2 * LI3 * LI3)
A3 = Acos(cosA3) * 180 / PI
If (AI2 <= A2N - 4 And Y_LB >= 0) Or (AI2 >= A2N + 4 And Y_LB < 0) Then
A3 *= -1
End If
'If A3 = last_a3 Then
' A3 *= -1
'End If
End If
'
'...............
'dari sini
'Dim X_LBu As Single = X_LB
'Dim Y_LBu As Single = Y_LB
'If X_LBu < 0 Then X_LBu *= -1
'If Y_LBu < 0 Then Y_LBu *= -1
'Dim A3in As Single = Atan(X_LB / Y_LB) * 180 / PI
'Dim A3 As Single
'
'If A3in > (A2N - 16) - 3 And A3in < (A2N - 16) + 3 Then
' A3 = 0
' Else
'If A3in > 0 Then
'A3 = A3in - (A2N - 16)
'Else
'A3 = A3in + 16
'End If
'End If
'sampai sini
'....................
If A1N < 0 Then
If Z_LA < 0 Then A1N = 0
If Y_LA < 0 Then A1N = 90
End If
If A2 < 0 Then A2 = 0
If A3 < -45 Then A3 = -45
If A4 < 0 Then A4 = 0
If A1N > 90 Then A1N = 90
If A2 > 90 Then A2 = 90
If A3 > 45 Then A3 = 45
If A4 > 90 Then A4 = 90
'last_a3 = A3 * -1
sudut1 = A1N
sudut2 = A2
sudut3 = A3
sudut4 = A4
'---------------------------------------------------------
If Single.IsNaN(sudut1) = True Then
sudut1 = 0
Else
sudut1 = Round(sudut1)
End If
If Single.IsNaN(sudut2) = True Then
sudut2 = 0
Else
sudut2 = Round(sudut2)
End If
If Single.IsNaN(sudut3) = True Then
sudut3 = 0
Else
sudut3 = Round(sudut3)
End If
If Single.IsNaN(sudut4) = True Then
sudut4 = 0
Else
sudut4 = Round(sudut4)
End If
'-----------------------------------
lservo1 = (1024 / 90 * sudut1) + 2048
lservo2 = (1024 / 90 * sudut2) + 2048
lservo3 = (153 / 45 * sudut3) + 512
If lservo3 > 665 Then lservo3 = 665
If lservo3 < 359 Then lservo3 = 359
lservo4 = (308 / 90 * sudut4) + 512
'lastservo1 = dxl_read_word(1, 36)
'lastservo2 = dxl_read_word(2, 36)
'If lservo3 < 0 Then lservo3 = 0
'If lservo3 > 1023 Then lservo3 = 1023
If lservo4 < 512 Then lservo4 = 512
If lservo4 > 820 Then lservo4 = 820
txtsudut_a1.Text = sudut1
txtsudut_a2.Text = sudut2
Txtsudut_a3.Text = sudut3
txtsudut_a4.Text = sudut4
TextBox5.Text = lservo1
VScrollBar5.Value = lservo1
TextBox6.Text = lservo2
VScrollBar6.Value = lservo2
TextBox7.Text = lservo3
VScrollBar7.Value = lservo3
TextBox8.Text = lservo4
VScrollBar8.Value = lservo4
End Sub
Private Sub TimerKinect_Tick(sender As System.Object, e As System.EventArgs) Handles TimerKinect.Tick
skeletonmethod()
PictB_Kinect.Image = piccolor
End Sub
Private Sub TimerKirimData_Tick(sender As System.Object, e As System.EventArgs) Handles TimerKirimData.Tick
lastservo1 = VScrollBar1.Value
dout1 = (khighpass * lastservo1) + (klowpass * lservo1)
If dout1 < 2048 Then dout1 = 2048
If dout1 > 3072 Then dout1 = 3072
nservo1 = dout1
TextBox1.Text = nservo1
VScrollBar1.Value = nservo1
'dxl_write_word(1, P_GOAL_POSITION_L, nservo1)
'-------------------------------------------------------------
lastservo2 = VScrollBar2.Value
dout2 = (khighpass * lastservo2) + (klowpass * lservo2)
If dout2 < 2048 Then dout2 = 2048
If dout2 > 3072 Then dout2 = 3072
nservo2 = dout2
TextBox2.Text = Str(nservo2) & "|" & Str(skalaplusb)
VScrollBar2.Value = nservo2
'dxl_write_word(2, P_GOAL_POSITION_L, nservo2)
'-----------------------------------------------------------
lastservo3 = VScrollBar3.Value
dout3 = (khighpass2 * lastservo3) + (klowpass2 * lservo3)
If dout3 < 250 Then dout3 = 250
If dout3 > 820 Then dout3 = 820
nservo3 = dout3
TextBox3.Text = Str(nservo3) '& "|" & Str(skalaplusb)
VScrollBar3.Value = nservo3
'dxl_write_word(3, P_GOAL_POSITION_L, nservo3)
'----------------------------------------
lastservo4 = VScrollBar4.Value
dout4 = (khighpass2 * lastservo4) + (klowpass2 * lservo4)
If dout4 < 512 Then dout4 = 512
If dout4 > 820 Then dout4 = 820
nservo4 = dout4
TextBox4.Text = Str(nservo4) '& "|" & Str(skalaplusb)
VScrollBar4.Value = nservo4
'dxl_write_word(4, P_GOAL_POSITION_L, nservo4)
Dim datakirimlan As String = "K" & nservo1 & "L" & nservo2 & "M" & nservo3 & "N" & nservo4 & "O" '& Form2.TextBox1.Text & "P"
Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(datakirimlan)
sendingClient.Send(datasiapkirim, datasiapkirim.Length)
End Sub
Private Sub Button1_Click_1(sender As System.Object, e As System.EventArgs) Handles Button1.Click
'Me.kinz.SkeletonStream.Enable()
Me.kinz.SkeletonStream.Enable(smooth)
Me.kinz.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30)
'AddHandler Me.kinz.ColorFrameReady, AddressOf colorready
AddHandler Me.kinz.DepthFrameReady, AddressOf depthready
AddHandler Me.kinz.SkeletonFrameReady, AddressOf skeletonready
Me.kinz.Start()
Me.kinz.SkeletonStream.TrackingMode = SkeletonTrackingMode.Seated
Me.kinz.ElevationAngle = 0
TimerKinect.Enabled = True
'aktifkan ini
'-----------
End Sub
Private Sub Button2_Click(sender As System.Object, e As System.EventArgs) Handles Button2.Click
TimerKinect.Enabled = False
TimerKirimData.Enabled = False
'TimerDepth.Enabled = False
Me.kinz.Stop()
End Sub
Private Sub Timer1_Tick(sender As System.Object, e As System.EventArgs) Handles Timer1.Tick
Dim datasiapkirim() As Byte = Encoding.ASCII.GetBytes(Form2.TextBox1.Text)
sendingClient.Send(datasiapkirim, datasiapkirim.Length)
End Sub
Private Sub Button5_Click(sender As System.Object, e As System.EventArgs) Handles Button5.Click
Form2.Show()
End Sub
Private Sub Button4_Click(sender As System.Object, e As System.EventArgs) Handles Button4.Click
Timer1.Enabled = True
End Sub
End Class
This code is used to control the speed and direction of omni wheel motors. Uno receives data from the client PC via serial communication. The results of data processing is also displayed on the LCD.
*data from PC: “OaaaaPbbbbQcRdS”
aaaa: front & rear motor data
bbbb: left & right motor data
c: CW, CCW, or free drive
d: hand grip condition