Monday 18 November 2013

Update on One Wheel Robot

I have made soms updates to both the hardware and the code, so here they are.

Hardware wiring:

2013 11 18 10 26 12 There are now four I/Os, the steering servo, a light detector under the from of the robot, two LEDs (red and green) which show the steering direction and the sonar detector for obstacle detection.

The latest code is:

// This is my second go at the one wheel robot. Sigificant program parts are: 
// 1 lots of defines to set parameters, hope to make the code easier to understand later
// 2 LED show the chosen steering direction, for no particular reason other than pretty
// 3 servo angles set it to centre, and R or L
// 4 when a random direction is chosen for forward or reverse, it is biased to the centre
// differently for fwd or rev. bigger number = more bias towards chosing centre direction
// 5 velocity is set, but in reverse it is reduced
// 6 the robot goes forward for a time, then choses a random direction, so as to "roam" around
// 7 if the sonar detects a crash then it backs out either R or L but not C
// 8 a couple of settle times are used to stabilise the operation against hardware glitches
// 9 if it comes across a white place then it stops (parking place!), waits for 10sec then gos off straight

#include "Servo.h"
#include "NewPing.h"

// piezo pin
#define PIEZOPIN A3

// LED pins
#define GREENPIN 2
#define REDPIN 1

// light detector pin
#define LIGHTPIN A4
#define DARK 500

// sonar pins & max distance (above this returns 0)
#define TRIGPIN 5
#define ECHOPIN 4
#define MAXDIST 100

// RobotShield driver pins for motor A
#define MOTORADIRPIN 8
#define MOTORASPEEDPIN 6

// servo pin
#define SERVOPIN A5

// steering positions
#define STEERCENTRE 85
#define STEERLEFT 115
#define STEERRIGHT 55

// random direction bias towards straight(0), 1-3 = left(1), 4-7 = right(2)
#define BIAS 15
#define REVBIAS 10
#define CENTRE 0
#define LEFT 1
#define RIGHT 2

// crash detect distance (cm)
#define CRASH 15

// drive speeds (0 - 255) and directions
#define FWDSPEED 130
#define REVSPEED 130
#define STOP 0
#define FWD 1
#define REV 2

// various timings (msec)
#define FWDTIME 500
#define REVTIME 2000
#define SETTLE 50
#define PARKTIME 10000

Servo steering;  // create servo object
NewPing sonar(TRIGPIN, ECHOPIN, 100); // create NewPing object, max 100 cm away

unsigned long ms, pms;

// =================SETUP================
void setup() {

  // define I/Os
  
  pinMode(GREENPIN, OUTPUT); // LED pins
  pinMode(REDPIN, OUTPUT);
  
  pinMode(LIGHTPIN, INPUT);
  
  pinMode(TRIGPIN, OUTPUT); // sonar pins
  pinMode(ECHOPIN, INPUT);
  
  pinMode(MOTORADIRPIN, OUTPUT); // motor pins for direction and speed
  pinMode(MOTORASPEEDPIN, OUTPUT);
  
  pinMode(SERVOPIN, OUTPUT); // servo pin
  
  // connect servo
  
  steering.attach(SERVOPIN);
  
  // seed random generator
  
  randomSeed(analogRead(A0));
}
  
// =================LOOP================
void loop() {
  
  // wander about
    
  long cm;
  unsigned int dir;
  
  // if find white parking place then stop
  
  if(analogRead(LIGHTPIN) < DARK) {
    drive(STOP, STOP);
    delay(PARKTIME);
    steering.write(STEERCENTRE); // go off straight
    while(analogRead(LIGHTPIN) < DARK) {  // go until off white area
     drive(FWD, FWDSPEED);
    }
  }  
  
  // check for obstacles, if none go forward
  

  cm = sonar.ping_cm(); // get distance (cm), convert us to cm, 60us/cm is roundtrip speed of sound
  delay(50); // at least 50usec between sonar pings
  
  if(cm  == 0 || cm > CRASH) { // no obstacle in CRASH distance out to MAXDIST (returns 0)
  
    drive(FWD, FWDSPEED); // go forward
    
    ms = millis(); // change direction if more than FWDTIME has passed
    if(ms > pms + FWDTIME) {
      dir = randir(BIAS); // save direction chosen, based on BIAS, for use in choice of reverse direction below
      pms = ms;
    }

  }
  
  // obstacle found, reverse away
  
  else {
    
    if(dir == LEFT) steering.write(STEERRIGHT); // if dir was L then R
    else if(dir == RIGHT) steering.write(STEERLEFT); // if dir was R then L
    else while(randir(BIAS) == CENTRE); // if was centre set to only L or R
    drive(REV, REVSPEED);
    delay(REVTIME); // for a time
  }
  
  // loop stabilisation settle time
  
  delay(SETTLE);

}

// chose a random direction, returns direction chosen, lights LEDs
int randir(int limit) {
  
              int x, result;
        
              digitalWrite(REDPIN, LOW); // LEDs off
              digitalWrite(GREENPIN, LOW);
              
              x = random(1, limit); // random direction
              if(x < 4) {
                  steering.write(STEERLEFT);
                  digitalWrite(REDPIN, HIGH); // red LED on
                  result = LEFT;
              }        
              else if(x > 3 && x < 7) {
                  steering.write(STEERRIGHT);
                  digitalWrite(GREENPIN, HIGH); // green LED on
                  result = RIGHT;
              }
              else {
                steering.write(STEERCENTRE);
                result = CENTRE;
              }
              
              delay(SETTLE);
              return result;
}

// drive forward at vel 0-255, cmd = 0 STOP, cmd = 1 FWD, cmd = 2 REV
void drive(int cmd, int vel) {
  
    switch (cmd) {
      case 0: // stop
        analogWrite(MOTORASPEEDPIN, STOP);
        break;
      case 1: // fwd
        digitalWrite(MOTORADIRPIN, HIGH);
        break;
       case 2: // rev
        digitalWrite(MOTORADIRPIN, LOW);
        break;
    }
    analogWrite(MOTORASPEEDPIN, vel);  // set speed
}  
I have the thought to put in some sound generation, maybe the pip, pip, pip when reversing. This would be done by connecting a piezo transducer to A3 output and using the "pitches.h" library and tone() function.

No comments: