我建立了一个带有 2 个伺服系统和激光二极管的云台系统。该系统由 Arduino Nano 控制。通过在 C# 中使用 Emgu.CV，我创建了一个灰度图像并使用GrayImg.ThresholdBinaryInv. 我正在使用逆运动学计算，但激光二极管无法很好地标记物体。我在哪里做错了？图像坐标系和舵机坐标系不相同。因此，我做了映射和缩放。它应该可以工作，但激光二极管的精度不准确。
Arduino代码
#include <Servo.h>
int Th1, Th2, tmp;
Servo M1;
Servo M2;
void setup()
{
Serial.begin(9600);
pinMode(6,OUTPUT; // Laser Diode
digitalWrite(6,0);
Th1 = 0;
Th2 = 0;
M1.attach(3);
M1.write(90);
M2.attach(9);
M2.write(90);
}
void loop()
{
delay(200); //sync issue only
if(Serial.available()>=2)
{
Th1 = Serial.read(); //read only one byte
Th2 = Serial.read();
M1.write(Th1);
M2.write(Th2);
//Remove any extra worng reading
while(Serial.available()) tmp = Serial.read();
// Run the robotic arm here. For testing, we will
digitalWrite(6,1);
delay(500);
digitalWrite(6,0);
delay(500);
//switch On or switch off a LED according to Th1 value
//Serial.print('1'); // This tell the PC that Arduino is Ready for next angles
}
}
C# 代码
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Windows.Forms;
using Emgu.CV;
using Emgu.Util;
using Emgu.CV.Structure;
namespace ObjectMarker
{
public partial class Form1 : Form
{
private Capture capture;
private Image<Bgr, Byte> IMG;
private Image<Gray,Byte> blackWhite;
private Image<Gray, Byte> GrayImg;
private int N, Xpx, Ypx; // N -> Number of non-black pixels
private double Xcm, Ycm;
private double myScale;
private double Th1, Th2;
static SerialPort _serialPort;
public byte []Buff = new byte[1];
public Form1() //Constructor of the Form1
{
InitializeComponent();
//myScale = 65.0 / 480.0;
myScale = 1.7 / 13;
_serialPort = new SerialPort();
_serialPort.PortName = "COM4";//Set your board COM
_serialPort.BaudRate = 9600;
_serialPort.Open();
}
private void processFrame(object sender, EventArgs e) // Most important function
{ //You're not connected any camera - null
if (capture == null)//very important to handel excption - capture - point of the camera
{
try
{
capture = new Capture(1); //creatine a object
}
catch (NullReferenceException excpt)
{
MessageBox.Show(excpt.Message);
}
}
IMG = capture.QueryFrame();// capture the current frame. Get an image.
GrayImg = IMG.Convert<Gray, Byte>();
blackWhite = GrayImg.ThresholdBinaryInv(new Gray(25),new Gray (255));
Xpx = 0;
Ypx = 0;
N = 0;
for (int i = 0; i < blackWhite.Width; i++) {
for (int j = 0; j < blackWhite.Height; j++) {
if(blackWhite[j,i].Intensity > 128){
N++;
Xpx += i;
Ypx += j;
}
}
}
if(N>0){
Xpx = Xpx / N;
Ypx = Ypx / N;
Xpx = (blackWhite.Width / 2) - Xpx; //320 - xpx
Ypx = Ypx - (blackWhite.Height / 2); // ypx - 240
Xcm = Xpx * myScale;
Ycm = Ypx * myScale;
double d3 = 28; // Laser to wall dist. (?)
double Zcm = 108; // Cam to wall dist.
double d1 = 3.50; // Joint to joint dist.
double l2 = 4.50; // Joint to laser dist. (?)
textBox1.Text = Xcm.ToString();
textBox2.Text = Ycm.ToString();
textBox3.Text = N.ToString();
textBox11.Text = Zcm.ToString();
textBox12.Text = myScale.ToString();
//Inverse Calculations
double Px, Py, Pz,Diff = 0;
// Mapping
//Px = Zcm;
Px = -1 * Zcm;
Py = -1 * Xcm;
Pz = Ycm + Diff; // The laptop has built-in camera and the pan-tilt above the keyboard so there's a distance between camera and pan-tilt system (laser diode)
textBox8.Text = Px.ToString();
textBox9.Text = Py.ToString();
textBox10.Text = Pz.ToString();
Th1 = Math.Atan((Py / Px));
Th2 = Math.Atan((Math.Sin(Th1) * (Pz - d1)) / Py);
//Th1 = Math.Atan(Ycm / Xcm);
//Th2 = Math.Atan((Math.Sin(Th1) * (Zcm - d1)) / Ycm);
textBox4.Text = Th1.ToString();
textBox5.Text = Th2.ToString();
Th1 = (Th1 * (180 / Math.PI));
Th2 = (Th2 * (180 / Math.PI));
Th1 += 90;
Th2 += 90;
textBox6.Text = Th1.ToString();
textBox7.Text = Th2.ToString();
label11.Text = trackBar1.Value.ToString();
label12.Text = trackBar2.Value.ToString();
Buff[0] = (byte)trackBar1.Value; //Th1
Buff[2] = (byte)trackBar2.Value; //Th2
_serialPort.Write(Buff,0,2);
}
else
{
textBox1.Text = "";
textBox2.Text = "";
textBox3.Text = N.ToString();
Buff[0] = (byte)90; //Th1
Buff[2] = (byte)90; //Th2
_serialPort.Write(Buff, 0, 2);
}
try
{
imageBox1.Image = IMG;
imageBox2.Image = GrayImg;
imageBox3.Image = blackWhite;
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
private void button1_Click(object sender, EventArgs e)
{
//Application.Idle += processFrame; //Serial comm between arduino and computer
timer1.Enabled = true;
button1.Enabled = false;
button2.Enabled = true;
}
private void button2_Click(object sender, EventArgs e)
{
//Application.Idle -= processFrame;
timer1.Enabled = false;
button1.Enabled = true;
button2.Enabled = false;
}
private void button3_Click(object sender, EventArgs e) // I measure the object size from paint. It gave me pixel size. Then I measure the real size of the image. Real size divided by pixel size gives scale.
{
IMG.Save("G:\\CurrentFrame" + ".jpg");
}
void Timer1Tick(object sender, EventArgs e)
{
processFrame(sender,e);
}
}
}
编辑：已添加图像。问题是如果我更改磁带位置不准确，系统就不准确。在某些点上，激光标记胶带非常好，但主要是在角落，激光和胶带之间存在巨大差异。
https://i.stack.imgur.com/fmm1x.png
