Ultrasonic Distance Sensor/ Module

I’m afraid that I’m being dense and cannot fathom some of the finer points of Hexy’s software.

The Ultrasonic Distance Module (hereafter referred to as UDM) is still baffling me. I’d like to use it to let Hexy get some feedback from his environment, hopefully leading to him being able to explore (map?) his surroundings.

I’m aware that the Servotor32.cpp file contains some case statements in a switch that perform actions based on being sent ‘S’ or ‘M’ through the serial link, however in the ServotorComm.py file the section marked “Retrieve waiting responses” is commented out with a TODO under it.

Two parts to my question (I think):
[ul]
[]How do I send command of ‘S’ or ‘M’ through the serial link from a Moves.py file?[/]
[]How can I capture the information that’s returned from Hexy through the serial link into the python code?[/]
[/ul]
Thanks all.

I’m using Nitroxleecher’s Servotor32.cpp code to scan in half circle when sending ‘S’. I only reduced the number of steps to 9:

#define ARCPING_STEPS 9

Then in servotorComm.py it is important to uncomment the following lines:

            if self.ser.readable():
                read = self.ser.readline()
                if len(read) == 0:
                    pass
                else:
                    self.recieveLock.acquire()
                    self.recieveQueue.append(read)
                    self.recieveLock.release()

I found that it is also vital to set the serial link timeout to very small number. The timeout also gives time between readings.

...
ser = serial.Serial(port, baudrate =  BAUD_RATE, timeout = 0.005)
...
ser = serial.Serial(i, baudrate = BAUD_RATE, timeout = 0.005)
...

Then in GUI.py or anywhere else send ‘S’ to the queue and wait for the data

        self.con.serialHandler.recieveQueue = [] # clear queue
        self.con.serialHandler.sendLock.acquire()
        self.con.serialHandler.sendQueue.append("S")
        self.con.serialHandler.sendLock.release()
        i = 0
        while (self.con.serialHandler.recieveQueue == 0) or (i < 20):
            time.sleep(0.1)
            i += 1
        for i in range(9):
            if self.con.serialHandler.recieveQueue > 0:
                dist = float(self.con.serialHandler.recieveQueue.pop(0))

BTW this code can be found in the SW I recently published. I just commented part of it because it’s not ready, but who search…

I find this discussion interesting, but I was looking for a simple syntax proposal for getting “ping” data back from Hexy.

Having an “arc-scan” built in was more that I was thinking, but a great idea.
I believe that both make sense in general.

That means that we would need syntax for two new commands:

  1. Just return a measurement where the sensor is currently pointing.
  2. Perform an “arc-scan”.

What I see as needed is a more complex syntax so that control over parameters can be included in the request. That way different situations could be more easily handled.

I propose using ‘S’ for arc-Scan, but it needs to provide allow for control over the number of scans in the arc, and the number of readings taken at each point.

We could also use say ‘M’ to make a “ping” measurement.

How do Sp:c; and Mc; sound?
Although I guess we could just use “newline” in place of the semi-colon.

The ‘p’ would represent the number of positions where scan measurements are taken,
and ‘c’ represents the “count” sent to multi ping.

An alternate proposal would be to configure these two numbers as global values. That would require two more letters in the parsing logic.

Maybe I need to thinks about this a bit more critically for it to be more easily implemented.
What do you all think?

With no opinions I assume that nobody cares that much about the level of control that I proposed.
However, I have come up with what I think is a better idea that:

  1. Better matches the model proposed earlier in this post
    because it allows measurements using a single character.
  2. Provides full control over to run-time parameters.
  3. Is much easier to implement in the command parser.

What I propose is to take advantage of the case sensitive nature of the parser as follows:

#m Sets the number of measurements by multi-ping
#s Sets the number of points taken during an arc-scan

These values will be the new defaults until ‘m’ or ‘s’ are sent again with another value.
That leaves the use of ‘S’ and ‘M’ to perform arc-scan and multi-scan respectively.

This should be easy enough to remember, although I would have used ‘A’ for arc-scan.

Example assuming multi-scan defaults to 5 readings and arc-scan to 9 points:
E1. Send “S” to scan 9 points at 5 readings each.
E2. Send “#5s#3mS” to scan 5 points at 3 readings each.
You could then just send “S” to repeat the above scan.
E3. Send “#9mMMM” to see what results from 3 consecutive multi-pings of 9 readings each.

