We Keep Coding

ESP32: How to send a jpg out of SPIFFS via WiFiClient to a python server socket?

I want that my ESP32 takes a picture, stores it in the SPIFFS and then send it via a WiFiClient from WiFi.h. The reciever is a python server socket. The server should store it as a jpg in the local windows filesystem.
So far i can connect the esp to the wifi, take a picture and store it in the SPIFFS. In another project i already send char from the WiFiClientto the python server without any problem. But if i want to send the picture a bytes with WiFiClient.print() it only sends two bytes and that cannot be correct i think.
What i need is help for sending the bytes of the jpg from the esp and decode the recieved bytes from the python socket server and store the bytes as a jpg. If there is a better way then using a the esp32 WiFiClient-class or python socket, feel free to say it.
Here is my ESP code (yes i know. The structure is not the best. Its only an example for me):
#include <WiFi.h>
const char* ssid = "xxx";
const char* password = "xxx";
const uint16_t port = xxx;
const char * host = "xxx";
/*********
Rui Santos
Complete instructions at https://RandomNerdTutorials.com/esp32-cam-projects-ebook/
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files.
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*********/
#include "esp_camera.h"
//#include "SPI.h"
//#include "driver/rtc_io.h"
#include <FS.h>
#include <SPIFFS.h>
#define CAMERA_MODEL_WROVER_KIT
#if defined(CAMERA_MODEL_WROVER_KIT)
#define PWDN_GPIO_NUM -1
#define RESET_GPIO_NUM -1
#define XCLK_GPIO_NUM 21
#define SIOD_GPIO_NUM 26
#define SIOC_GPIO_NUM 27
#define Y9_GPIO_NUM 35
#define Y8_GPIO_NUM 34
#define Y7_GPIO_NUM 39
#define Y6_GPIO_NUM 36
#define Y5_GPIO_NUM 19
#define Y4_GPIO_NUM 18
#define Y3_GPIO_NUM 5
#define Y2_GPIO_NUM 4
#define VSYNC_GPIO_NUM 25
#define HREF_GPIO_NUM 23
#define PCLK_GPIO_NUM 22
#else
#error "Camera model not selected"
#endif
// Photo File Name to save in SPIFFS
#define FILE_PHOTO "/photo.jpg"
File file;
camera_fb_t * fb;
WiFiClient client;
void setup() {
//WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); //disable brownout detector
Serial.begin(115200);
Serial.println();
// Connect to Wi-Fi
WiFi.begin(ssid, password);
Serial.print("Connecting to WiFi...");
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println();
if (!SPIFFS.begin(true)) {
Serial.println("An Error has occurred while mounting SPIFFS");
ESP.restart();
}
else {
delay(500);
Serial.println("SPIFFS mounted successfully");
}
// Print ESP32 Local IP Address
Serial.print("IP Address: http://");
Serial.println(WiFi.localIP());
camera_config_t config;
config.ledc_channel = LEDC_CHANNEL_0;
config.ledc_timer = LEDC_TIMER_0;
config.pin_d0 = Y2_GPIO_NUM;
config.pin_d1 = Y3_GPIO_NUM;
config.pin_d2 = Y4_GPIO_NUM;
config.pin_d3 = Y5_GPIO_NUM;
config.pin_d4 = Y6_GPIO_NUM;
config.pin_d5 = Y7_GPIO_NUM;
config.pin_d6 = Y8_GPIO_NUM;
config.pin_d7 = Y9_GPIO_NUM;
config.pin_xclk = XCLK_GPIO_NUM;
config.pin_pclk = PCLK_GPIO_NUM;
config.pin_vsync = VSYNC_GPIO_NUM;
config.pin_href = HREF_GPIO_NUM;
config.pin_sscb_sda = SIOD_GPIO_NUM;
config.pin_sscb_scl = SIOC_GPIO_NUM;
config.pin_pwdn = PWDN_GPIO_NUM;
config.pin_reset = RESET_GPIO_NUM;
config.xclk_freq_hz = 20000000;
config.pixel_format = PIXFORMAT_JPEG;
if(psramFound()){
config.frame_size = FRAMESIZE_UXGA;
config.jpeg_quality = 10;
config.fb_count = 2;
} else {
config.frame_size = FRAMESIZE_SVGA;
config.jpeg_quality = 12;
config.fb_count = 1;
}
// Initialize camera
esp_err_t err = esp_camera_init(&config);
if (err != ESP_OK) {
Serial.printf("Camera init failed with error 0x%x", err);
return;
}
}
void loop()
{
if (!client.connect(host, port)) {
Serial.println("Connection to host failed");
delay(1000);
return;
}
Serial.println("Connected to server successful!");
fb = NULL; // pointer
bool ok = 0; // Boolean indicating if the picture has been taken correctly
do {
// Take a photo with the camera
Serial.println("Taking a photo...");
fb = esp_camera_fb_get();
if (!fb) {
Serial.println("Camera capture failed");
return;
}
// Photo file name
Serial.printf("Picture file name: %s\n", FILE_PHOTO);
file = SPIFFS.open(FILE_PHOTO, FILE_WRITE);
// Insert the data in the photo file
if (!file) {
Serial.println("Failed to open file in writing mode");
}
else {
file.write(fb->buf, fb->len); // payload (image), payload length
Serial.print("The picture has been saved in ");
Serial.print(FILE_PHOTO);
Serial.print(" - Size: ");
Serial.print(file.size());
Serial.println(" bytes");
}
// check if file has been correctly saved in SPIFFS
ok = checkPhoto(SPIFFS);
} while ( !ok );
while(file.available()){
Serial.println(client.print(file.read()));
}
// Close the file
file.close();
esp_camera_fb_return(fb);
delay(1000);
}
// Check if photo capture was successful
bool checkPhoto( fs::FS &fs ) {
File f_pic = fs.open( FILE_PHOTO );
unsigned int pic_sz = f_pic.size();
return ( pic_sz > 100 );
}
and this is my python code:
import socket
s = socket.socket()
s.bind(('yyy', yyy ))
s.listen(0)
while True:
try:
print("-------------------------------------------------------------------")
client, addr = s.accept()
print("Accepted a connection request from %s:%s"%(addr[0], addr[1]));
while True:
content = client.recv(131072)
if len(content) ==0:
break
else:
msg_recieved = content
msg_recieved_decode = content.decode()
print(msg_recieved)
print("Message decoded recieved: " + msg_recieved_decode)
f = open("demofile.jpg", "wb")
f.write(msg_recieved)
f.close()
print("Client closed")
client.close()
except Exception as e:
print("===================================================================")
print("Something went wrong")
print(e)
print("===================================================================")

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);
}

