XBotMicroDemo.ino

//
// Demonstrate various behaviors for XBotMicro
//
// Matthias Neeracher: Derived from original XBotMicro demo4, rewritten in English, 
//    added pinouts for ATtiny X4/X5
// This code is licensed under the same conditions as the original XBotMicro demo code.
//

#include <Arduino.h>

///////////////////////////// Pin Definitions //////////////////////////////////

//
// We define 6 or 7 pins: 4 motor pins, 2 whisker sensor pin, and optionally an LED pin 
// (not on ATtiny85)
//
// Motor pins are defined in pairs: If the forward (Forw) pin is HIGH and the reverse (Back)
// pin is LOW, the wheel on that side turns forward. If Forw is LOW and Back HIGH, the wheel
// turns backward. To fine tune the speed, the Forw pins are attached to PWM (analog out) pins,
// so they can change quickly, while the Back pins are attached to regular digital pins.
//
// The whisker pins are active low digital inputs.
//
#if defined( __AVR_ATtinyX4__ )
//
// ATtiny84/44/24. using arduino-tiny core: https://code.google.com/p/arduino-tiny/
//
// Pinout is chosen to leave the ISP pins free to simplify reprogramming.
//
//                           +-\/-+
//                     VCC  1|    |14  GND
//  WhisR      (D  0)  PB0  2|    |13  AREF (D 10)      BackR
//  WhisL      (D  1)  PB1  3|    |12  PA1  (D  9)      BackL
// (RESET)             PB3  4|    |11  PA2  (D  8)      LED
//  ForwR  PWM (D  2)  PB2  5|    |10  PA3  (D  7) 
//  ForwL  PWM (D  3)  PA7  6|    |9   PA4  (D  6)     (SCK)
// (MOSI)  PWM (D  4)  PA6  7|    |8   PA5  (D  5) PWM (MISO)   
//                           +----+

#define BackR   10
#define ForwR    2
#define ForwL    3
#define BackL    9
#define WhisR    0
#define WhisL    1

#define LED      8
#elif defined( __AVR_ATtinyX5__ )
//
// ATtiny85/45/25. using arduino-tiny core: https://code.google.com/p/arduino-tiny/
//
// Requires setting the RSTDISBL fuse to use the RESET pin as an input, so you will 
// need a high voltage programmer to reprogram.
//
//                      +-\/-+
//  WhisR (D  5)  PB5  1|    |8   VCC
//  WhisL (D  3)  PB3  2|    |7   PB2  (D  2) BackR
//  BackL (D  4)  PB4  3|    |6   PB1  (D  1) ForwR
//                GND  4|    |5   PB0  (D  0) ForwL
//                      +----+

#define BackR  2
#define ForwR  1
#define ForwL  0
#define BackL  4
#define WhisR  5
#define WhisL  3
// No LED pin available
#else
//
// Regular Arduino. Pins 6 and 5 are PWM pins, and 13 is the built in LED
//
#define BackR  7
#define ForwR  6
#define ForwL  5
#define BackL  4
#define WhisR  3
#define WhisL  2

#define LED   13
#endif

///////////////////////// Convenience functions ////////////////////////////////

//
// Obstacle sensing
//
#define ObstL() !digitalRead(WhisL)
#define ObstR() !digitalRead(WhisR)

#ifdef LED
#define LedOn()  digitalWrite (LED, HIGH) 
#define LedOff() digitalWrite (LED,LOW)
#else
#define LedOn()  if (false) ; else
#define LedOff() if (false) ; else
#endif

///////////////////////////// Main program //////////////////////////////////

void setup() 
{ 
  //
  // Pin directions
  //  
  pinMode(BackR, OUTPUT);
  pinMode(ForwR, OUTPUT);
  pinMode(ForwL, OUTPUT);
  pinMode(BackL, OUTPUT);
  pinMode(WhisR, INPUT);
  pinMode(WhisL, INPUT); 

#ifdef LED
  pinMode(LED,OUTPUT);
#endif

  //
  // Motors off
  //
  digitalWrite(BackL, LOW);
  digitalWrite(ForwL, LOW);
  digitalWrite(BackR, LOW);
  digitalWrite(ForwR, LOW);
}

