BB1-1

Hi,

BB1-1 is looking good.  Must be a torquey monster!  I like where you mounted the sensors, nice and low.

As an exercise I wrote an Arduino sketch that tried out four different avoid algorithms.  I’d be willing to post it if you think it might be useful to you.  It’s based on the cooperative scheduler that Joseph Jones writes about in his Robot Programming book.  So each subroutine gets its work done ASAP and each subroutine has a priority that the arbitrate function uses to decide the winner.  The winner get control of the differential drive motors.  Is that something that would work with BB1-1?

Tom

@thrandell  Sounds a bit like the Space Shuttle's computer voting! 

Thanks Tom,

yes, that would interest me greatly!

Have based most of the control so far on Brian’s code, see the Henry IX post, but all input is gladly accepted 😊

mel.

Wow, that took a while to format.  Some of my comments were wrapping and making it hard to read, so I took them out.  If the code isn't clear let me know and I'll try to explain what's going on.

Tom

/* An attempt to implement the Avoid behavior with the addition of a Leaky Integrator 
 *  from Jones's Robot programming book, using Orangey.
 *  Avoidance using Differential Detectors
 *  The Sharp range finders will be used as proximity sensors that signal true if an object is 
 *  within 14 inches (35cm) of the sensor and false otherwise.
 *  The IR sensor's 14 inch analog value for IR1 is 130 and for IR2 is 136.  
 *  Hopefully it will cover the part of the curve that increases when an object is closer then 3.9 inches.
 *  
 *  Each behavior should have access to the sensor values
 *  and their set of parameter values.
 *  
 *  avoid - Simple but oscillates in corners and canyons
 *  avoid_consistent - Always turns in one direction, so it doesn't explore very far
 *  avoid_ballistic - Uses state but can still get caught in corners, just with a bigger oscillation
 *  avoid_leaky_integrator - Uses state and clears corners and canyons,  might eventually 
 *                           need a tweek to randomize the angle
 *  
 */

#include <AStar32U4.h>
#include <DRV8835MotorShield.h>

// Behavior IDs
#define STOP_ID    0   // Default stop behavior
#define CRUISE_ID  1   // Move at a constant speed/rotation
#define AVOID_ID   2   // Use IRs to avoid objects
#define BOSTON_ID  3   // Do something random
#define MAX_BEH    4   // One more then the highest behavior ID
#define CONTROL    beh_ctl[behavior_id] = behavior_id
#define PUNT       beh_ctl[behavior_id] = 0

extern int sensor_arr[2] = {0,0};
extern int IRSensor1  = 2;    // left sensor, connected to the A2 pin
extern int IRSensor2  = 3;    // right sensor, connected to the A3 pin
extern int pa_left_proximity = 159;     // = 130
extern int pa_right_proximity = 165;  // = 136 
extern int pg_speed = 200;           // pg = parameter global, why is it extern??
extern int pg_motor_factor = 130;   // should there even be global parameter like this?
extern int pc_dir = 0;             // pc = parameter cruise
extern int pa_dir = 45;           // pa = parameter avoid
extern int right_vel[MAX_BEH] = {0,0,0,0};  // Only place that STOP_ID velocity is set
extern int left_vel[MAX_BEH]  = {0,0,0,0};  // Only place that STOP_ID velocity is set
extern unsigned char beh_priority[MAX_BEH] = {AVOID_ID,BOSTON_ID,CRUISE_ID,STOP_ID}; 
extern unsigned char beh_ctl[MAX_BEH] = {0,0,0,0};     

int pr_dur  = 2000;   // global variable used as a parameter for duration 
int pr_time = 32000;  // global variable used as a parameter for time to wait
int pa_dur  = 500;   // global variable used as a parameter for the timer value T.  

DRV8835MotorShield motors;
AStar32U4Buzzer buzzer;

void setup() {
  Serial.begin(9600);
  
  motors.flipM1(true);
  // motors.flipM2(true);
  //  beep - play a wake up tune
  buzzer.playNote(NOTE_B(4), 400, 15);
  while (buzzer.isPlaying()) { }
  buzzer.playNote(NOTE_D(4), 800, 15);
  while (buzzer.isPlaying()) { }
  delay(1000); 
}

void loop() {                      // The Cooperative Scheduler
  unsigned char winner;
  uint16_t batteryLevel = readBatteryMillivoltsSV();

  acquire_sensors();
  cruise();                 // parameters: speed, turn_angle
  avoid_leaky_integrator(); // parameters: left_proximity, right_proximity, turn_angle
  boston();                 // parameters: time_between_events, event_duration
  // escape();              // parameters: backup_distance, spin_distance
  winner = arbitrate();
  move_dual(winner);        // this sends the motion commands to the motors  

//  Serial.print("Battery = "); Serial.print(batteryLevel);
//  Serial.print(" Winning behavior = ");  Serial.print(winner);
//  Serial.print(" Left sensor = "); Serial.print(sensor_arr[0]); 
//  Serial.print(" Right sensor = "); Serial.print(sensor_arr[1]);
//  Serial.print(" Left Velocity = "); Serial.print(left_vel[winner]);
//  Serial.print(" Right Velocity = "); Serial.println(right_vel[winner]);
  
}