Is there any possiblity to exit socket.receive() if it is empty

I am running my server using python like this:
Running multiple sockets using asyncio in python
c# client code:
Task class for calling server multiple times which run continously:
SocketConnector socketConnector = new SocketConnector();
while (true) {
socketConnector.establishConnection(60001);
Task task1 = Task.Factory.StartNew(() => doLogic(1, socketConnector));
socketConnector.establishConnection(60002);
Task task2 = Task.Factory.StartNew(() => doLogic(2, socketConnector));
socketConnector.establishConnection(60003);
Task task3 = Task.Factory.StartNew(() => doLogic(3, socketConnector));
socketConnector.establishConnection(60004);
Task task4 = Task.Factory.StartNew(() => doLogic(4, socketConnector));
socketConnector.establishConnection(60005);
Task task5 = Task.Factory.StartNew(() => doLogic(5, socketConnector));
Task.WaitAll(task1, task2, task3,task4, task5);
}
void doLogic(int batchIdentifier,SocketConnector socketConnector)
{
GC.Collect();
z = socketConnector.ServerRequest(batchIdentifier);
///process data
}
SocketConnector class
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Net.Sockets;
using System;
using System.IO;
public class SocketConnector
{
public string ip = "127.0.0.1";
public int port = 60000;
private Socket client;
[SerializeField]
private int dataOut;
private byte[] dataIn; //debugging
public SocketConnector()
{
}
public void establishConnection(int port)
{
client = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
client.Connect(ip, port);
if (!client.Connected)
{
Debug.LogError("Connection Failed");
}
}
public byte[] ServerRequest(int dataOut)
{
this.dataOut = dataOut; //debugging
this.dataIn = SendAndReceive(dataOut); //debugging
return this.dataIn;
}
private byte[] SendAndReceive(int dataOut)
{
byte[] intBytes = BitConverter.GetBytes(dataOut);
client.Send(intBytes);
byte[] bytes = new byte[1024 * 1024 * 1500];
Array.Clear(bytes, 0, bytes.Length);
byte[] z = new byte[1024 * 1024 * 1500];
int lenght = 1, byteIndex = 0;
while( (lenght = client.Receive(bytes)) != 0) // blocking point
{
Buffer.BlockCopy(bytes, 0, z, byteIndex, lenght);
byteIndex += lenght;
}
Array.Resize(ref z, byteIndex);
return z;
}
}
}
The problem that i think cause infinite loop/ block is the fact that every socket is running forever on server side so when i go for a second time to do client.Receive() and it finds nothing will just remain in an infinite waiting state. I need to use while for client.Receive() cos i saw that sometimes it is needed to come back in client.Receive to not lose some parts of the bytes send from server. Do you have any idea how i can tell to the receiver to move on if it is noting in receive without killing connection from server side.
Thank you in advance.

