Society of Robots - Robot Forum

Software => Software => Topic started by: tesseract on March 10, 2011, 03:59:01 PM

Title: Axon II: Wiring C Library
Post by: tesseract on March 10, 2011, 03:59:01 PM
Hi! I don't know much about the Axon and I'm just wondering will it work with wiring (a c library). I'm planning to build a robot that uses Pololu IR Beacon Transceiver. Here's the source code:
Code: [Select]
#include <Servo.h>
#include <AFMotor.h>
#define TOPSPEED 200
 
// SERVO SCANNING VARIABLES //
Servo myservo;  // create servo object to control a servo
int servoPosition = 0;    // variable to store the servo position
const int SERVO_PIN = 10;
int floorState = 0;
long frontReading = 0;
boolean scanIncrement = true;  //increase position?
byte servoIncrementValue = 6;
byte servoDecrementValue = 6;
 
// MOTOR VARIABLES //
AF_DCMotor rightMotor(4, MOTOR12_8KHZ);
AF_DCMotor leftMotor(3, MOTOR12_8KHZ);
 
// TRANSCEIVER VARIABLES //
int notFoundSensitivity = 500;
int west = 0;
int south = 0;
int east = 0;
int north = 0;
int dir = 0;
boolean detected = false;
 
// TRANSCEIVER DIRECTION MODE VARIABLES //
const int NUM_READINGS = 10;
int directionReadings[NUM_READINGS];
int modeOfDirections = 1;
int index = 0;
 
void setup()
{
  Serial.begin(38400);
  myservo.attach(SERVO_PIN);  // attaches the servo on pin 9 to the servo object
  myservo.write(0);
  rightMotor.setSpeed(200);
  leftMotor.setSpeed(200);
  delay(1500);
}
 
void loop()
{
  scan();
  move();
}
 
void move()
{
    if(servoPosition >= 0 && servoPosition <= 84 && frontReading > 550){ //Object on the left
      turnLeft();
    }else if((servoPosition >= 85 && servoPosition <= 105) && frontReading > 600){ // Object in Front
      turnAround();
    }else if((servoPosition >= 106 && servoPosition <= 180) && frontReading > 550){ // Object on the Right
      turnRight();
    }else{
      moveTowardBeacon();
    }
}
 
void moveTowardBeacon()
{
  readTransceiverandSetDirection();
  if(modeOfDirections == 3 || modeOfDirections == 4){ //South or West
    turnRight();
  } else if(modeOfDirections == 2) { // East
    turnLeft();
  } else if(modeOfDirections == 1){ //North
    moveForward();
  }
}
 
void scan()
{
   scanIncrement ? servoPosition+=servoIncrementValue : servoPosition-=servoDecrementValue; //increment or decrement current position
   if (servoPosition >= 180){
     scanIncrement = false;
     servoPosition = 180;
   } else if (servoPosition &lt;= 1){
     scanIncrement = true;
     servoPosition = 1;
   }
   frontReading = measureFront();
   //Serial.print(servoPosition);
   //Serial.print("|");
   //Serial.print(frontReading);
   //Serial.print(";");
   myservo.write(servoPosition);
   delay(15);
}
 
long measureFront()
{
  return analogRead(0);
}
 
void moveForward(){
  Serial.println("Move Forward");
  rightMotor.run(FORWARD);
  leftMotor.run(FORWARD);
}
 
void speedUp(){
  for (int i=0; i==TOPSPEED; i++) {
    rightMotor.setSpeed(i);
    leftMotor.setSpeed(i);
  }
}
 
void slowToStop(){
  for (int i=TOPSPEED; i==0; i--) {
    rightMotor.setSpeed(i);
    leftMotor.setSpeed(i);
  }
  rightMotor.run(RELEASE);
  leftMotor.run(RELEASE);
}
 
void turnLeft(){
  Serial.println("Turn Left");
  rightMotor.run(BACKWARD);
  leftMotor.run(FORWARD);
}
 
void turnRight(){
  Serial.println("Turn Right");
  rightMotor.run(FORWARD);
  leftMotor.run(BACKWARD);
}
 