The secret here is that the values sent by lower-case command become the new defaults until a new lower-case command is sent.

If I find no objections, or conflicting opinions, posted in the next couple of days I will put this together and send it into the code base.

that could work. You can make a code and post it here

I think you should always begin with the letter first. When you start with ‘#’, the software immediately thinks the next number is a servo number for a servo change. Definitely want to avoid command conflicts.

I understand that the current implementation is that a # followed by a number is currently interpreted as a servo.
Actually that is because the # is only currently used by commands that need a servo number and they are the only commands that need a number at all.

My plan is to capitalize on this fact in a way that will make the entire syntax easily extended in this manner.
My proposed syntax simply extends the # syntax in use by commands that require numeric values.

I made the proposal with full knowledge of the current syntax and the plan is for zero functional change to the current use of #; I see no actual conflict with my proposal.
Admittedly it will significantly modify the current parsing logic, but I expect it will actually simplify the overall command parsing logic.

I expect a fairly busy week this week so it may take until next weekend for me to find time to complete the task. Then again I may find time to work on it sooner.
Stay tuned for the result.

Okay I have something working. Still more work to do but ready for others to test.
Try something like “#3m#5sS”. After that you will just need S to rerun the same scan.
Another side effect is “#31p1500L” will center the sensor and then kill it because the servo number is remembered.

I had to modify multiPing() so that is functioned with attempts=1.
I also modified the posted arcScan() to perform less math.
I do not know why the delays work in arcScan(). It may be possible to eliminate them by using the new routine I added called waitForServoPosition().

There were a lot of changes so here is the entire new Servotor32.cpp file:

[code]#include “Servotor32.h”
#include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include “Arduino.h”

#include “SERVOTOR32_SPI.h”
#include “Servotor32_TimerOne.h”

//#define DEBUG_PROC_CHAR

#define MIN_POS 500
#define MAX_POS 2500
#define CENTER_POS 1500
#define KILL_POS -1

#define PING_SERVO 31

#define DEFAULT_MULTI_PING_CNT 5
#define DEFAULT_SCAN_PING_CNT 9
#define MAX_SCAN_PING_CNT (MAX_POS - MIN_POS)
#define MAX_MULTI_PING_CNT 64

Servotor32::Servotor32()
{

}

//stores information about the servos and groups
signed short servo_positions[SERVOS]; // where the servos are currently (supposed to be) at
signed char servos_sorted[GROUPS][SERVOS_PER_GROUP]; // index in servo_timings to where the servo ends
signed char servos_active_in_group[GROUPS]; // the number of servos in a group currently active
uint8_t active_servos_hex[GROUPS];

// all updates to shift registers in order of their updates
signed short servo_timings[MAX_TIMINGS]; // the timing where the change occurs
uint8_t shift_output[MAX_TIMINGS]; // the output of the shift register
uint8_t shift_latch[MAX_TIMINGS]; // the shift register latch used

// keeps track of whether its safe or not to update the servos
uint8_t update_reg_flag = 0;

// variables for the callback
uint16_t timer;
uint8_t counter = 0;
uint8_t pwm_active = 1;

uint16_t group_offsets[4] = {0,251,502,753};
uint8_t group_latches[4] = {5,6,7,4};
uint8_t pin_2_num[8] = {0x08,0x04,0x02,0x01, 0x80,0x40,0x20,0x10};

void Servotor32::begin(){
//setup pin modes
DDRF |= 0xF0; // sets pins F7 to F4 as outputs
DDRB = 0xFF; // sets pins B0 to B7 as outputs

//setup PC serial port
Serial.begin(9600);
// reconfigure bluetooth module to 9600 baud id needed
Serial1.begin(115200); // Changed from 9600 baud
Serial1.print(“AT+BAUD4”); // Tell the module to change the baud rate to 9600
delay(1100); // Wait a notch over 1 second to make sure the setting "sticks"
Serial1.begin(9600); // Changed from 9600 baud

SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV2);

Timer1.initialize(10);
Timer1.attachInterrupt(callback);

for(byte i=0; i<SERVOS; i++){
servo_positions[i] = -1;
}
for(byte i=0; i<GROUPS; i++){
for(byte j=0; j<SERVOS_PER_GROUP; j++){
servos_sorted[i][j] = -1;
}
}