Getting IOError: [Errno 121] Remote I/O error with smbus on python (raspberry) while trying to get data over I2C from Arduino

I am facing problems, that pyhton throws me on my raspberry pi 3 sometimes this IOError during starting a script which is requesting data from an Arduino over I2C.
Electrical connection is perfect so this is not the issues.
Furthermore I also dont get any errors while using i2cget -y 1 0x04
Only the python scripts sucks sometime and I dont know why.
This is my Arduino Code:
I register an onReceive and an onRequestEvent.
onReceive Callback will define what kind of data should be send back to the raspberry.
onRequest Callback does the response.
#include <CommonFunction.h>
#include <Wire.h>
#define I2C_ADDRESS 0x4
commonFunc GetCountsEverySecond;
int g_iOnRequestActionCode = 0;
unsigned long g_lSecondsSinceStart = 0;
void setup()
{
Wire.begin(I2C_ADDRESS);
Wire.onRequest(sendDataOverI2CGateway);
Wire.onReceive(defineOnRequestAction);
}
void loop()
{
tickSeconds();
}
void tickSeconds()
{
if (GetCountsEverySecond.TimeTriggerAt(1000))
{
g_lSecondsSinceStart++;
}
}
void sendOperationTimeDataOverI2C()
{
unsigned long longInt = g_lSecondsSinceStart;
byte size = sizeof(longInt);
byte arr[size];
for (int i = 0; i < size; i++)
{
int iBitShift = 8 * (size - i - 1);
if (iBitShift >= 8)
arr[i] = ((longInt >> iBitShift) & 0xFF);
else
arr[i] = (longInt & 0xFF);
}
Wire.write(arr, size);
g_bI2CSending = true;
}
void sendDataOverI2CGateway()
{
switch(g_iOnRequestActionCode)
{
case 0:
sendRainDataOverI2C();
break;
case 1: // send firmware version
sendVersionDataOverI2C();
break;
case 2: // send operation time of arduino in seconds from start
sendOperationTimeDataOverI2C();
break;
default: break;
}
}
void defineOnRequestAction(int iBuffer)
{
while (Wire.available())
{
g_iOnRequestActionCode = Wire.read();
}
}
Here is my python Code.
Pretty straight forward but it causes some headache.
import smbus
import time
bus = smbus.SMBus(1)
while True:
data = bus.read_i2c_block_data(0x04,0x02,4)
result = 0
for b in data:
result = result * 256 + int(b)
print(result)
time.sleep(1)
After executing my python script I am getting sometime this error:
pi#WeatherStation:~/workspace $ sudo python readTimeOperationData.py
Traceback (most recent call last):
File "readTimeOperationData.py", line 5, in <module>
data = bus.read_i2c_block_data(0x04,0x02,4)
IOError: [Errno 121] Remote I/O error
Can anyone help me to fix this issue?
Cheers Dieter

I solved it!!
I got a hint from this post:
https://www.raspberrypi.org/forums/viewtopic.php?t=203286
By adding a delay after bus = smbus.SMBus(1) solved this issue.
It seems that a short delay is somehow needed so that the I2C can settle.
Working Code tested by calling script 100times without issues.
import smbus
import time
bus = smbus.SMBus(1)
time.sleep(1) #wait here to avoid 121 IO Error
while True:
data = bus.read_i2c_block_data(0x04,0x02,4)
result = 0
for b in data:
result = result * 256 + int(b)
print(result)
time.sleep(1)

This worked for me
import smbus
import time
bus = smbus.SMBus(1)
time.sleep(1)
if you use RPLCD library, add this code before initialize the object
bus = smbus.SMBus(1)
time.sleep(1) #wait here to avoid 121 IO Error
lcd = CharLCD('PCF8574', 0x27)
lcd.cursor_pos = (0,0)
thank you

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);
}