diff --git a/txt2-M4-rt-core/CMakeLists.txt b/txt2-M4-rt-core/CMakeLists.txt
index bd5be3ea24289bc8fda427926091e899bb735507..1fd007a965bc9f39f20c6b8754fa8bfef66249e6 100644
--- a/txt2-M4-rt-core/CMakeLists.txt
+++ b/txt2-M4-rt-core/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5.2)
project(ftproj)
set(LIB_VERSION_MAJOR 0)
-set(LIB_VERSION_MINOR 1)
+set(LIB_VERSION_MINOR 2)
set(LIB_VERSION_PATCH 0)
set(LIB_VERSION_STRING ${LIB_VERSION_MAJOR}.${LIB_VERSION_MINOR}.${LIB_VERSION_PATCH})
set(CMAKE_SKIP_BUILD_RPATH TRUE)
diff --git a/txt2-M4-rt-core/TxtControlLib/TxtControlLib.c b/txt2-M4-rt-core/TxtControlLib/TxtControlLib.c
index 5d2af5f35e71fce265a6a6b5b9f28a361de267dc..cece08f2d3c30dbe10e49327590e40d0e980f0ea 100644
--- a/txt2-M4-rt-core/TxtControlLib/TxtControlLib.c
+++ b/txt2-M4-rt-core/TxtControlLib/TxtControlLib.c
@@ -35,6 +35,8 @@
#include <sys/mman.h>
+//#define USE_SERVO_CYCLIC_SYNC 1
+
/**
@brief Init object structures
@param txt TXT2 controller
@@ -357,6 +359,128 @@ static void *ftopThread(void *info)
return NULL;
}
+/**
+ * @brief Get the Servo Min Max calibration values
+ *
+ * @param num Servo number 1-3
+ * @param [out] pu16Min Servo minimum calibration value
+ * @param [out] pu16Max Servo maximum calibration value
+ */
+static void getServoMinMax(int num, uint16_t* pu16Min, uint16_t* pu16Max)
+{
+ FILE *fp;
+ char buffer[128];
+ char valName[20];
+ char* pstr;
+ int i;
+
+ memset(buffer,0,sizeof(buffer));
+ memset(valName,0,sizeof(valName));
+
+ if ((pu16Min == NULL) || (pu16Max == NULL))
+ {
+ return;
+ }
+
+ *pu16Min = 600;
+ *pu16Max = 2400;
+ switch (num)
+ {
+ case 1:
+ fp = popen("/usr/bin/sudo -n /usr/sbin/txt_servo getvalues 1", "r");
+ break;
+ case 2:
+ fp = popen("/usr/bin/sudo -n /usr/sbin/txt_servo getvalues 2", "r");
+ break;
+ case 3:
+ fp = popen("/usr/bin/sudo -n /usr/sbin/txt_servo getvalues 3", "r");
+ break;
+ default:
+ return;
+ }
+ if (fp == NULL) {
+ return;
+ }
+
+ while (fgets(buffer, sizeof(buffer), fp) != NULL) {
+ //printf("%s", buffer);
+ }
+ pclose(fp);
+
+ pstr=buffer;
+ valName[0] = 0;
+ while(*pstr != 0)
+ {
+ while((*pstr != 0) && (*pstr != '"'))
+ {
+ pstr++;
+ }
+ if (*pstr == '"') pstr++;
+ if (*pstr == 0) return;
+ for(i = 0;i < 20;i++)
+ {
+ if (*(pstr + i) == '"')
+ {
+ *(pstr + i) = 0;
+ if (valName[0] == 0) {
+ strncpy(valName,pstr,sizeof(valName));
+ } else
+ {
+ //printf("%s = %s\n",valName,pstr);
+ if (strcmp("min",valName) == 0)
+ {
+ *pu16Min=atoi(pstr);
+ } else if (strcmp("max",valName) == 0)
+ {
+ *pu16Max=atoi(pstr);
+ }
+ valName[0] = 0;
+ }
+ pstr += i + 1;
+ break;
+ }
+ }
+ }
+}
+
+
+static void syncServoCalibration(ftobject_txt2 *txt)
+{
+ uint16_t u16Min, u16Max;
+ for(int i = 0; i < FT_MAX_SERVO;i++)
+ {
+ getServoMinMax(i+1,&u16Min,&u16Max);
+ txt->dynamic_settings.servo[i].min = u16Min;
+ txt->dynamic_settings.servo[i].max = u16Max;
+ }
+}
+
+#if defined(USE_SERVO_CYCLIC_SYNC)
+/**
+ @brief Process input packet from M4
+ @param [in] txt TXT2 controller
+ */
+static void ProcessDynamicSettings(ftobject_txt2 *txt)
+{
+ while(1) {
+ syncServoCalibration(txt);
+ sleep(30);
+ }
+}
+
+/**
+ @brief Thread to process dynamic settings
+ @param [in] info unused
+ */
+static void *ftsyncDynSettingsThread(void *info)
+{
+ ftobject_txt2 *txt = info;
+
+ ProcessDynamicSettings(txt);
+ return NULL;
+}
+#endif
+
/**
@brief Allocate resources for local TXT
@param [in] host host with txt controller, "auto" must be used
@@ -399,6 +523,19 @@ fttxt2 *ftop_alloc_txt(const char *host, int port)
txt = NULL;
}
+ /* Start thread for sync servo calibration settings */
+ #if defined(USE_SERVO_CYCLIC_SYNC)
+ err = pthread_create(&txt->sync_settings_thread, NULL, ftsyncDynSettingsThread, txt);
+ if (err != 0) {
+ perror("pthread_create");
+ free(txt);
+ txt = NULL;
+ }
+ #else
+ syncServoCalibration(txt);
+ #endif
+
+
/* Wait and check timeout */
while (txt->uaddr == 0) {
/* Get configuration */
@@ -430,6 +567,9 @@ void ftop_free_txt(fttxt2 *txt)
};
pthread_cancel(txt2->thread);
+ #if defined(USE_SERVO_CYCLIC_SYNC)
+ pthread_cancel(txt2->sync_settings_thread);
+ #endif
ftop_socket_send(txt2->transport, &disconnect, sizeof(disconnect));
close(txt2->memid);
ftop_socket_free(txt2->transport);
diff --git a/txt2-M4-rt-core/TxtControlLib/ftactions.c b/txt2-M4-rt-core/TxtControlLib/ftactions.c
index a3ebc3d492a4352684f05bcaad64180e75520dd5..687a087c3e04517b58f1154d652e66e77b001217 100644
--- a/txt2-M4-rt-core/TxtControlLib/ftactions.c
+++ b/txt2-M4-rt-core/TxtControlLib/ftactions.c
@@ -25,6 +25,7 @@
#include "ftconfig.h"
#include <stdio.h>
+#include <stdlib.h>
#include <math.h>
/**
@@ -298,6 +299,38 @@ void ftop_set_brightness(void *device, int value)
}
}
+/**
+ @brief Set output PWM
+ @param [in] device device
+ @param [in] value PWM duty in us
+ */
+void ftop_set_pwm_us(void *device, int value)
+{
+ ftobject_txt2 *txt2 = ftop_get_txt(device);
+ ftobject_motor *cobj = device;
+ ft_pwm_type pwm = value;
+
+ if (pwm > 3000)
+ pwm = 3000;
+
+ pwm = pwm | 0x8000;
+
+ if (cobj->conn < ftdev_s1 || cobj->conn > ftdev_s3)
+ return;
+
+ cobj->speed = pwm;
+
+ struct ftop_set_pwm command = {
+ {sizeof(command), ftop_op_set_pwm, txt2->canid, 0},
+ cobj->conn, pwm};
+
+ ftop_socket_send(txt2->transport, &command, sizeof(command));
+
+ if (ftdebug & FT_DEBUG_A7_OP) {
+ fprintf(stderr, "set pwm %d 0x%x\n", cobj->conn, pwm);
+ }
+}
+
/**
@brief Set output PWM
@param [in] device device
@@ -308,10 +341,27 @@ void ftop_set_pwm(void *device, int value)
ftobject_txt2 *txt2 = ftop_get_txt(device);
ftobject_motor *cobj = device;
ft_pwm_type pwm = value;
+ uint16_t u16Min,u16Max;
+ uint32_t u32Range;
+ uint16_t u16Duty;
if (pwm > 512)
pwm = 512;
+ if (cobj->conn >= ftdev_s1 && cobj->conn <= ftdev_s3)
+ {
+ u16Min = txt2->dynamic_settings.servo[cobj->conn - ftdev_s1].min;
+ u16Max = txt2->dynamic_settings.servo[cobj->conn - ftdev_s1].max;
+ //getServoMinMax(device,&u16Min,&u16Max); do not use - causing too much delay...
+ if ((u16Min != 600) || (u16Max != 2400))
+ {
+ u32Range = (u16Max - u16Min);
+ u16Duty = (u32Range*pwm)/512UL + u16Min;
+ ftop_set_pwm_us(device,u16Duty);
+ return;
+ }
+ }
+
if (cobj->conn < ftdev_o1 || cobj->conn > ftdev_s3)
return;
@@ -336,9 +386,13 @@ void ftop_set_pwm(void *device, int value)
@param [in] device device
@return PWM (0-512) value
*/
-unsigned int ftop_get_pwm(void *device)
+unsigned int ftop_get_pwm(void *device)
{
ftobject_motor *cobj = device;
+ ftobject_txt2 *txt2 = ftop_get_txt(device);
+ uint16_t u16Min,u16Max;
+ uint32_t u32Range;
+ uint16_t u16Pwm;
if (cobj->conn < ftdev_o1 || cobj->conn > ftdev_s3)
return 0;
@@ -346,6 +400,24 @@ unsigned int ftop_get_pwm(void *device)
if (cobj->conn >= ftdev_c1 && cobj->conn <= ftdev_c4)
return 0;
+
+ if (((cobj->speed & 0x8000) != 0) && cobj->conn >= ftdev_s1 && cobj->conn <= ftdev_s3)
+ {
+ u16Min = txt2->dynamic_settings.servo[cobj->conn - ftdev_s1].min;
+ u16Max = txt2->dynamic_settings.servo[cobj->conn - ftdev_s1].max;
+ u32Range = (u16Max - u16Min);
+ u16Pwm = cobj->speed & 0x7FFF;
+ if (u16Pwm > u16Min)
+ {
+ u16Pwm -= u16Min;
+ } else
+ {
+ u16Pwm = 0;
+ }
+ u16Pwm = u16Pwm*512UL/u32Range;
+ return u16Pwm;
+ }
+
return cobj->speed;
}
diff --git a/txt2-M4-rt-core/ft/ft.hpp b/txt2-M4-rt-core/ft/ft.hpp
index 67f4c151af6026fb1a20764271ad395af2d8a6aa..c8e31ad23942b8c75d7da9859ea8593d15a7c4f2 100644
--- a/txt2-M4-rt-core/ft/ft.hpp
+++ b/txt2-M4-rt-core/ft/ft.hpp
@@ -235,6 +235,12 @@ public:
@param [in] pwm new position 0-512
*/
void setPwm(int pwm) { ftop_set_pwm(adaptee, pwm); }
+
+ /**
+ @brief Set servo motor position
+ @param [in] pwm new position 0-512
+ */
+ void setPwmUs(int pwm_us) { ftop_set_pwm_us(adaptee, pwm_us); }
};
/**
diff --git a/txt2-M4-rt-core/ft/ftop.h b/txt2-M4-rt-core/ft/ftop.h
index 63da1a3d3f6d86fc0617a8248537a2ce1aa3008e..cd37c872de14a9e806f5f61bf4b643a89577e662 100644
--- a/txt2-M4-rt-core/ft/ftop.h
+++ b/txt2-M4-rt-core/ft/ftop.h
@@ -81,6 +81,13 @@ int ftop_get_finfo(void *device, struct ft_faults *finfo);
*/
void ftop_set_brightness(void *device, int value);
+/**
+ @brief Set output PWM
+ @param [in] device device
+ @param [in] value PWM duty in us
+ */
+void ftop_set_pwm_us(void *device, int value);
+
/**
@brief Set output PWM
@param [in] device device
diff --git a/txt2-M4-rt-core/ftinc/ftinc.i b/txt2-M4-rt-core/ftinc/ftinc.i
index d5d8b088d68c84fd7aa871269468a06f605d3484..d9540fa32e4fceb5c338903eb7660a0d14a38a11 100644
--- a/txt2-M4-rt-core/ftinc/ftinc.i
+++ b/txt2-M4-rt-core/ftinc/ftinc.i
@@ -468,6 +468,12 @@ unsigned int fttxt2_nslaves_get(fttxt2 *txt2) {
"""
self.pwm_set(level)
+ def setLevelUs(self, levelUs):
+ """Set pwm level in us
+ :param level: new level in us
+ """
+ self.pwm_set_us(levelUs)
+
def setSpeed(self, speed):
"""Start the motor with selected speed
:param speed: new speed -512 - 512"""
@@ -547,6 +553,11 @@ unsigned int fttxt2_nslaves_get(fttxt2 *txt2) {
ftop_set_pwm($self, value);
}
+ %feature("autodoc", "Set pwm value in us for O1-O8 output, for example, lamp") pwm_set_us;
+ void pwm_set_us(int value) {
+ ftop_set_pwm_us($self, value);
+ }
+
%feature("autodoc", "Set a motor speed [-512, 512], for next distance operation") speed_set;
void speed_set(int value) {
ftop_motor_set_speed($self, value);
diff --git a/txt2-M4-rt-core/ftinc/ftlibtypes.h b/txt2-M4-rt-core/ftinc/ftlibtypes.h
index 34ba29de4e1a8c222f4db7cd69f54fabb15c3373..58772e3c0cba54d853657928f1190c31a57377b6 100644
--- a/txt2-M4-rt-core/ftinc/ftlibtypes.h
+++ b/txt2-M4-rt-core/ftinc/ftlibtypes.h
@@ -127,6 +127,22 @@ struct fttxt_events {
uint8_t counts[ft_event_max]; /*!< conters for different event types */
};
+/*!
+ * TXT2 controller dynamic settings for servos implementation
+ */
+typedef struct ftobject_txt2_dynamic_servo_settings {
+ uint16_t min;
+ uint16_t max;
+ uint8_t updated;
+} ftobject_txt2_dynamic_servo_settings;
+
+/*!
+ * TXT2 controller dynamic settings implementation
+ */
+typedef struct ftobject_txt2_dynamic_settings {
+ ftobject_txt2_dynamic_servo_settings servo[FT_MAX_SERVO];
+} ftobject_txt2_dynamic_settings;
+
/*!
* TXT2 controller implementation
*/
@@ -135,6 +151,7 @@ typedef struct ftobject_txt2 {
struct ftop_transport *transport; /*!< transport */
struct ftobject_txt2 *main; /*!< pointer to the main controller */
pthread_t thread; /*!< communication thread */
+ pthread_t sync_settings_thread; /*!< settings thread */
int cmdid; /*!< next cmdid */
int memid; /*!< memory file id */
@@ -180,6 +197,8 @@ typedef struct ftobject_txt2 {
/* Current values from M4 */
ftvalues current_values; /*!< Current values copy */
+ ftobject_txt2_dynamic_settings dynamic_settings;
+
} ftobject_txt2;
#endif /* FTINC_FTLIBTYPES_H_ */
diff --git a/txt2-M4-rt-core/ftsrc/ftserver.c b/txt2-M4-rt-core/ftsrc/ftserver.c
index da1aff5be52bd4fff1dd2f236504740f98acc5f9..044f61b20730297892f253e74e904e24eb9007c1 100644
--- a/txt2-M4-rt-core/ftsrc/ftserver.c
+++ b/txt2-M4-rt-core/ftsrc/ftserver.c
@@ -767,8 +767,19 @@ void ProcessRemoteOp(struct ft_master_remote *stxt, struct ft_remote_op *op)
continue;
uint16_t ipwm = op->pwm[i + FT_MAX_OUTPUTS];
- uint16_t cpwm = ipwm >= 512 ? 512 : ipwm;
- HWServo_SetDuty (i, cpwm);
+ uint16_t cpwm = ipwm;
+
+ if ((cpwm & 0x8000) != 0)
+ {
+ cpwm = cpwm & 0x7FFF;
+ cpwm = cpwm >= 3000 ? 3000 : cpwm;
+ HWServo_SetDutyUs(i, cpwm);
+ }
+ else
+ {
+ cpwm = cpwm >= 512 ? 512 : cpwm;
+ HWServo_SetDuty(i, cpwm);
+ }
}
OS_UNLOCK(out_mutex);
@@ -955,13 +966,22 @@ void ftProcessRequest(ftop_client *ftc, unsigned char *buffer)
break;
case ftop_op_set_pwm:
-
{
struct ftop_set_pwm *c = (void*) buffer;
- uint16_t npwm = c->pwm >= 512 ? 512 : c->pwm;
+ uint16_t npwm = c->pwm;
if ((c->devid >= ftdev_s1) && (c->devid <= ftdev_s3)) {
- HWServo_SetDuty(c->devid - ftdev_s1, npwm);
+ if ((npwm & 0x8000) != 0)
+ {
+ npwm = npwm & 0x7FFF;
+ npwm = npwm >= 3000 ? 3000 : npwm;
+ HWServo_SetDutyUs(c->devid - ftdev_s1, npwm);
+ }
+ else
+ {
+ npwm = npwm >= 512 ? 512 : npwm;
+ HWServo_SetDuty(c->devid - ftdev_s1, npwm);
+ }
} else if (c->devid >= ftdev_o1 && c->devid <= ftdev_o8) {
uint8_t cpwm = npwm >= 512 ? 255 : npwm / 2;
diff --git a/txt2-M4-rt-core/inc/HWServo.h b/txt2-M4-rt-core/inc/HWServo.h
index df52c6af58a9b0574e14f2f711c809b90f0523ef..a7d88bae2984bd4769dd12a319b6c881cdaff225 100644
--- a/txt2-M4-rt-core/inc/HWServo.h
+++ b/txt2-M4-rt-core/inc/HWServo.h
@@ -46,6 +46,13 @@ void HWServo_Init();
*/
void HWServo_SetDuty(unsigned int n, uint16_t duty);
+/**
+ * @brief Servo set duty procedure
+ * @param n servo number 0-2
+ * @param duty new duty value in us
+ */
+void HWServo_SetDutyUs(unsigned int n, uint16_t duty_us);
+
#endif /* INC_HWSERVO_H_ */
/** @} */
diff --git a/txt2-M4-rt-core/src/HWServo.c b/txt2-M4-rt-core/src/HWServo.c
index 81d94c62c158eab5474de721c6aab9f64f49e7a8..9eecd720cf0231add3eca41597a9e72f8eb05766 100644
--- a/txt2-M4-rt-core/src/HWServo.c
+++ b/txt2-M4-rt-core/src/HWServo.c
@@ -93,3 +93,16 @@ void HWServo_SetDuty(unsigned int n, uint16_t duty)
__HAL_TIM_SET_COMPARE(ftServoMotors[n].htim, ftServoMotors[n].Channel,
SERVO_MIN_PULSE + (SERVO_MAX_PULSE - SERVO_MIN_PULSE) * duty / 512);
}
+
+/**
+ * @brief Servo set duty procedure
+ * @param n servo number 0-2
+ * @param duty new duty value in us
+ */
+void HWServo_SetDutyUs(unsigned int n, uint16_t duty_us)
+{
+ if (n >= FT_MAX_SERVO)
+ return;
+
+ __HAL_TIM_SET_COMPARE(ftServoMotors[n].htim, ftServoMotors[n].Channel,duty_us);
+}