for(uint8_t i=0; i<MAX_TIMINGS; i++){
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}

TIMSK0 &= ~(_BV(TOIE0)); // disables the arduino delay function, but also
// all but eliminates servo jitter
TIMSK2 &= ~(_BV(TOIE2)); // disable the arduino tone function, but also
// also helps eliminate some jitter
TIMSK3 &= ~(_BV(TOIE3)); // for good measure
TIMSK4 &= ~(_BV(TOIE4)); // for good measure
}

long unsigned int us_counter = 0;
long unsigned int startTime = 0;
long unsigned int currentTime = 0;
long unsigned int last_update = 0;

long unsigned int Servotor32::micros_new(){
return us_counter;
}

long unsigned int Servotor32::millis_new(){
return us_counter/1000;
}

void Servotor32::delay_ms(long unsigned int delay_time){
startTime = millis_new();
currentTime = millis_new() - startTime;
while(currentTime < delay_time){
delayMicroseconds(10);
currentTime = millis_new() - startTime;
}
}

void Servotor32::delay_us(long unsigned int delay_time){
startTime = micros_new();
currentTime = micros_new() - startTime;
while(currentTime < delay_time){
delayMicroseconds(10);
currentTime = micros_new() - startTime;
}
}

void Servotor32::callback(){
cli();
if(timer < 1100){ // keep it from updating servos mid-array change by some weird coincidence
if(timer == servo_timings[counter]){ // if the time has arrived to update a shift reg
SPDR = shift_output[counter]; // push the byte to be loaded to the SPI register
while(!(SPSR & (1<<SPIF))); //wait till the register completes
PORTF &= ~(shift_latch[counter]); // clock the shift register latch pin low, setting the register
PORTF |= shift_latch[counter]; // clock the shift register latch pin high, ready to be set low next time
counter++;
}
}

timer++;
us_counter += 10;
if(timer == 1100){ // all servo timing completed
update_reg_flag = 1; // allow updates to the timing arrays
}
if(timer == 1900){ // getting close to servo start-up again,
update_reg_flag = 0; // don’t allow any new timing array updates
}
if(timer == 2000){
timer=0;
counter=0;
}
sei();
}

void Servotor32::delete_from_sorted_array(byte servo, byte group, signed short pos){
for(byte i=0; i<servos_active_in_group[group]; i++){ // go through all the servos
if(servos_sorted[group][i] == servo){ // find its place
for(signed char j=i; j<servos_active_in_group[group]-1; j++){//move all servos in front of it back by one
servos_sorted[group][j] = servos_sorted[group][j+1];
}
servos_sorted[group][servos_active_in_group[group]-1] = -1; //insert a -1 at the end of the move
break; //break out of previous for loop, now that the job is done
}
}
active_servos_hex[group] &= ~pin_2_num[servo-group*SERVOS_PER_GROUP];
servos_active_in_group[group] -= 1;// decrease the number of active servos in the group by 1
}

void Servotor32::add_to_sorted_array(byte servo, byte group, signed short pos){
for(byte i=0; i<=servos_active_in_group[group]; i++){ // find the servo
if(servos_sorted[group][i] == -1){ // if no servos yet entered, set as first
servos_sorted[group][i] = servo; //insert the servo in its sorted place
break; //stop the for loop, as the job is done
}
else{
if(servo_positions[servos_sorted[group][i]] > pos){ // if this servo should go before this one
for(signed char j=servos_active_in_group[group]-1; j>=i; j–){// move all others forward one
servos_sorted[group][j+1] = servos_sorted[group][j];
}
servos_sorted[group][i] = servo; //insert the servo in its sorted place
break;
}
}
}
active_servos_hex[group] |= pin_2_num[servo-group*SERVOS_PER_GROUP];
servos_active_in_group[group] += 1;
}

// wait for the servos to stop pulsing before updating the timing arrays
void waitForServoPosition()
{
while (update_reg_flag == 0)
{
delayMicroseconds(10);
}
}

