We Keep Coding

Does anyone have a simple code I can reference for serial communication from Pi to Arduino?

If I make a loop on my Raspberry Pi from 1 to 10 and assigned to a variable x for a small example, how do I take it and transfer it to an Arduino via Serial to be able to be used for an angle for my stepper motor or to simply make it usable as a variable in a loop?
Is there a small code from a Pi and Arduino each that can help me? I know this is like an easy thing, but I'm trying to find a reference because I'm expanding upon this, using Node Red, Stepper Motors, Water Valves, and a ton of other things.

Are you talking about general serial communication? I have something that will work on both ends. It is not simple
Here is what you should run on the Pi.
Change Baud rate to proper rate for your device
Change "Possible_Parameters" to a list of possible angles to run
import time
import serial
import numpy as np
import serial.tools.list_ports
from serial.tools.list_ports import comports
import sys
import glob
import serial
def serial_ports():
""" Lists serial port names
:raises EnvironmentError:
On unsupported or unknown platforms
:returns:
A list of the serial ports available on the system
"""
if sys.platform.startswith('win'):
ports = ['COM%s' % (i + 1) for i in range(256)]
elif sys.platform.startswith('linux') or
sys.platform.startswith('cygwin'):
# this excludes your current terminal "/dev/tty"
ports = glob.glob('/dev/tty[A-Za-z]*')
elif sys.platform.startswith('darwin'):
ports = glob.glob('/dev/tty.*')
else:
raise EnvironmentError('Unsupported platform')
result = []
for port in ports:
try:
s = serial.Serial(port)
s.close()
result.append(port)
except (OSError, serial.SerialException):
pass
return result
for i in range(len(serial_ports())):
print(i, serial_ports()[i])
if len(serial_ports()) > 0:
Port_Selected = int(input("Select Port like 0: "))
Port = serial_ports()[Port_Selected]
Baud = 9600
X=1
else:
print("No ports detected")
X=0
pass
if X==1:
with serial.Serial(Port, Baud, timeout=.1) as Qtpy:
if Qtpy.isOpen():
print('{} connected'.format(Qtpy.port))
try:
while True:
if X==1:
Possible_Parameters=["List", "Keys", "Here"]
for i in range(len(Possible_Parameters)):
print(i, Possible_Parameters[i])
Possible_Parameter_Choice = int(input("Choose Parameter to change :"))
Msg1 = Possible_Parameters[Possible_Parameter_Choice]
Msg_1=Msg1 + '\n' #add ending parameter for C++ serial communication
Msg2 = (input("Input a new value for parameter: "))
Msg_2=Msg2 + '\n'
#print (Msg_B)
Qtpy.write(Msg_1.encode())
Qtpy.write(Msg_2.encode())
X=0
while Qtpy.inWaiting() == 0:
pass
if Qtpy.inWaiting() > 0:
Msg_Back = Qtpy.readline()
print (Msg_Back)
#Qtpy.flushInput()
#X = input("Set X to 1")
#time.sleep(0.02)
except KeyboardInterrupt:
print('Keyboard interrupted')
Here is something for your arduino. Notice that I am using pairs. I do this to make one a key for the value. The first item received identifies where the value will go. Note: I did cannibalize my code for the arduino part so you will need to double check it for errors
// Prep Serial Communication Variables - 7 variables
const uint8_t Max_Length = 32;
const uint8_t Max_Value_Length = 16;
char MSG_In[Max_Length]; //Parameter name (Send in pairs with value
char MSG_In_Value[Max_Value_Length]; //Parameter value
char MSG_Out[Max_Length];
char Bits_N_Pieces; // bytes recieved
bool Incoming; // Gets set to 1 when serial communication is recieved
char Key[] = Your_Keyword;
// Convert you angles to integers before sending
int Value;
// Or use this and change atoi in later statement
// Library to convert float to char
/* #include <avr/dtostrf.h>
float Value; */
void setup() {
Serial.begin(9600);
}
void loop() {
// Serial Communication
while (Serial.available() > 0) {
Incoming=1;
static unsigned int MSG_Position = 0;
// read the incoming byte:
Bits_N_Pieces = Serial.read();
//Serial.println(Bits_N_Pieces);
if (Bits_N_Pieces != '\n' && MSG_Position < Max_Length -1) {
MSG_In[MSG_Position] = Bits_N_Pieces;
// Serial.println(MSG_In);
MSG_Position++;
} else {
MSG_In[MSG_Position] = '\0';
MSG_Position = 0;
break;
}
}
if (Incoming==1) {
Incoming=0;
while (Serial.available() > 0) {
// Serial.println("Reading Value");
static unsigned int MSG_Position = 0;
Bits_N_Pieces = Serial.read();
if(Bits_N_Pieces != '\n' && MSG_Position < Max_Value_Length -1) {
MSG_In_Value[MSG_Position] = Bits_N_Pieces;
//Serial.println(MSG_In_Value);
MSG_Position++;
} else {
MSG_In_Value[MSG_Position] = '\0';
MSG_Position =0;
break;
}
}
if (strcmp(MSG_In, Key) == 0) {
Value = atoi(MSG_In_Value);
/* Value = atof(MSG_In_Value);*/
}
//Zero out MSG_XXX to prep for next communication
bzero(MSG_In, Max_Length);
bzero(MSG_In_Value, Max_Value_Length);
}
delay(1000);
}

