From f56ef2c74cb9d98b47c58c58067c03c653c08516 Mon Sep 17 00:00:00 2001 From: koennecke Date: Wed, 18 Mar 2020 15:53:44 +0100 Subject: [PATCH] Committing the last status of selene work in the corona lockdown. The problem is in the MCU, probably --- .gitignore | 121 ++++++++++++++++++++++++++++ 10-llbDevices.rules | 1 + sinqEPICSApp/src/pmacAxis.cpp | 68 ++++++++++++---- sinqEPICSApp/src/pmacAxis.h | 14 +++- sinqEPICSApp/src/pmacController.cpp | 4 +- testEuroMoveUsb.py | 88 ++++++++++++++++++++ 6 files changed, 277 insertions(+), 19 deletions(-) create mode 100644 .gitignore create mode 100644 10-llbDevices.rules create mode 100755 testEuroMoveUsb.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b205e48 --- /dev/null +++ b/.gitignore @@ -0,0 +1,121 @@ +# Took these from the https://github.com/github/gitignore project on October 21, 2011 + +# **** 'Personal' entries don't belong in here - put them in your .git/info/exclude file **** + +# Ignore text editor (e.g. emacs) autosave files +*~ + + +# Compiled Object files +*.slo +*.lo +*.o +*.obj +*.d +SICServer* + +# Compiled Dynamic libraries +*.so + +# Compiled Static libraries +*.lai +*.la +*.a + +# Compiled python files +*.py[co] + +# Eclipse-generated files +*.pydevproject +.project +.metadata +bin/** +tmp/** +tmp/**/* +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +*.sln +*.vcproj +*.exe +*.vcxproj +*.filters + +# User-specific files +*.suo +*.user +*.sln.docstates +*.sdf + +#Test results +*.log + +# Build results +[Dd]ebug/ +[Rr]elease/ +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.vspscc +.builds + + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf + +# Visual Studio profiler +*.psess +*.vsp + +# ReSharper is a .NET coding add-in +_ReSharper* + + + +# Others +*.autosave + + +# Windows image file caches +Thumbs.db + + +# Mac OS X Finder +.DS_Store +._* diff --git a/10-llbDevices.rules b/10-llbDevices.rules new file mode 100644 index 0000000..c939cfd --- /dev/null +++ b/10-llbDevices.rules @@ -0,0 +1 @@ +SUBSYSTEM=="usb", ATTR{idVendor}=="04b4", ATTR{idProduct}=="1002", MODE="0666" GROUP="plugdev", TAG+="uaccess" diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp index c7a5d7f..e11c3ba 100644 --- a/sinqEPICSApp/src/pmacAxis.cpp +++ b/sinqEPICSApp/src/pmacAxis.cpp @@ -44,6 +44,9 @@ #define MULT 1000. +#define IDLEPOLL 2. +#define BUSYPOLL .05 + #define ABS(x) (x < 0 ? -(x) : (x)) extern "C" void shutdownCallback(void *pPvt) @@ -56,9 +59,10 @@ extern "C" void shutdownCallback(void *pPvt) } // These are the pmacAxis class methods -pmacAxis::pmacAxis(pmacController *pC, int axisNo) +pmacAxis::pmacAxis(pmacController *pC, int axisNo, bool enable) : SINQAxis(pC, axisNo), - pC_(pC) + pC_(pC), + autoEnable(enable) { static const char *functionName = "pmacAxis::pmacAxis"; @@ -80,6 +84,7 @@ pmacAxis::pmacAxis(pmacController *pC, int axisNo) status6Time = 0; starting = 0; homing = 0; + next_poll = -1; /* Set an EPICS exit handler that will shut down polling before asyn kills the IP sockets */ epicsAtExit(shutdownCallback, pC_); @@ -156,13 +161,18 @@ asynStatus pmacAxis::getAxisInitialStatus(void) } // Enable the axis. After startup, the axis are disabled on the controller... - sprintf(command, "M%2.2d14=1", axisNo_); - cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); - if (cmdStatus ) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: enaabling axis %d failed.\n", functionName, axisNo_); - return asynError; - } + // Warning: Selene lift axis should not be automatically enabled + if (autoEnable) { + sprintf(command, "M%2.2d14=1\n", axisNo_); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "Enable axis %d: %s",axisNo_,command); + cmdStatus = pC_->lowLevelWriteRead(axisNo_,command, response); + if (cmdStatus ) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: enaabling axis %d failed.\n", functionName, axisNo_); + return asynError; + } + } + callParamCallbacks(); return asynSuccess; @@ -202,7 +212,9 @@ asynStatus pmacAxis::move(double position, int relative, double min_velocity, do sprintf( command, "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", axisNo_, axisNo_, realPosition, axisNo_); status = pC_->lowLevelWriteRead(axisNo_,command, response); - + + next_poll = -1; + return status; } @@ -224,6 +236,8 @@ asynStatus pmacAxis::home(double min_velocity, double max_velocity, double accel status = pC_->lowLevelWriteRead(axisNo_,command, response); homing = 1; + next_poll = time(NULL) + BUSYPOLL; + return status; } @@ -277,6 +291,11 @@ asynStatus pmacAxis::poll(bool *moving) static const char *functionName = "pmacAxis::poll"; char message[132]; + // Protect against excessive polling + if(time(NULL) < next_poll){ + return asynSuccess; + } + sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_); pC_->debugFlow(message); @@ -285,7 +304,13 @@ asynStatus pmacAxis::poll(bool *moving) asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: getAxisStatus failed to return asynSuccess. Controller: %s, Axis: %d.\n", functionName, pC_->portName, axisNo_); } - + + if(*moving){ + next_poll = time(NULL) + BUSYPOLL; + } else { + next_poll = time(NULL) + IDLEPOLL; + } + callParamCallbacks(); return status ? asynError : asynSuccess; } @@ -346,8 +371,8 @@ asynStatus pmacAxis::getAxisStatus(bool *moving) int done = 0, posChanging = 0; double position = 0; int nvals = 0; - int axisProblemFlag = 0; - epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim= 0; + int axisProblemFlag = 0, axStat = 0; + epicsUInt32 axErr = 0, highLim = 0, lowLim= 0; char message[132], *axMessage; @@ -420,7 +445,7 @@ asynStatus pmacAxis::getAxisStatus(bool *moving) previous_position_ = position; previous_direction_ = direction; - // errlogPrintf("Polling, axStat = %d, axErr = %d, position = %f\n", axStat, axErr, position); + errlogPrintf("Polling %d, axStat = %d, axErr = %d, position = %f\n", axisNo_, axStat, axErr, position); /* are we done? */ if((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0 ){ @@ -618,7 +643,9 @@ asynStatus SeleneAxis::move(double position, int relative, double min_velocity, errlogPrintf("Sending drive command: %s\n", command); status = pC_->lowLevelWriteRead(axisNo_,command, response); - + + next_poll = -1; + return status; } /*----------------------------------------------------------------------------------------------------*/ @@ -656,6 +683,8 @@ asynStatus LiftAxis::move(double position, int relative, double min_velocity, status = pC_->lowLevelWriteRead(axisNo_,command, response); waitStart = 1; + next_poll = -1; + return status; } /*-------------------------------------------------------------------------------------------------------- @@ -666,6 +695,11 @@ asynStatus LiftAxis::poll(bool *moving) { asynStatus status; + // Protect against excessive polling + if(time(NULL) < next_poll){ + return asynSuccess; + } + status = getAxisStatus(moving); if(*moving == false && waitStart == 1){ *moving = true; @@ -675,6 +709,12 @@ asynStatus LiftAxis::poll(bool *moving) if(*moving){ waitStart = 0; } + + if(*moving){ + next_poll = time(NULL) + BUSYPOLL; + } else { + next_poll = time(NULL) + IDLEPOLL; + } callParamCallbacks(); return status; } diff --git a/sinqEPICSApp/src/pmacAxis.h b/sinqEPICSApp/src/pmacAxis.h index 824f91d..d7a297b 100644 --- a/sinqEPICSApp/src/pmacAxis.h +++ b/sinqEPICSApp/src/pmacAxis.h @@ -28,7 +28,7 @@ class pmacAxis : public SINQAxis { public: /* These are the methods we override from the base class */ - pmacAxis(pmacController *pController, int axisNo); + pmacAxis(pmacController *pController, int axisNo, bool enable=true); virtual ~pmacAxis(); asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); @@ -62,6 +62,10 @@ class pmacAxis : public SINQAxis int homing; double statusPos; + time_t next_poll; + + bool autoEnable; + friend class pmacController; }; @@ -84,7 +88,7 @@ class SeleneAxis : public pmacAxis { public: - SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController,axisNo) + SeleneAxis(SeleneController *pController, int axisNo, double limitTarget) : pmacAxis((pmacController *)pController, axisNo, false) { this->limitTarget = limitTarget; }; @@ -113,11 +117,15 @@ class SeleneAxis : public pmacAxis Mark Koennecke, February 2020 + The axis should not be enabled automatically + + Michele Brambilla, February 2020 + */ class LiftAxis : public pmacAxis { public: - LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo) {}; + LiftAxis(pmacController *pController, int axisNo) : pmacAxis((pmacController *)pController,axisNo,false) {}; asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); asynStatus poll(bool *moving); asynStatus stop(double acceleration); diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp index a4975ed..2884b77 100644 --- a/sinqEPICSApp/src/pmacController.cpp +++ b/sinqEPICSApp/src/pmacController.cpp @@ -726,14 +726,14 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu // TODO: somethign is really shitty here: lowLimit and highLimit cannot be on the command if (function == motorLowLimit_) { - sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/MULT)); + sprintf(command, "Q%d54=%f", pAxis->axisNo_, value/MULT); // sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT)); asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Setting low limit on controller %s, axis %d to %f\n", functionName, portName, pAxis->axisNo_, value); errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command); } else if (function == motorHighLimit_) { - sprintf(command, "Q%d53=%d", pAxis->axisNo_, (int)(value/MULT)); + sprintf(command, "Q%d53=%f", pAxis->axisNo_, value/MULT); // sprintf(command, "Q%d53=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT)); asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s: Setting high limit on controller %s, axis %d to %f\n", diff --git a/testEuroMoveUsb.py b/testEuroMoveUsb.py new file mode 100755 index 0000000..8fd6989 --- /dev/null +++ b/testEuroMoveUsb.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +# -*- coding: iso-8859-1 -*- +""" +Created on Thur Feb 06 10:22:42 2020 + +@author: gaston emmanuel exil +""" + +#pip install pyusb +#sudo cp 10-llbDevices.rules /etc/udev/rules.d/ +#sudo udevadm control --reload-rules && udevadm trigger +#reboot + +import sys +import usb.core +import usb.util +from time import sleep + +# 1. Device +llbVendorID=0x04B4 +llbProductID=0x1002 + +# 2. Configuration +CONFIGURATION_EV3 = 1 # 1-based +# 3. Interface +INTERFACE_EV3 = 0 # 0-based +# 4. Alternate setting +SETTING_EV3 = 0 # 0-based +# 5. Endpoint +ENDPOINT_EV3 = 1 # 0-based + +# find our device +device = usb.core.find(idVendor=llbVendorID, idProduct=llbProductID) + +# was it found? +if device is None: + raise ValueError('Device not found') + sys.exit(1) +else: + if device._manufacturer is None: + device._manufacturer = usb.util.get_string(device, device.iManufacturer) + print("manufacturer: ", str(device._manufacturer)) + if device._product is None: + device._product = usb.util.get_string(device, device.iProduct) + print("product: ", str(device._product)) + + +# By default, the kernel will claim the device and make it available via +# /dev/usb/hiddevN and /dev/hidrawN which also prevents us +# from communicating otherwise. This removes these kernel devices. +# Yes, it is weird to specify an interface before we get to a configuration. + +if device.is_kernel_driver_active(INTERFACE_EV3): + print("Detaching kernel driver") + device.detach_kernel_driver(INTERFACE_EV3) + + # claim the device + usb.util.claim_interface(device, INTERFACE_EV3) + +# write endpoint +endpoint_BulkOut = device[0][(0,0)][0] +# Read endpoint +endpoint_BulkIn = device[0][(0,0)][2] +try: + command = "L"+chr(13) + assert device.write(endpoint_BulkOut, command.encode('utf-8'), 100) == len(command) + ret = device.read(endpoint_BulkIn, endpoint_BulkIn.wMaxPacketSize, timeout=30000) + print(ret) + #byte_str = "".join(chr(n) for n in ret) + print(ret.tostring()) + + + + command = "A1,4"+chr(13) + assert device.write(endpoint_BulkOut, command.encode('utf-8'), 100) == len(command) + ret = device.read(endpoint_BulkIn, endpoint_BulkIn.wMaxPacketSize, timeout=30000) + print(ret) + byte_str = "".join(chr(n) for n in ret) + print(ret.tostring()) + +except Exception as e: + print(e) + + +# release the device +usb.util.release_interface(device, INTERFACE_EV3) +# reattach the device to the OS kernel +#device.attach_kernel_driver(INTERFACE_EV3)