void Servotor32::update_registers_fast(byte servo, signed short pos){
byte group = servo/8;
while(update_reg_flag == 0){ // wait for the servos to stop pulsing before updating the timing arrays
delayMicroseconds(10);
}
// ----- put the servo into, or take it out of its sorted array ------

if(pos > 0){ // if the sevo isn’t a kill command, then its an add/change
if(servo_positions[servo] == -1){// if the servo is inactive
// insert the servo into the array sorted
add_to_sorted_array(servo,group,pos);
}
else{
// updating the servo. First delete its existing entry, then insert it

  delete_from_sorted_array(servo,group,pos);
  add_to_sorted_array(servo,group,pos);
}

}
else{ // servo is a kill command
if(servo_positions[servo] != -1){ // make sure its even on first
delete_from_sorted_array(servo,group,pos);
}
}

servo_positions[servo] = pos;

// ----- create timing idicies from servo/group data -------

// clear the timing arrays for fresh start
for(uint8_t i=0; i<MAX_TIMINGS; i++){
servo_timings[i] = 0;
shift_output[i] = 0xFF;
shift_latch[i] = 0xFF;
}

uint8_t counter_index=0;
uint8_t current_timing=0;
uint8_t current_shift_output=0;

for(byte group=0; group<GROUPS; group++){ //go through each group
if(servos_active_in_group[group] > 0){ // skip it if the group is active, otherwise:
servo_timings[counter_index] = group_offsets[group];
shift_output[counter_index] = active_servos_hex[group];
shift_latch[counter_index] = (1<<group_latches[group]);
counter_index +=1;

  //create additional timings
  for(byte i=0; i<servos_active_in_group[group]; i++){ //create the timings for each servo after that, using the previous output
    if(servo_positions[servos_sorted[group][i]] == servo_positions[servos_sorted[group][i-1]]){ // if this servo's time is the same as the last's
      if(i != 0){
        counter_index -= 1; //reverse the index count-up
      }
      else{
        current_shift_output = shift_output[counter_index-1];
        servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
        shift_latch[counter_index] = (1<<group_latches[group]);
      }
    }
    else{
      current_shift_output = shift_output[counter_index-1];
      servo_timings[counter_index] = servo_positions[servos_sorted[group][i]]+ group_offsets[group];
      shift_latch[counter_index] = (1<<group_latches[group]);
    }
    
    //subtract the current servo from the shift register output
    current_shift_output &= ~pin_2_num[servos_sorted[group][i]-group*SERVOS_PER_GROUP]; 
    shift_output[counter_index] = current_shift_output;
    counter_index +=1;
  }
}      

}

}

void Servotor32::printStatus(){
Serial.println("--------------------- Registers ----------------------");

Serial.println(“Servo Data:”);
Serial.println(“Servo\tPos\tTimeEnd\t”);
for(byte i=0; i<SERVOS; i++){
Serial.print(i);
Serial.print("\t");
Serial.print(servo_positions[i]);
Serial.println("");
}
Serial.println("");

Serial.println(“Sorted Groups”);
for(byte i=0; i<GROUPS; i++){
Serial.print("Group: ");
Serial.println(i);
for(byte j=0; j<SERVOS_PER_GROUP; j++){
Serial.print(“Servo: “);
Serial.print(servos_sorted[i][j]);
Serial.print(”\t”);
Serial.println(servo_positions[servos_sorted[i][j]]);

}

}

Serial.println(“Group Data:”);
Serial.println("#\tActive\tHex");
for(byte i=0; i<GROUPS; i++){
Serial.print(i);
Serial.print("\t");
Serial.print(servos_active_in_group[i]);
Serial.print("\t");
Serial.println(active_servos_hex[i],HEX);
}
Serial.println("");

Serial.println(“Timings:”);
Serial.println(“Pos\tTiming\tOutput\tLatch”);
for(uint8_t i=0; i<MAX_TIMINGS; i++){ // clear existing registers, so they can be cleanly written
Serial.print(i);
Serial.print(":\t");
Serial.print(servo_timings[i]);
Serial.print(",\t");
Serial.print(shift_output[i],HEX);
Serial.print(",\t");
Serial.println(shift_latch[i],HEX);
}
Serial.println("----------------------------------------------------");
}

// modify the state of a servo
void Servotor32::changeServo(byte servo, short pos){
if(pos == 0){
pos = -1;
}
if(pos == -1){
update_registers_fast(servo, pos);
}
else{
update_registers_fast(servo, pos/10);
}
}

// // // //
short inServo = -1;
short inPos = -1;
// // // //
boolean accumulating = false;
boolean gotDigits = false;
unsigned long accumulator = ~0L;