'std::out_of_range' error in sending an image with sockets

I am using this code in Python to send an image with sockets (and OpenCV) to another computer:
#!/usr/bin/env python3
import socket
import cv2
serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serversocket.bind(("XXX.XXX.X.XXX", 9999))
serversocket.listen(5)
print ('server started and listening')
while 1:
(clientsocket, address) = serversocket.accept()
print ("connection found!")
img = cv2.imread('testImage.jpeg')
if (img.any()):
clientsocket.sendall(img)
It works pretty well, but at some point it eventually shuts down, showing this error message:
terminate called after throwing an instance of 'std::out_of_range'
what(): basic_string::substr: __pos (which is 140) > this->size() (which is 0)
Aborted
I have searched in forums but I can't find an answer to my problem. Anyone can help me?
EDIT:
My socket client, that receives the image, is written in C#, and the code is the following:
private void connectSocket()
{
receiveSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
IPEndPoint hostIpEndPoint = new IPEndPoint(IPAddress.Parse("XXX.XXX.X.XXX"), 9999);
receiveSocket.Connect(hostIpEndPoint);
}
private void btnImagem_Click(object sender, EventArgs e)
{
if (stream==0)
{
stream = 1;
}
else
{
stream = 0;
}
while (stream == 1)
{
connectSocket();
wait(300);
int dataSize;
int y, x, t = 0;
Bitmap bmp2 = new Bitmap(160, 120);
dataSize = 0;
byte[] b = new byte[receiveSocket.ReceiveBufferSize];
dataSize = receiveSocket.Receive(b);
if (dataSize > 0)
{
for (y = 0; y < 120; y++)
{
for (x = 0; x < 160; x++)
{
bmp2.SetPixel(x, y, Color.FromArgb(Convert.ToInt32(b[t + 2]), Convert.ToInt32(b[t + 1]), Convert.ToInt32(b[t])));
t = t + 3;
}
}
}
videoBox.Image = bmp2;
receiveSocket.Close();
wait(10);
}
videoBox.Image = null;
}
public void wait(int milliseconds)
{
System.Windows.Forms.Timer timer1 = new System.Windows.Forms.Timer();
if (milliseconds == 0 || milliseconds < 0) return;
timer1.Interval = milliseconds;
timer1.Enabled = true;
timer1.Start();
timer1.Tick += (s, e) =>
{
timer1.Enabled = false;
timer1.Stop();
};
while (timer1.Enabled)
{
Application.DoEvents();
}
}

Streaming camera from Python to C# over sockets?

