Ultrasonic Distance Sensor/ Module

Post all your hardware and software upgrades/projects here.

Ultrasonic Distance Sensor/ Module

Postby rpcook » Wed Jul 10, 2013 9:46 pm

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):
  • 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?
Thanks all.
Rob
A record of my exploits and musings with Hexy and Sparki: http://robcook.eu, content available under a Creative Commons license.
rpcook
 
Posts: 121
Joined: Tue Sep 04, 2012 6:42 pm
Location: Luton, UK

Re: Ultrasonic Distance Sensor/ Module

Postby michal » Thu Jul 11, 2013 5:06 pm

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:

Code: Select all
#define ARCPING_STEPS 9


Then in servotorComm.py it is important to uncomment the following lines:
Code: Select all
            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.
Code: Select all
...
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
Code: Select all
        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...
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Ultrasonic Distance Sensor/ Module

Postby mrmacro » Fri Jul 12, 2013 1:43 am

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?
mrmacro
 
Posts: 12
Joined: Fri Jul 12, 2013 12:16 am

Re: Ultrasonic Distance Sensor/ Module

Postby mrmacro » Sat Jul 13, 2013 1:04 pm

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:

#<number>m Sets the number of measurements by multi-ping
#<number>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.
mrmacro
 
Posts: 12
Joined: Fri Jul 12, 2013 12:16 am

Re: Ultrasonic Distance Sensor/ Module

Postby michal » Sat Jul 13, 2013 3:33 pm

that could work. You can make a code and post it here
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Ultrasonic Distance Sensor/ Module

Postby roboalchemist » Sat Jul 13, 2013 4:47 pm

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.
---
Joseph Schlesinger - ArcBotics Co-Founder/CTO
roboalchemist
Site Admin
 
Posts: 671
Joined: Mon Jul 16, 2012 3:02 am

Re: Ultrasonic Distance Sensor/ Module

Postby mrmacro » Mon Jul 15, 2013 2:03 am

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 #<number><functional number identifier> 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.
mrmacro
 
Posts: 12
Joined: Fri Jul 12, 2013 12:16 am

Re: Ultrasonic Distance Sensor/ Module

Postby mrmacro » Fri Jul 19, 2013 9:14 pm

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 "#31p1500<enter>L" 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: Select all
#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];
}


Edit TooManySecrets:
Please use code brackets when adding code into a post. Thx
mrmacro
 
Posts: 12
Joined: Fri Jul 12, 2013 12:16 am

Re: Ultrasonic Distance Sensor/ Module

Postby mrmacro » Sat Jul 20, 2013 1:55 pm

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.
mrmacro
 
Posts: 12
Joined: Fri Jul 12, 2013 12:16 am

Re: Ultrasonic Distance Sensor/ Module

Postby rpcook » Sat Jul 20, 2013 5:53 pm

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:
File "PoMoCo/servotorComm.py", line 72, in run
if self.ser.readable():
AttributeError: 'NoneType' object has no attribute 'readable'


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.
Rob
A record of my exploits and musings with Hexy and Sparki: http://robcook.eu, content available under a Creative Commons license.
rpcook
 
Posts: 121
Joined: Tue Sep 04, 2012 6:42 pm
Location: Luton, UK

Next

Return to Projects and Upgrades

Who is online

Users browsing this forum: No registered users and 2 guests

cron