Shop OBEX P1 Docs P2 Docs Learn Events
measuring speed of the ActivityBot — Parallax Forums

measuring speed of the ActivityBot

BlackSoldierBBlackSoldierB Posts: 45
edited 2014-04-17 03:53 in Learn with BlocklyProp
What is the best way to calculate the speed of the ActivityBot?

My idea was to measure the time between two edges of the encoder.

This is my code:
 #include "simpletools.h"
 #include "abdrive.h"

 #ifdef __PROPELLER_USE_XMM__
 #define STACK_LONGS (1024+128+32+sizeof(_thread_state_t))/4
 #else
 #define STACK_LONGS (16+sizeof(_thread_state_t))/4
 #endif
  
 void calculateSpeed(void*);
  
 volatile static unsigned int timeInBetween;

 int main()
 {
     low(26);
     low(27);
     print("Starting\n");

     unsigned int stack[STACK_LONGS + 25];
     int cog = cogstart(&calculateSpeed, NULL, stack, sizeof(stack));    
     print("Started cog %d\n", cog);

     drive_speed(64,64);
  
     while(1)
     {
         print("dTime= %u\n", timeInBetween / (CLKFREQ/1000));
         pause(100);
     }
 }
  
 void calculateSpeed(void*)
 {
     unsigned int prevTime;
  
     while(1)
     {
         prevTime = CNT; //Remember the current time

         while(!input(14)); //wait when pin 14 is low 

         timeInBetween = CNT - prevTime; //Calculate the difference in time

         while(input(14)); //wait when pin 14 is high
     }
 }

The output is shown below, as you can see it deviates quite a bit.
I expect times around 15 milliseconds, is my method not accurate enough?
 dTime= 17
 dTime= 15
 dTime= 17
 dTime= 15
 dTime= 22
 dTime= 18
 dTime= 19
 dTime= 23
 dTime= 18
 dTime= 17
 dTime= 17
 dTime= 15

Maybe i can use the encoder function from the abdrive library, but it's really hard to understand with all the short variable names.

Does anyone have some tips for me?

Comments

  • Andy Lindsay (Parallax)Andy Lindsay (Parallax) Posts: 1,919
    edited 2014-04-02 16:20
    I think your code might only be measuring one spoke's passage. If you measure the passage of a spoke and a space between spokes, you will have a full cycle, which should improve the measurement. Averaging the passage of a few spokes will also help.
  • BlackSoldierBBlackSoldierB Posts: 45
    edited 2014-04-14 03:12
    I took a look at the "encoder(void *par)" function and it uses a "td" variable.

    Can i just divide the "abd_dlca" or "abd_dlc" with the "td" variable to get the correct speed?

    A further improvement is to calculate the average over multiple samples.

    EDIT: How do i use the "interactive_development_mode" function? My xbee pins are 9 and 8.
  • Andy Lindsay (Parallax)Andy Lindsay (Parallax) Posts: 1,919
    edited 2014-04-15 18:11
    Sorry for the slow reply. You are very close; those particular variables calculate target distances. The measured distances are stored in abd_ticksL and abd_ticksR. The drive_getTicks function will copy those values into your main file's variables so that your code can take snapshots as needed. Using this for calculating speed could go something like this:
    int distLeftInit, distRightInit, distLeftFinal, distRightFinal, velocityLeft, velocityRight;
...
drive_getTicks(&distLeftInit, &distRightInit);
pause(100);
drive_getTicks(&distLeftFinal, &distRightFinal);

