The code below is NOT perfect and my mouse lurches a bit, especially after turns and entering the new hall way. I have not yet found out why...
This sketch uses a different MouseSensors.h file to the box sketch so take care! It needs to do this as instead of the events being detected at a fixed distance (DETECT) two variables are use for the side and front distances. A further variable is used for the detection of walls. These variables are set by a call to the sketch calibrate() function. The mouse reads the side distances, then turns east and measures the front distance and the wall distance with one side absent.
I use a Bluetooth module connected to Arduino pins 0 & 1 to send data back to the Mac. I use the program CoolTerm on the Mac to display the readings from the function printsens().
Here's my mouse in a simple labyrinth:
and here's the code in three parts: the ".ino" sketch and the two ".h" class files for motors and sensors:
MOUSELABRTYNTH.INO
/* MouseLabrynth *** MASTE R FILE *** 16-7-14 his program tracks through a Labrynth - that is a twisted, closed pathway with the goal at the end */ #include#include "MouseMotors.h" #include "MouseSensors.h" // `LED & button #define LED 13 #define BUTTON 12 // proportional multiplier (float), KP1 for hall, KP2 for sides #define KP1 0.25 #define KP2 2.5 MouseMotors motors; // motors object MouseSensors sensors; // pass pin numbers to sensors object // event returned by state() byte event; void setup() { Serial.begin(9600); motors.attach(); // attach servos sensors.enable(); // enable emitter pins pinMode(BUTTON, INPUT_PULLUP); // button input pinMode(LED, OUTPUT); // LED output digitalWrite(LED, LOW); while(digitalRead(BUTTON)); // wait for button press delay(100); while(!digitalRead(BUTTON)); // wait for button release digitalWrite(LED, HIGH); // place mouse between walls to calibrate, then push button calibrate(); // get front, and side limits for sensors.state } void loop() { printsens(); // temp DEBUG code labNav(sensors.state()); } // Navigate the labarynth based on event returned by state() function // events map // 7 // 3 1 5 // 2 6 4 void labNav(byte event) { switch(event) // get event { case 0: // no event or north only case 1: motors.halt(); error(); break; case 2: // wall to west fwdNav(); break; case 3: // walls west & north motors.turn(E, 90); sensors.init(); break; case 4: // wall est fwdNav(); break; case 5: // walls west & east motors.turn(W, 90); sensors.init(); break; case 6: // walls north & east fwdNav(); break; case 7: // walls all round motors.halt(); error(); break; } } // go forwards checking sides // if L&R stay in centre, if L or R follow it, if none go strait // Threshold thset is found by calibrate() void fwdNav() { while(sensors.north < sensors.front) // while no wall north { sensors.sense(); if(sensors.west > sensors.wall && sensors.east > sensors.wall) // walls west & east { motors.fwd(0, KP1 * (float)(sensors.east - sensors.west)); // error is a float } else if(sensors.east < sensors.wall) // no east wall { motors.fwd(0, KP2 * (float)(sensors.side - sensors.west)); // follow west wall } else if(sensors.west < sensors.wall) // no west wall { motors.fwd(0, KP2 * (float)(sensors.east - sensors.side)); // follow east wall } else { motors.fwd(0, 0); // just go forwards } } } // get side width, north and wall limits void calibrate() { sensors.init(); // read sensors sensors.side = (sensors.east + sensors.west)/2; // set side limit motors.turn(E, 90); // turn east sensors.init(); // re-read sensors sensors.front = sensors.north; // set north limit sensors.wall = (sensors.side + sensors.west) / 2; // detect wall limit motors.turn(W, 90); // turn back west sensors.init(); // initialise sensors. west, north, east } void error() { while(true) { digitalWrite(LED, HIGH); delay(100); digitalWrite(LED, LOW); delay(100); } } // DEBUG, print sensors data void printsens() { Serial.print("\n front wall side W N E event\n"); Serial.print(sensors.front); Serial.print("\t"); Serial.print(sensors.wall); Serial.print("\t"); Serial.print(sensors.side); Serial.print("\t"); Serial.print(sensors.west); Serial.print("\t"); Serial.print(sensors.north); Serial.print("\t"); Serial.print(sensors.east); Serial.print("\t"); Serial.println(sensors.state()); }
MOUSEMOTORS
// *** MouseMotors MASTER FILE *** 16-7-14 #include#include // servo pins west & east #define WS 10 #define ES 11 // servo HALT position, forward SPEED speed, TURN speed, ROTATION (ms/deg) // and CM (ms/cm) #define HALT 1500 #define SPEED 30 #define TURN 30 #define ROTATE 13 #define CM 185 // sensor bits, event value // 3 2 1 0 // N 0 0 0 1 = 1 // W 0 0 1 0 = 2 // E 0 1 0 0 = 4 // S 1 0 0 0 = 8 // events map // 7 // 3 1 5 // 2 4 #define N 0 #define W 1 #define E 2 #define S 3 Servo westServo; // left side Servo eastServo; // right side class MouseMotors { public: // attach servos to pins void attach() { westServo.attach(WS); eastServo.attach(ES); } // halts the motors void halt() { westServo.writeMicroseconds(HALT); eastServo.writeMicroseconds(HALT); delay(100); // wait 100 ms for inertia } // forward for dist (cm, 0 = continuous) error + W, error - E void fwd(byte dist, float error) { westServo.writeMicroseconds(HALT + SPEED - error); eastServo.writeMicroseconds(HALT - SPEED - error); if(dist != 0) { delay(CM * dist); halt(); } } // turn in direction by degrees void turn(byte dir, byte deg) // turn in dir by deg { switch (dir) { case W: // left westServo.writeMicroseconds(HALT - TURN); eastServo.writeMicroseconds(HALT - TURN); break; case E : // right westServo.writeMicroseconds(HALT + TURN); eastServo.writeMicroseconds(HALT + TURN); break; } delay(deg * ROTATE); // convert degrees to ms runtime halt(); } };
MOUSESENSORS
// *** MouseSensors MASTER FILE version for mouselabrynth *** 16-7-14 // cellwalls and mouse direction use NWES, see bitmap below and event values #include// sensor emitter and detector L, F & R pins #define LE 2 #define LD A0 #define FE 3 #define FD A1 #define RE 4 #define RD A2 // wall ID, event // N 0 0 0 1 = 1 // W 0 0 1 0 = 2 // E 0 1 0 0 = 4 // S 1 0 0 0 = 8 // events map // 7 // 3 1 5 // 2 6 4 // wall ID bits #define N 0 #define W 1 #define E 2 #define S 3 // number of sensor reading to take for init #define AVG 10 class MouseSensors { public: // sensor smoothed readings int west; int north; int east; // north wall detect, side distance & side walls detect int front; int side; int wall; // init ports void enable() { pinMode(LE, OUTPUT); pinMode(FE, OUTPUT); pinMode(RE, OUTPUT); } // read sensors, laod west, north & east ints void sense() { float comb; float amb; float refl; // WEST digitalWrite(LE, HIGH); // emitter on delay(1); comb = analogRead(LD); // save combined value digitalWrite(LE, LOW); // emitter off delay(1); amb = analogRead(LD); // save ambient value refl = comb - amb;// calculate reflected west = int(refl * 0.1 + west * 0.9); // NORTH digitalWrite(FE, HIGH); // emitter on delay(1); comb = analogRead(FD); // save combined value digitalWrite(FE, LOW); // emitter off delay(1); amb = analogRead(FD); // save ambient value refl = comb - amb;// calculate reflected north = int(refl * 0.1 + north * 0.9); // RIGHT digitalWrite(RE, HIGH); // emitter on delay(1); comb = analogRead(RD); // save combined value digitalWrite(RE, LOW); // emitter off delay(1); amb = analogRead(RD); // save ambient value refl = comb - amb;// calculate reflected east = int(refl * 0.1 + east * 0.9); } // read sensors, return event byte (0 - 7) byte state() { byte event; byte detect; sense(); event = 0; // init to zero if(north >= front) event +=1; // add 1 if(west >= wall) event += 2; // add 2 if(east >= wall) event += 4; // add 4 return event; } // init sensors averages by calling sense() AVG times void init() { byte i; for(i = 0; i < AVG; i++) sense(); } };
No comments:
Post a Comment