void acquire_sensors() {
  // right now this is using two Sharp IR sensors as proximity sensors, no other sensors
  // eventually add bumper, motor current, photocells, 
  // Running Average, my effort to smooth out those nasty IR readings
  int reading;
  float k = 20.0;  // this had to have a decimal to work
  float sample;

  reading = analogRead(IRSensor1);
  sample = (1/k * reading) + ((1 - 1/k) * sensor_arr[0]);
  sensor_arr[0] = sample;
  reading = analogRead(IRSensor2);
  sample = (1/k * reading) + ((1 - 1/k) * sensor_arr[1]);  
  sensor_arr[1] = sample;
 
}

void cruise() {
  unsigned char behavior_id = CRUISE_ID;

  _drive_angle(pc_dir, pg_speed, behavior_id);  // rotation, translation, behavior  
  CONTROL;
 
}

void avoid_leaky_integrator() {  // This subroutine introduces state that leaks away.
  unsigned char behavior_id = AVOID_ID;  // Timer value that increases with each switch
  unsigned int incr = 200;                   // increament duration of Timer by this amount. 
  static unsigned char test_timer = 0;       // 0 = left sensor, 1 = right sensor
  static unsigned long end_time = 0;         // of timer, same data type as millis()
  static unsigned int  duration = pa_dur;    // timer duration in milliseconds

 
 if (millis() < end_time) {  
    if (test_timer == 0) {  // the left timer is running
      _drive_angle(-pa_dir, 0, behavior_id);  // turn only one direction during interval
    }
    else if (test_timer == 1) {  // the right timer is running
     _drive_angle(pa_dir, 0, behavior_id);
    } 
  }  
  else if (sensor_arr[0] >= pa_left_proximity) {      // start timing an interval
    // increase timer each time there is a switch of which side was triggered last (oscillation)                    
    if (test_timer == 1)  duration = duration + incr;  
    test_timer = 0;
    end_time = millis() + duration;                 
    CONTROL;
  }   
  else if (sensor_arr[1] >= pa_right_proximity) {
    if (test_timer == 0)  duration = duration + incr;
    test_timer = 1;
    end_time = millis() + duration;
    CONTROL;
  }
  else {
    PUNT;   
    // leak away the duration back down to the minimum time
    if (duration > pa_dur)  duration = duration - incr;  
  }
}

void avoid_ballistic() {  // This subroutine introduces state to the avoid behavior.
  unsigned char behavior_id = AVOID_ID;
  static unsigned char test_timer = 0;             // 0 = left sensor, 1 = right sensor
  static unsigned long end_time = 0;              // time to wait before starting again.

  if (millis() < end_time) {  // a timer is running
    if (test_timer == 0) {  // the left timer is running
      _drive_angle(-pa_dir, 0, behavior_id);  // turn only one direction during interval
    }
    else if (test_timer == 1) {  // the right timer is running
     _drive_angle(pa_dir, 0, behavior_id);
    } 
  }  
  else if (sensor_arr[0] >= pa_left_proximity) {   
    test_timer = 0;
    end_time = millis() + pa_dur;                 
    beh_ctl[behavior_id] = behavior_id; 
  }   
  else if (sensor_arr[1] >= pa_right_proximity) {
    test_timer = 1;
    end_time = millis() + pa_dur;
    beh_ctl[behavior_id] = behavior_id; 
  }
  else
    PUNT;
 
} 


void avoid_consistent() {  
// This simple subroutine should not have problems with corners and box canyons, 
//  but may not explore very much.
  unsigned char behavior_id = AVOID_ID;

  if ((sensor_arr[0] >= pa_left_proximity) || (sensor_arr[1] >= pa_right_proximity)) {
    _drive_angle(pa_dir, 0, behavior_id);          
    CONTROL;
  }
  else 
    PUNT;
  
}

void avoid() {  // This simple subroutine will have problems with corners and box canyons.

  unsigned char behavior_id = AVOID_ID;

  if (sensor_arr[0] >= pa_left_proximity) {         // left sensor detected something, turn right
    _drive_angle((-1 * pa_dir), 0, behavior_id);  
    CONTROL;
  }
  else if (sensor_arr[1] >= pa_right_proximity) {   // right sensor detected something, turn left
    _drive_angle(pa_dir, 0, behavior_id);           // angle to turn, speed, behavior identifier
    CONTROL;
  }
  else 
    PUNT;
  
}