void stop(){
  Serial.println("Stop");
  rightMotor.run(RELEASE);
  leftMotor.run(RELEASE);
  delay(500);
}
 
void turnAround(){
   stop();
   delay(500);
   moveBackward();
   delay(300);
   turnLeft();
   delay(300);
   stop();
   //runAway = true;
}
 
void moveBackward(){
  Serial.println("Move Backward");
  rightMotor.run(RELEASE);
  leftMotor.run(RELEASE);
  rightMotor.run(BACKWARD);
  leftMotor.run(BACKWARD);
}
 
//  BEACON LOGIC  //
void readTransceiverandSetDirection(){
  west = analogRead(2);
  south = analogRead(3);
  east = analogRead(4);
  north = analogRead(5);
  getDirection();
  setModeOfDirections();
}
 
boolean foundBeacon(){
  if(west < notFoundSensitivity and east < notFoundSensitivity and south < notFoundSensitivity and north < notFoundSensitivity){     return false;   }else{     return true;   } } void getDirection(){   int minValue = 1200;   if(minValue > west){
    minValue = west;
    dir = 4;
  }
 
  if(minValue > south){
    minValue = south;
    dir = 3;
  }
 
  if(minValue > east){
    minValue = east;
    dir = 2;
  }
 
  if(minValue > north){
    minValue = north;
    dir = 1;
  }
 
  addDirectionToReadings();
 
  //Serial.print("W:");
  //Serial.print(west);
  //Serial.print(" | S:");
  //Serial.print(south);
  //Serial.print(" | E:");
  //Serial.print(east);
  //Serial.print(" | N:");
  //Serial.println(north);
  //Serial.println("=================================");
  //if(dir == 1){
  //  Serial.println("North");
  //}else if(dir == 2){
  //  Serial.println("East");
  //}else if(dir == 3){
  //  Serial.println("South");
  //}else if(dir == 4){
  //  Serial.println("West");
  //}
}
 
void addDirectionToReadings(){
  directionReadings[index] = dir;
 
  index = (index + 1);
  if (index &gt;= NUM_READINGS)             // if we're at the end of the array...
    index = 0;
}
 
// =========================================
// In order to smooth out the directions readings from the
// IR transceiver, You have to take the mode (most prevalent number in a collection)
// of the directionReadings Array.  This allows the program to determine which
// direction is being read the most from the device.
// Otherwise, the readings make the robot squirrelly.
// ========================================
void setModeOfDirections(){
  int currentValue = directionReadings[0];
  int counter = 1;
  int maxCounter = 1;
  int modeValue = modeOfDirections;
  int directionCounts[4] = {0,0,0,0}; //{North(1), East(2), South(3), West(4)}
 
  for (int i = 1; i < NUM_READINGS; ++i){
    Serial.print(directionReadings[i]);
    Serial.print("|");
     ++directionCounts[directionReadings[i]-1];
  }
 
  //Determine mode of directions from count array
  Serial.println("");
  int modeCount[2] = {1,directionCounts[0]}; //This array holds the current maximum count and the direction it points to.
  for(int i = 0; i < 4; ++i){
    //Serial.print(directionCounts[i]);
    //Serial.print("|");
    if(modeCount[1] >= directionCounts[i]){
      modeCount[0] = i + 1; // set direction
      modeCount[1] = directionCounts[i]; //set count
    }
  }
 
  modeOfDirections = modeCount[0];
  Serial.println("");
  Serial.print("Direction Mode: ");
  Serial.println(modeOfDirections);
}
Title: Re: Axon II: Wiring C Library
Post by: Admin on March 12, 2011, 02:13:59 PM
As I understand it, Wiring was the grandfather of Arduino, meaning the Arduino software would be better.

That said, the user madsci was thinking of writing some compatibility to allow you to use the Arduino software to program the Axon, but not sure if he's decided yet . . .

Even then, I'd recommend using WebbotLib:
How to Program Axon II in PD (http://www.youtube.com/watch?v=vWG1rfwgAX0#)