I need to stream video from a Raspberry Pi's camera (Python server), to a C# script client (Unity, in fact).
I have successfully done this, using a Python to Python workflow, but I can't manage to make It work on C# client side.
This is my python server, where I'm sending the Stream over sockets:
Server.py:
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(('0.0.0.0', 24000))
s.listen(1)
connection = s.accept()[0].makefile('wb')
print ('Client connected')
while True:
stream = io.BytesIO()
for foo in camera.capture_continuous(stream, 'jpeg'):
connection.write(struct.pack('<L', stream.tell()))
connection.flush()
stream.seek(0)
connection.write(stream.read())
stream.seek(0)
stream.truncate()
And this is my Python client side, which is actually working:
Client.py:
def threadLoop(self):
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((self.ip, self.port))
connection = self.socket.makefile('rb')
print('Recibiendo fotos')
while not self.haltSignal():
image_len, image_width, image_height = struct.unpack('<LLL', connection.read(struct.calcsize('<LLL')))
print ("L: " + str(image_len) + " W: " + str(image_width) + " H: " + str(image_height))
if not image_len:
break
image_stream = io.BytesIO()
image_stream.write(connection.read(image_len))
image_stream.seek(0)
self.callback(image_stream.getvalue(), image_width, image_height)
self.socket.close()
self.socket = None
self.thread = None
So, the question is... having this Server.py, how can I make a C# client that receives the stream and show the pictures?
I've tried to capture the received data in C#, over the socket, store It in a Byte array and convert It to a readable image, which sometimes, when It's a (very)small sized picture, works fine. I guess there must be a better way to achieve this.
Thanks!
Edit: This is what I've been trying in C#. Thanks again!
public int Connect()
{
if (_clientSocket.Connected == false)
{
_clientSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
mutexToSend = new Mutex();
mutexReceived = new Mutex();
ThreadStart ts = new ThreadStart(SendReceiveData);
threadSend = new Thread(ts);
threadSend.Start();
return 1;
}
else
return 0; //There is a socket already connected.
}
//----------------------------------------------------------
private void SendReceiveData()
{
try
{
_clientSocket.SendTimeout = 1000;
_clientSocket.ReceiveTimeout = 1000;
System.Net.IPAddress ipaddress = this.serverIP;
_clientSocket.Connect(new IPEndPoint(ipaddress, this.serverPort));
}
catch (SocketException ex)
{
//turn ex.Message;
}
_clientSocket.BeginReceive(_recieveBuffer, 0, _recieveBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveCallback), null);
}
//----------------------------------------------------------
private void ReceiveCallback(IAsyncResult AR)
{
//Check how much bytes are recieved and call EndRecieve to finalize handshake
int recieved = _clientSocket.EndReceive(AR);
if (recieved <= 0)
{
GestorCamaraSala.imgReceived = _recieveBuffer; //I copy the received data to the image texture
return;
}
//Start receiving again
_clientSocket.BeginReceive(_recieveBuffer, 0, _recieveBuffer.Length, SocketFlags.None, new AsyncCallback(ReceiveCallback), null);
}// end ReceivedCallback

Reducing Lag between Arduino and Python