void startAccumulating() {
accumulating = true;
gotDigits = false;
accumulator = 0;
}
void stopAccumulating() {
accumulating = false;
}
void accumulateDigit(char digit) {
if (accumulating) {
gotDigits = true;
accumulator = (accumulator * 10) + (digit - ‘0’);
}
}
boolean isAccumulatorInRange(const unsigned long min, const unsigned long max) {
return (min <= accumulator) && (max >= accumulator);
}

boolean haveValidServo = false;
boolean haveValidPos = false;
boolean acquiringPos = false;
//short servoNum = -1;
//short servoPos = -1;
short multiPingCnt = DEFAULT_MULTI_PING_CNT;
short arcScanPingCnt = DEFAULT_SCAN_PING_CNT;

/**

  • @brief Acquires a new servo number.

  • If not told to acquire a value, or no digits were supplied, then there is

  • no new number to acquire. When no new number is detected, the previous

  • servo number is used.

  • When the supplied number is not valid, an error condition is set up to be

  • reported by the caller.
    */
    void acquireServo() {
    if (accumulating) { // told to acquire

     stopAccumulating();
    
     if (gotDigits) { // digits were supplied
    
         haveValidServo = isAccumulatorInRange(0, 31);
    
         if (haveValidServo) {
             inServo = (short)accumulator;
         }
         else {
             inServo = -1;
         }
     }
    

    }
    }

/**

  • @brief Acquires a new servo position.

  • If not told to acquire a value, or no digits were supplied, then there is

  • no new posiitopn to acquire. When no new positiom is detected, the

  • previous position is used.

  • When the supplied position is not valid, an error condition is set up to

  • be reported by the caller.
    */
    void acquirePos() {
    if (accumulating)
    {
    stopAccumulating();
    haveValidPos = isAccumulatorInRange(500, 2500);

     if (haveValidPos) {
         inPos = (short)accumulator;
     }
     else {
         inPos = -1;
     }
    

    }
    }

// measure distances over a 180 degree arc and save them into the array arcPingDistances with size ARCPING_STEPS
void Servotor32::arcPing() {

float step = ((float)(MAX_POS - MIN_POS)) / (arcScanPingCnt - 1);

for (int i = 0; i < arcScanPingCnt; i++)
{
    int pos = ((int)(step * i)) + MIN_POS;
    changeServo(PING_SERVO, pos);
    if (i == 0)
    {
        delay_ms(300);
    }
    else
    {
        delay_ms(500 / arcScanPingCnt);
    }
    float d = multiPing(multiPingCnt);
    Serial.println(d);
    Serial1.println(d);
}
changeServo(PING_SERVO, CENTER_POS);
delay_ms(400);
changeServo(PING_SERVO, KILL_POS);

}

// // // //