velocityLeft = (distLeftFinal - distLeftInit) * 10;
velocityRight = (distRightFinal - distRightInit) * 10;

    This particular measurement is in ticks per second, because the sample is over 1/10 second, and the distance traveled is multiplied by 10. At constant speeds, a larger sample time will reduce measurement uncertainty, but it also takes longer to measure. Your code can also add details like waiting for a transition before starting the measurement to reduce measurement error.

    Ah, so you found the hidden development tools. Uh, they're kinda broken. ...but I'll bet there's some arlodrive test code that'll work well for you if harvested and adjusted for the ActivityBot. Let me go hunt that down. Be back in a while...
  • Andy Lindsay (Parallax)Andy Lindsay (Parallax) Posts: 1,919
    edited 2014-04-15 19:19
    Alright, so here is an ActivityBot app that takes navigation instructions from a TV remote, and sends abdrive info to a terminal over an XBee connection. It has some code I was experimenting with as a possible replacement for abdrive's interactive development mode. I'll probably add drive_getSpeedCalc (last function) as well.

    Circuits, libraries, and test code examples for TV remote, XBee, and ActivityBot are here:
      TV Remote
    http://learn.parallax.com/propeller-c-simple-devices/ir-receiver-and-remote
      XBee
    http://learn.parallax.com/propeller-c-simple-protocols/full-duplex-serial
      ActivityBot
    http://learn.parallax.com/activitybot
    When they are all verified to work, try the code below.

    Note: The comments in the code have more info on remote control buttons and the values in the XBee display.
    /*
  XBee displays serial updates of calculated and traveled
  ticks with error info.

  XBee DO -> P9
  IR receiver -> P10

  Use XBee + USB adapter or XStick for PC connection.
  Set PC terminal to 9600 bps.

  Use 2 on TV remote to go 256 ticks forward at top speed
  Use 8 on TV remote to go 256 ticks backward at top speed

  CH+ -> forward, tap for slow, hold to go to full speed.
  CH- -> backward, tap for slow, hold to go to full speed.

  VOL- -> left rotate, tap for slow, hold to go to full speed. 
  VOL+ -> right rotate, tap for slow, hold to go to full speed. 
  
  Variables displayed by XBee:
  
  - i    index
  - eL   error Left (difference between calculated and measured ticks)
  - eL   error Right
  - LSC  Left Speed Calculated
  - LTC  Left Ticks Calculated
  - LT  Left Ticks (Measured)
  - RSC  Right Speed Calculated
  - RTC  Right Ticks Calcualted
  - RT   Right Ticks (measured)
*/                

#include "simpletools.h"
#include "wavplayer.h"
#include "sirc.h" 
#include "ping.h"             
#include "abdrive.h"
#include "fdserial.h"

int key;                                // Global var for remote key
int ticksLeft, ticksLeftCalc, ticksRight, ticksRightCalc; 
int ticksLeftOld, ticksLeftCalcOld, ticksRightOld, ticksRightCalcOld; 
int t, dt, i = 0;
volatile int running = 0;
static int speedLeft, speedRight;

fdserial *xbee;

void gofwd(void *par);
static int fstack[128];
void gobkwd(void *par);
static int bstack[128];

void getTicks();
void displayTicks();
void drive_getSpeedCalc(int *left, int *right);

int main()                              // Main - execution begins!
{
  xbee = fdserial_open(9, 8, 0, 9600);

  drive_speed(0,0);                     // Start servos/encoders cog
  drive_setRampStep(10);                // Set ramping at 10 ticks/sec per 20 ms
  sirc_setTimeout(50);                  // Remote timeout = 50 ms

  dt = CLKFREQ/10;
  t  = CNT;

  while(1)                               // Outer loop
  {
    int button = sirc_button(10);        // check for remote key press

    // Motion responses - if key pressed, set wheel speeds
    if(button == 2)
    {
      if(!running) 
        cogstart(&gofwd, NULL, fstack, sizeof fstack);
    }
    if(button == 8)
    {
      if(!running) 
        cogstart(&gobkwd, NULL, bstack, sizeof bstack);
    }
    if(button == CH_UP)drive_rampStep(128, 128); // Left turn      
    if(button == CH_DN)drive_rampStep(-128, -128); // Right turn 
    if(button == VOL_DN)drive_rampStep(-128, 128); // Left turn      
    if(button == VOL_UP)drive_rampStep(128, -128); // Right turn 
    if(button == MUTE)drive_rampStep(0, 0);        // Stop 
    
    if(button == ENTER)
    {
      getTicks();
      displayTicks();
    }

    if(CNT - t > dt)
    {
      t+=dt;
      i++;
      getTicks();
      if
      (
           ticksLeftCalc  != ticksLeftCalcOld 
        || ticksRightCalc != ticksRightCalcOld
        || ticksLeft      != ticksLeftOld
        || ticksRight     != ticksRightOld
      )
      {
        displayTicks();
      }
    }
  }
}

void gofwd(void *par)
{
  running = 1;
  drive_goto(256, 256);
  running = 0;
  cogstop(cogid());
}

void gobkwd(void *par)
{
  running = 1;
  drive_goto(-256, -256);
  running = 0;
  cogstop(cogid());
}


void getTicks(void)
{
  ticksLeftOld = ticksLeft;
  ticksRightOld = ticksRight;
  ticksLeftCalcOld = ticksLeftCalc;
  ticksRightCalcOld = ticksRightCalc;      
  drive_getTicks(&ticksLeft, &ticksRight);
  drive_getTicksCalc(&ticksLeftCalc, &ticksRightCalc);      
  drive_getSpeedCalc(&speedLeft, &speedRight);
}

void displayTicks(void)
{
  writeStr(xbee, "i = ");
  writeDec(xbee, i);
  writeStr(xbee, " eL = ");
  writeDec(xbee, ticksLeftCalc - ticksLeft);
  writeStr(xbee, " eR = ");
  writeDec(xbee, ticksRightCalc - ticksRight);
  writeStr(xbee, "  LSC = ");
  writeDec(xbee, speedLeft);
  writeStr(xbee, "  LTC = ");
  writeDec(xbee, ticksLeftCalc);
  writeStr(xbee, " LT = ");
  writeDec(xbee, ticksLeft);
  writeStr(xbee, "  RSC = ");
  writeDec(xbee, speedRight);
  writeStr(xbee, "   RTC = ");
  writeDec(xbee, ticksRightCalc);
  writeStr(xbee, " RT = ");
  writeDec(xbee, ticksRight);
  writeChar(xbee, '\r');
}


volatile int abd_speedL;
volatile int abd_speedR;

void drive_getSpeedCalc(int *left, int *right)
{
  *left  = abd_speedL;
  *right = abd_speedR;
}
  • BlackSoldierBBlackSoldierB Posts: 45
    edited 2014-04-16 05:30
    Thanks, i don't have a TV remote, but I will use a cog to test different settings and speeds.

    The calculated speed is the used as setpoint for the PID controller right?

    With the following code i am trying to calculate the the time that has passed between two spokes, but for some reasons it fluctuates a lot.
    void watch_for_ticks(void *par) 
{
     lastTimeL = CNT;
     deltaTimeL = 0;

     lastTimeR = CNT;
     deltaTimeR = 0;

     lastStateL = 0;
     lastStateR = 0;
 
     while(1)
     {       
         if( input(14) != lastStateL ) //Left change
         {
             deltaTimeL = CNT - lastTimeL;
             lastTimeL = CNT;
             //print("deltaTimeL=%d lastStateL=%d\n", deltaTimeL, lastStateL);
         }

         if( input(15) != lastStateR ) //Right change
         {
             deltaTimeR = CNT - lastTimeR;
             lastTimeR = CNT;
             //print("deltaTimeR=%d, lastStateR=%d\n", deltaTimeR, lastStateR);
         }
 
         lastStateL = input(14);
         lastStateR = input(15);
     }    
 }

    Then i print the result every second with the following line: 
    print("lastTimeL= %f ms, lastTimeR= %f ms\n", deltaTimeL / (CLKFREQ/1000.0), deltaTimeR / (CLKFREQ/1000.0)); //Time difference in ms
  • Andy Lindsay (Parallax)Andy Lindsay (Parallax) Posts: 1,919
    edited 2014-04-16 16:41
    The abdrive control system knows what speed you want in encoder ticks per second, and it uses distance = velocity x time to repeatedly recalculates how many encoder ticks should have been measured. The control system makes adjustments to how fast it tells the servo motors to go based on the difference between calculated ticks (like LTC) and measured ticks (LT).

    I think that updating lastStateL and lastStateR outside of the if statements is going to lead to incorrect measurements. After that gets fixed, the measured speed may still be somewhat error prone because the time that it takes a spoke to pass by vs a space to pass by will vary with the distance between the wheel and the sensor. If the wheel has any wobble in it, the spoke time might be 60% of a cycle, and the space time might only be 40% when the wheel is closer to the sensor. When it's further away, the times might be the other way around. So, it would probably also help to take a full cycle (time of spoke + space) or even the average of several cycles.

    Check the speedMonitor function in this example. It keeps track of speed based on a cycle (spoke + space).
    /*
  XBee displays serial updates of calculated and traveled
  ticks with error info.

  XBee DO -> P9
  IR receiver -> P10

  Use XBee + USB adapter or XStick for PC connection.
  Set PC terminal to 9600 bps.

  Use 2 on TV remote to go 256 ticks forward at top speed
  Use 8 on TV remote to go 256 ticks backward at top speed

  CH+ -> forward, tap for slow, hold to go to full speed.
  CH- -> backward, tap for slow, hold to go to full speed.

  VOL- -> left rotate, tap for slow, hold to go to full speed. 
  VOL+ -> right rotate, tap for slow, hold to go to full speed. 
  
  Variables displayed by XBee:
  
  - i        index
  - eL       error Left (difference between calculated and measured ticks)
  - eL       error Right
  - LSC      Left Speed Calculated
  - LTC      Left Ticks Calculated
  - LT       Left Ticks (Measured)
  - RSC      Right Speed Calculated
  - RTC      Right Ticks Calcualted
  - RT       Right Ticks (measured)
  - tCycleL  Left time for spoke and space to pass.
  - tCycleR  Right time for spoke and space to pass.
  - vL       Velocity left based on spoke + space time.
  - vR       Velocity right based on spoke + space time.
*/                