I have 3 Arduino-sensor nodes connected to a PC running Python, with XBee Series 1 radios as tools for wireless communications. The baud rate is set at 9600, and all address (ATDL, ATDH, ATMY) are set correctly as all the Arduino-sensor nodes are able to send data correctly to the XBee coordinator connected to my PC running on Python. The Xbee radios connected to the respective Arduinos (there are 2 Arduino Unos and 1 Arduino Nano) are configured as End Devices.
I recently found a problem in that any changes effected at the Arduino will lag by 5 seconds when it reaches the PC, and written to a CSV file. For example, I am reading the state of a pin on one of the Arduinos, and writing this state to a CSV file after it has been transmitted via XBee to my computer. However, I realized that when I effected the state change at 08:30:30 (HH:MM:SS), the change was only reflected in the CSV file at 08：30：35 (HH:MM:SS).
May I ask why is this the case and how should I resolve it? I have the following codes for Arduino and Python respectively.
Arduino (these codes are roughly the same across the 3 Arduino nodes):
#include <SoftwareSerial.h>
#define IR 10 // IR sensor at D10 position
#define pirPin 9 // Input for HC-S501
#define LEDPinPIR 12 // LED at Pin 12 (PIR)
#define lightLED 11 // LED at Pin 11 (Relay, Neg.Logic - ON = Relay off)
SoftwareSerial xbee(2, 3); // RX, TX
int pirValue; // Place to store read PIR Value
int pirNum = 0;
int pirNumyes = 0;
int pirNumno = 0;
int sw_door = 0; //sw_door has been updated to "sw_relay" w.e.f 27-Feb-2018
int IR_val = 0;
char incomingByte;
unsigned long prevMillis = 0;
void setup() {
Serial.begin(9600);
xbee.begin(9600);
pinMode(pirPin, INPUT); // PIR sensor
pinMode(LEDPinPIR, OUTPUT); // Ultrasound sensor indicator
pinMode(lightLED, OUTPUT); // LED at Pin 11 (Relay, Neg.Logic - ON = Relay off)
pinMode(SW, INPUT); // Switch
digitalWrite(SW, LOW);
digitalWrite(LEDPinPIR, LOW);
}
void loop() {
unsigned long currentMillis = millis();
if((unsigned long)currentMillis - prevMillis == 1000){
//IR sensor "d" refers to door
if (digitalRead(IR) == LOW){
IR_val = 1;
String ID = "d";
String IRID = ID + IR_val;
Serial.print(IRID);
Serial.print(',');
xbee.print(IRID);
xbee.print(',');
}
else{
IR_val = 0;
String ID = "d";
String IRID = ID + IR_val;
Serial.print(IRID);
Serial.print(',');
xbee.print(IRID);
xbee.print(',');
}
// Motion sensor
pirValue = digitalRead(pirPin);
if (pirValue == HIGH) {
pirNumyes = 1;
Serial.print(pirNumyes);
Serial.print(',');
xbee.print(pirNumyes);
xbee.print(',');
digitalWrite(LEDPinPIR, HIGH);
}
else {
pirNumno = 0;
Serial.print(pirNumno);
Serial.print(',');
xbee.print(pirNumno);
xbee.print(',');
digitalWrite(LEDPinPIR, LOW);
}
// Switch
if(digitalRead(lightLED)== HIGH){
sw_door = 0;
Serial.print(sw_door);
Serial.println(',');
xbee.print(sw_door);
xbee.println(',');
}
else{
sw_door = 1;
Serial.print(sw_door);
Serial.println(',');
xbee.print(sw_door);
xbee.println(',');
}
prevMillis = currentMillis;
}
// Xbee to Arduino Added: 18-Feb-2018
if (xbee.available()){
incomingByte = xbee.read();
if(incomingByte == '1'){
digitalWrite(lightLED, HIGH);
//xbee.println("OK");
}
else if(incomingByte == '0'){
digitalWrite(lightLED, LOW);
//xbee.println("OK");
}
}
}
Python:
import threading
import time
import serial
import csv
# Arduino; Arduino is now replaced by XBee modules
arduino = serial.Serial('COM18', 9600, timeout=1) # Open serial port.
def acquire_data():
while True:
try:
data_in = arduino.readline() # read serial data from Arduino
except:
pass
data_stripped = data_in.strip() # Removes spaces and \n
for data_stripped in arduino:
if data_stripped.startswith('b') and data_stripped.count(
',') == 3: # first char identifies where data is coming from; count commas to double-check incoming string
field = data_stripped.split(',') # split data to be put into 'boxes'
bed_sen = field[0] + ',' + field[1] + ',' + field[2] # We have 3 data sensor fields
bed_sen_fill = True # A flag to show that this if-statement has been completed
if data_stripped.startswith('t') and data_stripped.count(',') == 3:
field = data_stripped.split(',')
table_sen = field[0] + ',' + field[1] + ',' + field[2]
table_sen_fill = True
if data_stripped.startswith('d') and data_stripped.count(',') == 3:
field = data_stripped.split(',')
door_sen = field[0] + ',' + field[1] + ',' + field[2]
door_sen_fill = True
try:
if bed_sen_fill == True and table_sen_fill == True and door_sen_fill == True:
data_combi = bed_sen + ',' + table_sen + ',' + door_sen
break
except:
pass
if data_combi:
datasplit = data_combi.split(",")
field1 = datasplit[0]
field2 = datasplit[1]
field3 = datasplit[2]
field4 = datasplit[3]
field5 = datasplit[4]
field6 = datasplit[5]
field7 = datasplit[6]
field8 = datasplit[7]
field9 = datasplit[8]
with open('abs_testing.csv', 'ab') as csvfile: # 'ab' to remove newline char after each print
writer = csv.writer(csvfile)
sensor_fields = [field1, field2, field3, field4, field5, field6, field7, field8, field9,
time.strftime("%H%M%S")]
writer.writerow(sensor_fields)
time.sleep(1)
def counting():
while True:
sum = 3 + 2
sum2 = sum*8
print sum2
time.sleep(0.2)
def on_light():
strin = '1'
arduino.write(strin.encode())
print "Confirm ON"
def off_light():
strin = '0'
arduino.write(strin.encode())
print "Confirm OFF"
# now threading1 runs regardless of user input
threading1 = threading.Thread(target = acquire_data)
threading2 = threading.Thread(target = counting)
threading1.daemon = False # May remove later. Unsure at the moment.
threading2.daemon = False # May remove later. Unsure at the moment.
threading1.start()
threading2.start()
while True:
if raw_input() == 't':
on_light()
print "ON"
if raw_input() == 'r':
off_light()
print "OFF"
time.sleep(1)
Multithreading is implemented here by a silly operation that finds what is 8*5, because later, this will be expanded to a real-time machine learning function that determines when the lights should turn on/off. The raw_input() functions are proofs that data can be relayed back to the Arduino-sensor node.
Thank you so much for your help! :)