void Servotor32:: processChar(char inChar){
switch(inChar){
case ‘#’:
startAccumulating();
acquiringPos = false;
break;
case ‘D’:
printStatus();
break;
case ‘P’:
acquireServo();
startAccumulating();
acquiringPos = true;
break;
case ‘\r’:
case ‘\n’:
// do nothing when not in “acquiring position” state.
if (acquiringPos) {
acquiringPos = false;
acquirePos();
if (!haveValidServo) {
Serial.println(“Error: Bad Servo”);
Serial1.println(“Error: Bad Servo”);
}
else if (!haveValidPos) {
Serial.println(“Error: Bad Position”);
Serial1.println(“Error: Bad Position”);
}
else {
#ifdef DEBUG_PROC_CHAR
Serial.print(“changeServo(”);
Serial.print(inServo);
Serial.print(", “);
Serial.print(inPos);
Serial.println(”)");
#endif
changeServo(inServo,inPos);
haveValidPos = false;
}
}
break;
case ‘V’:
Serial.println(“SERVOTOR32_v2.0a”);
Serial1.println(“SERVOTOR32_v2.0a”);
break;
case ‘C’:
for(int i=0; i<32; i++){
changeServo(i, 1500);
}
Serial.println(“All Centered”);
Serial1.println(“All Centered”);
break;
case ‘K’:
for(int i=0; i<32; i++){
changeServo(i,-1);
}
Serial.println(“All Turned Off”);
Serial1.println(“All Turned Off”);
break;
case ‘L’:
acquireServo();
if (haveValidServo) {
#ifdef DEBUG_PROC_CHAR
Serial.print(“changeServo(”);
Serial.print(inServo);
Serial.println(", -1)");
#endif
changeServo(inServo, -1);
Serial.println(“Servo Turned Off”);
Serial1.println(“Servo Turned Off”);
}
else {
Serial.println(“Error: Bad Servo.”);
Serial1.println(“Error: Bad Servo.”);
}
break;

case ‘m’:
if (accumulating) // told to acquire
{
stopAccumulating();

      if (gotDigits) // digits were supplied
      {
          if (1 >= accumulator)
          {
              multiPingCnt = 1; // use min valid value
          }
          else if (MAX_MULTI_PING_CNT <= accumulator)
          {
              multiPingCnt = MAX_MULTI_PING_CNT; // use max valid value
          }
          else
          {
              multiPingCnt = (short)accumulator;
          }
      }
      else
      {
          // no digits supplied, return to default
          multiPingCnt = DEFAULT_MULTI_PING_CNT;
      }
  }
  Serial.print("Multi-ping count = ");
  Serial1.print("Multi-ping count = ");
  Serial.println(multiPingCnt);
  Serial1.println(multiPingCnt);
  break;

case ‘s’:
if (accumulating) // told to acquire
{
stopAccumulating();

      if (gotDigits) // digits were supplied
      {
          if (2 >= accumulator)
          {
              arcScanPingCnt = 2; // use min valid value
          }
          else if (MAX_SCAN_PING_CNT <= accumulator)
          {
              arcScanPingCnt = MAX_SCAN_PING_CNT; // use max valid value
          }
          else
          {
              arcScanPingCnt = (short)accumulator;
          }
      }
      else
      {
          // no digits supplied, return to default
          arcScanPingCnt = DEFAULT_SCAN_PING_CNT;
      }
  }
  Serial.print("Scan-ping count = ");
  Serial1.print("Scan-ping count = ");
  Serial.println(arcScanPingCnt);
  Serial1.println(arcScanPingCnt);
  break;

case ‘M’:
{
float d = multiPing(multiPingCnt);
Serial.println(d);
Serial1.println(d);
}
break;
case ‘S’:
arcPing();
break;

default:
if ((inChar >= ‘0’) && (inChar <= ‘9’)) {
accumulateDigit(inChar);
}
break;
}
}

#define MAX_TIME 1000000

float Servotor32::ping(){
//PB0 for Trigger (17)
//PB7 for Echo (11)

pinMode(17,OUTPUT);
pinMode(11,INPUT);

long duration;
float cm;
digitalWrite(17, LOW);
delayMicroseconds(2);
digitalWrite(17, HIGH);
delayMicroseconds(10);
digitalWrite(17, LOW);

uint8_t bit = digitalPinToBitMask(11);
uint8_t port = digitalPinToPort(11);
uint8_t stateMask = (HIGH ? bit : 0);

unsigned long startCount = 0;
unsigned long endCount = 0;
unsigned long width = 0; // keep initialization out of time critical area

// convert the timeout from microseconds to a number of times through
// the initial loop; it takes 16 clock cycles per iteration.
unsigned long numloops = 0;
unsigned long maxloops = 500;

// wait for any previous pulse to end
while ((*portInputRegister(port) & bit) == stateMask)
if (numloops++ == maxloops)
return 0;

// wait for the pulse to start
while ((*portInputRegister(port) & bit) != stateMask)
if (numloops++ == maxloops)
return 0;

startCount = micros_new();
// wait for the pulse to stop
while ((*portInputRegister(port) & bit) == stateMask) {
if (numloops++ == maxloops)
return 0;
delayMicroseconds(10); //loop ‘jams’ without this
if((micros_new() - startCount) > 58000 ){ // 58000 = 1000CM
return 0;
break;
}
}
duration = micros_new() - startCount;
//--------- end pulsein
cm = (float)duration / 29.0 / 2.0;
return cm;
}

float Servotor32::multiPing(unsigned short attempts=DEFAULT_MULTI_PING_CNT){
float distances [attempts];

for (int i=0; i<attempts; i++) {
    distances[i] = ping();
}

int i, j;

if (1 < attempts) {
    float temp;

    // sort them in order
    for (i = (attempts - 1); i > 0; i--)
    {
        for (j = 1; j <= i; j++)
        {
            if (distances[j-1] > distances[j])
            {
                temp = distances[j-1];
                distances[j-1] = distances[j];
                distances[j] = temp;
            }
        }
    }
    // choose the middle entry
    i = (int)ceil((float)attempts/2.0);
}
else {
    i = 0;
}
return distances[i];

}[/code]

Edit TooManySecrets:
Please use code brackets when adding code into a post. Thx

Thanks for the edit.
I didn’t notice the “code” control until after I saw the post and then I didn’t know that I could repair it.

This is a little embarrassing, I’m getting an exception thrown from servotorComm.py when I include the read from serial code that was mentioned in michal’s post and I can’t find the clues to resolve it in PySerial’s documentation…

When I include the mods to servotorComm.py, I get the following exception from Python:

[quote] File “PoMoCo/servotorComm.py”, line 72, in run
if self.ser.readable():
AttributeError: ‘NoneType’ object has no attribute ‘readable’
[/quote]

This suggests to me that .readable() isn’t part of the Serial object. Does this work for others without modification? I’m running Ubuntu (Linux), but I don’t see that this should make a difference in the Python environment… Any thoughts/ tips greatly appreciated.

[quote=“rpcook”] … I get the following exception from Python:

[quote] File “PoMoCo/servotorComm.py”, line 72, in run
if self.ser.readable():
AttributeError: ‘NoneType’ object has no attribute ‘readable’
[/quote][/quote]

I get the same exception when no serial communication is established (USB or Bluetooth). Do you get confirmation of connection before the error raises?

michal, you’re absolutely right, I had not been connected to Hexy when those exceptions were coming. No exception when the serial port is open. Just need to charge up my batteries before I can get going on it again now!

Update: fresh batteries; success! I now have distances being passed back to the Python. Many thanks all.

Got a nice little set of code now working which allows Hexy to look about and then move based on what he can see. What I can’t fathom is how to set this running as a loop that I can interrupt from a GUI. Hoping to have something like a dialogue box saying “Hexy is exploring, press button to stop” while the code loop runs in the background. I can’t use except KeyboardInterrupt: as CTRL+C kills the main GUI thread when your focus is on the console…

Thanks all

I don’t know how exactly you call the loop, but user interrupt is very easy. Just in each cycle test some parameter for True/False and if False do not continue.
The parameter can be changed by 2 buttons:

def Init(...):
   ...
   Button(self.frame, text="Start Exploring", command=self.startExploring).grid()
   Button(self.frame, text="Stop Exploring", command=self.stopExploring).grid()
   self.bExplore = False
   ...

def startExploring(self):
   self.bExplore = True

def stopExploring(self):
   self.bExplore = False

def poll(self):
      ...
      if self.bExplore:
            autonomousMode (....)

     self.master.after(1000/FPS, self.poll)

if you run it in a separate thread you can pass bExplore as a parameter and in case of False stop the loop

Thanks all for the help. Does anyone have any experience with the ArcPing command being very slow? I think this might be a function of low batteries, but some of my recent tests the movement of Hexy’s head has been very slow, seemingly pausing randomly within the movement, which causes the code to time out. Usually it will complete in ~0.5s and pass back a vector of distances, but these pauses can push it to several seconds.

How many steps you use in arcPing? I use 9, which takes about 2 seconds to perform. The default was 90, I think, which is way to many.

#define ARCPING_STEPS 9 
  1. Same software load on the Servotor32 that you published, michal.

I’ll try to document the slow scans when I next get back on with Hexy. Trying to write up a blog post summarising this part of my learning-curve with Hexy, should have a video of Hexy exploring after this weekend :slight_smile:

Progress, of sorts.

I’ve published what I’ve done to make this stuff work on my blog, hopefully should be enough for others to replicate Hexy exploring autonomously.

Unfortunately I’ve not managed to make a video showing off Hexy’s new skills because he’s doing his funny slow ArcPing thing again. Managed to catch the offending behaviour on camera. Any thoughts? Fully charged set of AA batteries, possibly old 9V on the logic circuit, need to go to the shops to get another one to confirm that though :frowning:

This slow ArcsPing is strange. Once you call the function, it should be handled internally by Servotor32, hence independent of any serial communication until the whole scan is done. I have only one idea - that the ping function itself produces delay. Do you get reasonable output when you use multiPing()function? If you copied the code from me it might be that I messed up something in the released code. Check it against this one. I remember that I played with the ping() and mutiPing() functions, trying to optimize it without any success. So I reversed the code back, but it might be that I introduced some errors.