3pi - Spinning Line Follower

vegewell

Pololu 3pi Robot 能高速旋轉是兩輪不同轉向,一個輪子正轉,另一個反轉,
在高速旋轉時又能移動,必定是其中一個輪子轉慢一點,

旋轉時QTR-RC reflectance sensors (left to right, x5)感受到黑線,一圈是兩次,
感受到黑線第一次是前線,
一開始 變數 x = 零, 每感受到黑線一次 x + 1,
於是 x =0,2,4,6.....是前線,
x =1,3,5,7.....是後線(不於理會),
即使在旋轉時 ,五個sensors 感受到前黑線,只要黑線在中間,便能得到固定模式的電壓值,
若值有改變,則表示偏離黑線,
因此增減左右輪的速度,調至中間,
調整時, 輪子在極速的旋轉時,總是其中一個輪子轉慢一點,

vegewell

由於程式使用到
Pololu AVR library
#include <pololu/3pi.h>
不是玩家,也無須多看,

vegewell

作者已經很大方的公佈程式碼
http://forum.pololu.com/viewtopic.php?t=1371#p6306
質問場合、彼求
======================
// Spinning Line Following code for the Pololu 3pi -- Byon Garrabrant - byon @ byon . com
// Ramp each wheel from MIN_SPEED to MAX_SPEED and back as it spins so it will translate along the line.
// Assumes the sensors have already been calibrated.

#include <pololu/3pi.h>

#define   FORWARD_OFFSET   0xA0            // Offset (0..255) of forward from the the front line
#define   MAX_SPEED      255               // Maximum speed the wheels will go
#define   MIN_SPEED      200               // Minimum speed the wheels will go

void Spinning_Line_Follow( void )
{
   unsigned short phase_start = get_ms();   // Start time of this rotation
   unsigned short last_phase_len = 100;   // Duration of the last rotation
   char last_line_side = 0;            // which side was the line on?
   char line_count = 0;               // Is this the front or back line?
   char led_duration = 0;               // How much longer should the LED be on

   while ( 1 ) {
      unsigned short cur_time = get_ms();   // Grab the current time in ms
      unsigned int sensors[5];         // Is the line left or right?
      char line_side = (read_line(sensors,IR_EMITTERS_ON) < 2000);   
      left_led( 0 );                  // Turn off the "FRONT" LED
      if (line_side & !last_line_side) {   // If it just changed,
         if ( ++line_count & 1 ) {      // and if this is the front line
            left_led( 1 );            // Turn on "FRONT" LED
            last_phase_len = cur_time - phase_start;// save the last rotation duration
            phase_start = cur_time;      // and start counting this rotation
         }
      }
      last_line_side = line_side;         // Remember where the line was

      unsigned short cur_phase = cur_time - phase_start;   // How far are we into the curent rotation?
      cur_phase <<= 8;               // Multipy by 256
      cur_phase /= last_phase_len;      // based on the last rotation duration
      cur_phase += FORWARD_OFFSET;      // offset by which direction is "FORWARD"
      short left = cur_phase & 0xFF;      // Wrap back to 0 .. 255
      if ( left >= 128 ) {            // Convert to 0 .. 127 .. 0
         left = 256 - left;
      }
      left = (((left * (MAX_SPEED - MIN_SPEED))>>7) + MIN_SPEED);   // Scale the wheel speed to be MIN at 0, MAX at 127
      short right = MAX_SPEED + MIN_SPEED - left;   // the right is 180 degress out of phase from the left
      set_motors(left, -right);         // and the right goes backwards
   }
}
Byon

Posts: 2
Joined: Thu Feb 26, 2009 4:34 pmTop
--------------------------------------------------------------------------------