A few thoughts on possible improvements:
Increase Serial() baud rate to 115200 on Arduino.
Increase XBee baud rate to 115200 on Arduino (or as high as possible with software serial).
Increase baud rate to 115200 on PC running Python (it can definitely handle that rate, and the rates on each device don't need to match).
Remove or reduce the sleep(1) in acquire_data().
Check the sleep configuration on the end devices. Is it possible they're sleeping for seconds at a time? Can you configure them as routers to eliminate that as a possible cause of delays?
Change your sending code to create a single string (see below) and then send it. Then you could easily send it to the XBee first and the Serial output separately. If the Serial() interface isn't buffered, you could be timing your XBee packet out (see ATRO setting) and having your response sent as multiple packets (which is less efficient). Try increasing ATRO from its default of 3 to 20 and see if that helps.
Right now you send every second. What if you checked I/O more frequently and only sent responses when an input changed (or it had been 5 seconds since the last transmission)?
Your sending every second, so on average you'll send 0.5 seconds after an I/O change. You could add some timing code on the Arduino to print out how many milliseconds it takes to assemble and send your data (which could account for some of the delay). Or try this replacement code:
char buffer[16];
pirValue = digitalRead(pirPin);
digitalWrite(LEDPirPIN, pinValue);
sprintf(buffer, "d%u,%u,%u,\n",
digitalRead(IR) == LOW,
pirValue == HIGH,
digitalRead(lightLED) == LOW);
xbee.print(buffer);
Serial.print(buffer);

C++ GUI freezes during communication over socket thread with Python

First of all I want to mention that I've gone through a lot of socket threading tutorials before attempting to integrate it into my own GUI, but I still consider myself fairly new to the subject.
I have a server written in python that opens a socket and starts a thread over localhost, and listens over the port for a client:
def StartThread():
tcpsock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcpsock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
tcpsock.bind((TCP_IP, TCP_PORT))
threads = []
tcpsock.listen(5)
print "Waiting for incoming connections from client..."
(conn, (ip,port)) = tcpsock.accept()
print 'Got connection from ', (ip,port)
newthread = ClientThread(ip,port,conn, 1)
newthread.start()
return newthread
Here is the ClientThread class:
class ClientThread(Thread):
def __init__(self,ip,port,sock, status):
Thread.__init__(self)
self.ip = ip
self.port = port
self.sock = sock
self.status = 1
print " New thread started for "+ip+":"+str(port)
After I run the server code, I click a button on my C++ GUI which runs the client code StartConnection() based off the Microsoft Winsock client template.
void CPSR_CRSDlg::OnBnClickedInit2()
{
if(!checkTimerStarted())
// Set mmtimer to aquire the data
{
m_ntimerID = timeSetEvent( TIMERINTRRVAL,
1,
timerHandler,
0,
TIME_PERIODIC);
SetTimer(1,TIMERINTRRVAL_2,NULL);
m_bTimer_started = true;
}
StartConnection();
}
The connection does start successfully and the python code prints the address of the connection and the thread, but then my GUI freezes, so that I can't click any of the other buttons to actually send data over the connection.
Why does my application freeze? Am I not instantiating the thread correctly?
The StartConnection() code is off the msdn website but here is my slightly modified version if needed:
void StartConnection(){
//printf("Connection Starting... \n");
WSADATA wsaData;
ConnectSocket = INVALID_SOCKET;
struct addrinfo *result = NULL,
*ptr = NULL,
hints;
int argc = 2;
// Validate the parameters
if (argc != 2) {
printf("usage: %s server-name\n", "client");
return;
}
// Initialize Winsock
iResult = WSAStartup(MAKEWORD(2,2), &wsaData);
if (iResult != 0) {
printf("WSAStartup failed with error: %d\n", iResult);
return;
}
ZeroMemory( &hints, sizeof(hints) );
hints.ai_family = AF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
hints.ai_protocol = IPPROTO_TCP;
// Resolve the server address and port
//iResult = getaddrinfo(argv[1], DEFAULT_PORT, &hints, &result);
iResult = getaddrinfo("localhost", DEFAULT_PORT, &hints, &result);
if ( iResult != 0 ) {
printf("getaddrinfo failed with error: %d\n", iResult);
WSACleanup();
return;
}
// Attempt to connect to an address until one succeeds
for(ptr=result; ptr != NULL ;ptr=ptr->ai_next) {
// Create a SOCKET for connecting to server
ConnectSocket = socket(ptr->ai_family, ptr->ai_socktype,
ptr->ai_protocol);
if (ConnectSocket == INVALID_SOCKET) {
printf("socket failed with error: %ld\n", WSAGetLastError());
WSACleanup();
return;
}
// Connect to server.
iResult = connect( ConnectSocket, ptr->ai_addr, (int)ptr->ai_addrlen);
if (iResult == SOCKET_ERROR) {
closesocket(ConnectSocket);
ConnectSocket = INVALID_SOCKET;
continue;
}
break;
}
freeaddrinfo(result);
if (ConnectSocket == INVALID_SOCKET) {
printf("Unable to connect to server!\n");
WSACleanup();
return;
}
return;
}
void StopConnection(){
closesocket(ConnectSocket);
WSACleanup();
return;
}
void SendJointValues(double *joints){
iResult = recv(ConnectSocket, recvbuf, recvbuflen, 0);
j = parseJSON(joints[0],joints[1],joints[2],joints[3], \
joints[4],joints[5]);
int x = send(ConnectSocket, j, strlen(j), 0);
//iResult = recv(ConnectSocket, recvbuf, recvbuflen, 0);
}
Edit:
The GUI does respond again after I send data via the server, and the client then successfully sends back one iteration of data. But after this exchange the GUI freezes again until more data is sent.
Here is the send+receive portion of the server:
def receiveData(self):
while (self.status == 1):
joints = [0,0,0,0,0,0]
self.sock.sendall('send')
print "Data Sent!"
data = self.sock.recv(4096)
print "Data received: ", data
self.checkStatus(data)
print "Status is: ", self.status
if (self.status == 1):
data_loaded = json.loads(data)
joints = self.createJointArr(data_loaded['Joints'])
time.sleep(0.5)
print joints
return joints
And the SendJointValues() in the client is called in this Timer function:
void CPSR_CRSDlg::OnTimer(UINT_PTR nIDEvent)
{
CString str;
long Position;
double engPosition;
double dblPosition;
double dblSpeed;
bool ret;
// Arch
USB4_01.Controller_3.getEncoderPosition(Position);
countTodegree(Position,engPosition,ARCH);
str.Format(_T("%10.2f"),engPosition);
m_staPosArch.SetWindowText(str);
// Wrist Pitch
USB4_01.Controller_2.getEncoderPosition(Position);
countTodegree(Position,engPosition,TILT);
str.Format(_T("%10.2f"),engPosition);
m_staPosPitch.SetWindowText(str);
// Linear
USB4_02.Controller_1.getEncoderPosition(Position);
countTodegree(Position,engPosition,LINEAR);
str.Format(_T("%10.2f"),engPosition);
m_staPosLinear.SetWindowText(str);
// Turret
USB4_02.Controller_2.getEncoderPosition(Position);
countTodegree(Position,engPosition,TURRET);
str.Format(_T("%10.2f"),engPosition);
m_staPosTurret.SetWindowText(str);
// Roll
USB4_02.Controller_4.getEncoderPosition(Position);
countTodegree(Position,engPosition,ROLL);
str.Format(_T("%10.2f"),engPosition);
m_staPosRoll.SetWindowText(str);
// Drill/Tool
USB4_02.Controller_3.getEncoderPosition(Position);
countTodegree(-Position,engPosition,DRILL);
str.Format(_T("%10.2f"),engPosition);
m_staPosDrill.SetWindowText(str);
// For Penetrate joint
if(JntPenetration.isInitialized())
{
// Get Position feedback
ret = JntPenetration.getPosition(dblPosition);
if(ret) // get position feedback successfully
{
Position = (long) dblPosition;
countTodegree(Position,engPosition,PENETRATE);
str.Format(_T("%10.2f"),engPosition);
m_staPosPenetrate.SetWindowText(str);
m_dCurentPentrationPosition = engPosition;
}
// Get Speed feedback;
if(m_bDrilling_started)
{
// Penetration position close enough AND At least on cycle reached
if( (abs(engPosition - m_dDrilling_target_position) < 0.1) &&(m_Direction_Changed == true))
m_bPenetrationStopped = true;
else
m_bPenetrationStopped = false;
//JntPenetration.getSpeed(dblSpeed);
//if(dblSpeed < .05 )
// m_bPenetrationStopped = true;
//else
// m_bPenetrationStopped = false;
}
}
SendJointValues(JointArray);
// For drilling motion control
if(m_bDrilling_started)
drilingProcedure();
CDialog::OnTimer(nIDEvent);
}