void boston(void) {    // Periodically cause the robot to swerve randomly.
  unsigned char behavior_id = BOSTON_ID;
  static int angle;                         
  static unsigned char state = 0;           
  static unsigned long del_time;             // time to wait before starting again
  static unsigned long b_time;               // begin time

  switch(state) 
  {
    // State 0 - Compute time until random event
    case 0:
      b_time = millis();                                    // starting time
      del_time = ((rand() % 11) * (unsigned int) pr_time);  // random between 0 and 10
      del_time = b_time + del_time;
      PUNT;
      state++;                                              // Go to the next state
      break;
    // State 1 - Wait for event to start
    case 1:
      if (millis() >= del_time) {                          // Done waiting ? then go Boston!
        b_time = millis();                                 // New starting time
        del_time = ((rand() % 6) * (unsigned int) pr_dur); // random between 0 and 5
        del_time = b_time + del_time;
        angle = rand()>>7;                                 // Pick a random angle
        _drive_angle(angle, pg_speed, behavior_id); 
        CONTROL;
        state++;                                           // go to the next state
      } 
      break;
    // State 2 - Random heading until timeout
    default:
      if (millis() >= del_time)
        state = 0;
      break;
  }
}

int arbitrate(void) {

  unsigned char pri_index;
  unsigned char beh_index;
  
  for (pri_index = 0; pri_index < MAX_BEH; pri_index++) {
    beh_index = beh_priority[pri_index];  // get slot number of next highest behavior
    if (beh_ctl[beh_index])               // if this behavior wants control return it as winner
      return beh_index;
  }
  return 0;                         // no behavoir wants control
}

int norm_360(int raw_angle) {
  return (raw_angle % 360);  // returns number between 0 and 359
}

int norm_180(int raw_angle) {
  return (norm_360(raw_angle + 180) - 180);  // returns number between -180 and 180
}
  
void _drive_angle(int dir, int speed, unsigned char beh_id) {
//  This bad boy takes the angle and velocity arguments and converts them to the 
//   left and right wheel velocities and writes them to the corresponding velocity 
//   arrays, right_vel[beh_id] and left_vel[beh_id].
//   A negative direction angle is a turn right, i.e. clockwise.
//   Using Jones' convention: consider Angle coordinates as positive numbers only 
//   in the range of 0 to 360; when used as a Rotation, an angle should have values 
//   between -180 and 180 degrees.
  int rot;
  
  rot = norm_180(dir);
  
  if (abs(rot) == 180) {  // a 180 seems like a special case here 
    left_vel[beh_id] = right_vel[beh_id] = -1 * speed;
  }
  else if (!speed) {  // a turn in place
    left_vel[beh_id] = ((-1 * rot) * pg_motor_factor)/100;  
    right_vel[beh_id] = (rot * pg_motor_factor)/100;
  }
  else { // speed and possibly an angle.  This needs investigation. At a certain speed and time 
         // slice what rot value gives me say 5 degrees?  No compass or encoders here.
    left_vel[beh_id] = speed + (-1 * rot);
    right_vel[beh_id] = speed + rot;
  }
}

void move_dual(unsigned char winner) {
 // This subroutine executes the left and right wheel veolocity choices of the winning behavior.  
 // If behavior is the STOP_ID then all stop,
 // else get the wheel velocities from the right_vel[winner] and left_vel[winner] arrays.
   
   if (left_vel[winner]  >  400)  
     left_vel[winner]  =  400; 
   else if (left_vel[winner]  < -400)  
     left_vel[winner]  = -400; 
     
   if (right_vel[winner] >  400)  
     right_vel[winner] =  400; 
   else if (right_vel[winner] < -400)  
     right_vel[winner] = -400; 
   
   motors.setSpeeds(left_vel[winner], right_vel[winner]);
 /*  
 *  setM1Speed(int speed);
 *  setM2Speed(int speed);
 *  setSpeeds(int m1Speed, m2Speed)
 *  speed is >= -400 and <= 400
 */
  
}

I see that it's still wrapping.  Ugh.  I can attach the file if you like.

Thanks Tom,

I will take a better look later this week when I get off work 👍

Hey Mel,

    Have you had a chance to implement any of those Avoid subroutines?

    I caught up on Brian's youtube videos.  I keep hoping to find something on a software architecture for autonomous robots.  

    If you want a second set of eyes on your program let me know.  I'd be happy to help.

Tom

Hi Tom,

Not yet unfortunately, due to a few family issues recently. 
I’m on vacation for a week now, but will be away from home, so will not have chance for a couple of weeks due to work load when I return!

I’m hoping Brian posts another video soon that includes his take on obstacle avoidance that will tie into what I have already written.