#include "simpletools.h"
#include "wavplayer.h"
#include "sirc.h" 
#include "ping.h"             
#include "abdrive.h"
#include "fdserial.h"

int key;                                // Global var for remote key
int ticksLeft, ticksLeftCalc, ticksRight, ticksRightCalc; 
int ticksLeftOld, ticksLeftCalcOld, ticksRightOld, ticksRightCalcOld; 
int t, dt, i = 0;
volatile int running = 0;
static int speedLeft, speedRight;

fdserial *xbee;

void gofwd(void *par);
static int fstack[128];
void gobkwd(void *par);
static int bstack[128];

void speedMonitor(void *par);
static int speedstack[128];
static volatile int tCycleL, tCycleR, vL, vR; 
 
void getTicks();
void displayTicks();
void drive_getSpeedCalc(int *left, int *right);

int main()                              // Main - execution begins!
{
  xbee = fdserial_open(9, 8, 0, 9600);

  drive_speed(0,0);                     // Start servos/encoders cog
  drive_setRampStep(10);                // Set ramping at 10 ticks/sec per 20 ms
  sirc_setTimeout(50);                  // Remote timeout = 50 ms

  cogstart(&speedMonitor, NULL, speedstack, sizeof speedstack);

  dt = CLKFREQ/10;
  t  = CNT;

  while(1)                               // Outer loop
  {
    int button = sirc_button(10);        // check for remote key press

    // Motion responses - if key pressed, set wheel speeds
    if(button == 2)
    {
      if(!running) 
        cogstart(&gofwd, NULL, fstack, sizeof fstack);
    }
    if(button == 8)
    {
      if(!running) 
        cogstart(&gobkwd, NULL, bstack, sizeof bstack);
    }
    if(button == CH_UP)drive_rampStep(128, 128); // Left turn      
    if(button == CH_DN)drive_rampStep(-128, -128); // Right turn 
    if(button == VOL_DN)drive_rampStep(-128, 128); // Left turn      
    if(button == VOL_UP)drive_rampStep(128, -128); // Right turn 
    if(button == MUTE)drive_rampStep(0, 0);        // Stop 
    
    if(button == ENTER)
    {
      getTicks();
      displayTicks();
    }

    if(CNT - t > dt)
    {
      t+=dt;
      i++;
      getTicks();
      if
      (
           ticksLeftCalc  != ticksLeftCalcOld 
        || ticksRightCalc != ticksRightCalcOld
        || ticksLeft      != ticksLeftOld
        || ticksRight     != ticksRightOld
      )
      {
        displayTicks();
      }
    }
  }
}

