Society of Robots - Robot Forum
Software => Software => Topic started 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:
#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 <= 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 >= 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);
}
-
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#)