void loop()
{
  switch (GetProgram()) {  // Until we get a valid program number
  case 1:
    while (true) 
      ObstacleAvoidance();
  case 2:
    while (true)
      Ballet();
  case 3:
    while (true)
      Ballet2();
  case 4:
    while (true) {
        LedOn();  delay(1000);
        LedOff(); delay(500);
        ForwardAndBack();
    }  
  }
}

///////////////////////////// Motor Driver ////////////////////////////////////

void Drive(int left, int right)
{
  if (left >= 0) {
    analogWrite(ForwL, left);
    digitalWrite(BackL, LOW);
  } else {
    analogWrite(ForwL, 255-(-left));
    digitalWrite(BackL, HIGH);
  }
  if (right >= 0) {
    analogWrite(ForwR, right);
    digitalWrite(BackR, LOW);
  } else {
    analogWrite(ForwR, 255-(-right));
    digitalWrite(BackR, HIGH);
  }
}

///////////////////////// Input Demo Program # ////////////////////////////////

byte GetProgram()
{
  //
  // Enter program # by tapping left whisker
  //
  byte counter = 0;
  while (!ObstL()) {
    LedOn();  delay(10);
    LedOff(); delay(100);
  }
whiskerTapped:
  if (++counter == 10)
    counter = 9;  // Saturate at 9 taps

  do {
    delay(5);
  } while (ObstL()); // Wait for release

  for (int wait=0; wait<200; ++wait) {
    delay(5);
    if (ObstL())
      goto whiskerTapped; // Another tap  
  } 
  // Timed out after 200x5ms == 1s

  //
  // Acknowledge command
  // 
  for (byte ack = counter; ack>0; --ack) {
    LedOn();  delay(200); 
    LedOff(); delay(300);  
  }
  delay(1000); // Wait before rushing off
  
  return counter;
}

///////////////////////// Demo: Obstacle Avoidance ////////////////////////////////

void ObstacleAvoidance()
{
  if (ObstR()) {       // Hit right whisker, turn left
    Drive(-100, -100); // Reverse
    delay(500);
    Drive(-100, +100); // Turn left
    delay(200);
  } else if (ObstL()) {// Hit left whisker, turn right
    Drive(-100, -100); // Reverse
    delay(500);
    Drive(+100, -100); // Turn right
    delay(200);
  } else {             // No obstacle, straight ahead
    Drive(200, 200);
  }
}

//////////////////////////////// Demo: Ballet //////////////////////////////////////

void Ballet()
{
  Drive(100,100);
  delay(500);
  Drive(50,50);
  delay(500);
  Drive(150,-150);
  delay(500);
  Drive(-150,150);
  delay(500);

  Drive(250,250);
  delay(300);
  Drive(250,-250);
  delay(500);
  Drive(-250,250);
  delay(500);
  Drive(80,80);
  delay(500);
  Drive(-80,880);
  delay(500);
  
  Drive(0,0);   // Pause
  delay(2500);
}

void Ballet2()
{
  Drive(100,100);
  delay(500);
  Drive(50,50);
  delay(500);
  for (int i=0;i<5;i++) {
    Drive(150,-150);
    delay(500);
    Drive(-150,150);
    delay(500);
  }
  for (int i=0;i<3;i++) {
    Drive(150,150);
    delay(500);
    Drive(-150,-150);
    delay(500);
  }
  Drive(0,0);   // Pause
  delay(2500);
}

//////////////////////////////// Demo: Forward and back //////////////////////////////////////

void ForwardAndBack()
{
  //
  // Advance
  //
  LedOn();
  for (int v=0; v<255; ++v) { // Accelerate
    Drive(v,v); 
    delay(6);
  }
  LedOff();
  for (int v=255; v>0; --v) { // Slow down
    Drive(v,v); 
    delay(6);
  }
  for (int v=0; v>-255; --v) { // Accelerate reverse
    Drive(v,v); 
    delay(10);
  }
  LedOn();
  for (int v=-255; v<=0; ++v) // Slow down reverse
  {
    Drive(v,v); 
    delay(10);
  }
  LedOff();
  Drive(0,0); // Pause
  delay(1000);  
}