void gofwd(void *par)
{
  running = 1;
  drive_goto(256, 256);
  running = 0;
  cogstop(cogid());
}

void gobkwd(void *par)
{
  running = 1;
  drive_goto(-256, -256);
  running = 0;
  cogstop(cogid());
}


void getTicks(void)
{
  ticksLeftOld = ticksLeft;
  ticksRightOld = ticksRight;
  ticksLeftCalcOld = ticksLeftCalc;
  ticksRightCalcOld = ticksRightCalc;      
  drive_getTicks(&ticksLeft, &ticksRight);
  drive_getTicksCalc(&ticksLeftCalc, &ticksRightCalc);      
  drive_getSpeedCalc(&speedLeft, &speedRight);
}

void displayTicks(void)
{
  writeStr(xbee, "i = ");
  writeDec(xbee, i);
  writeStr(xbee, " eL = ");
  writeDec(xbee, ticksLeftCalc - ticksLeft);
  writeStr(xbee, " eR = ");
  writeDec(xbee, ticksRightCalc - ticksRight);
  writeStr(xbee, "  LSC = ");
  writeDec(xbee, speedLeft);
  writeStr(xbee, "  LTC = ");
  writeDec(xbee, ticksLeftCalc);
  writeStr(xbee, " LT = ");
  writeDec(xbee, ticksLeft);
  writeStr(xbee, "  RSC = ");
  writeDec(xbee, speedRight);
  writeStr(xbee, "  RTC = ");
  writeDec(xbee, ticksRightCalc);
  writeStr(xbee, " RT = ");
  writeDec(xbee, ticksRight);
  writeChar(xbee, '\r');
  writeStr(xbee, "tCycleL = ");
  writeDec(xbee, tCycleL);
  writeStr(xbee, "  tCycleR = ");
  writeDec(xbee, tCycleR);
  writeStr(xbee, "  vL = ");
  writeDec(xbee, vL);
  writeStr(xbee, "  vR = ");
  writeDec(xbee, vR);
  writeStr(xbee, "\n\n");
}


volatile int abd_speedL;
volatile int abd_speedR;

void drive_getSpeedCalc(int *left, int *right)
{
  *left  = abd_speedL;
  *right = abd_speedR;
}

void speedMonitor(void *par) 
{
  int xL, xR;
  drive_getTicks(&xL, &xR); 
  int xLprev = xL;
  int xRprev = xR;
  
  int cntL = CNT;
  int cntR = CNT;

  int cntLprev = cntL;
  int cntRprev = cntR;

  while(1)
  {
    drive_getTicks(&xL, &xR); 
    if(xL == xLprev + 2 || xL == xLprev - 2)
    {
      cntL = CNT;
      tCycleL = cntL - cntLprev;
      if(xLprev > xL) tCycleL = -tCycleL;
      cntLprev = cntL;
      vL = (CLKFREQ/tCycleL) * 2;
      xLprev = xL;
    }     
    if(xR == xRprev + 2 || xR == xRprev - 2)
    {
      cntR = CNT;
      tCycleR = cntR - cntRprev;
      if(xRprev > xR) tCycleR = -tCycleR;
      cntRprev = cntR;
      vR = (CLKFREQ/tCycleR) * 2;
      xRprev = xR;
    }     
  }  
}
  • BlackSoldierBBlackSoldierB Posts: 45
    edited 2014-04-17 03:53
    That works a lot better than my code. I didn't thought about the wobbling of the wheel.

    When i do drive_speed(50,50), it will vary from 48 to 52 (when printing every 100 ms). This accuracy will probably good enough for my application.
Sign In or Register to comment.