Steering Engine Sample Sketch

Transcription

1 Steering Engine Sample Sketch #include <SoftwareServo.h> #include <Servo.h> #define LEFT_CENTER 93 #define RIGHT_CENTER 95 #define SPEED 5 Servo servo_left; Servo servo_right; SoftwareServo servo_scanner; int pingpin = 7; int scanned_values[20]; int run = 1; void setup() { servo_scanner.attach(5); // we attach drive servos later to keep them from creeping Serial.begin(9600); Serial.print("Ready"); void loop() { static int value = 0; static char CurrentServo; static long duration; int closest; if (run == 1) { // scan horizon and store values in scanned_values scan_horizon(); // figure out which direction has the closest value // 0 means it's all the way on the left, 9 middle, 18 full right closest = find_closest(); // rotate to face nearest scanned value // positive values are clockwise turn((closest - 9));

2 // advance for a bit drive_straight(5); delay(200); // Serial command interface if ( Serial.available()) { char ch = Serial.read(); switch(ch) { case 'g':// go run = 1; case 's':// stop run = 0; case 'A': CurrentServo='A'; // center 93 case 'B': CurrentServo='B'; // center 95 case 'C': CurrentServo='C'; // 0 left to 180 full right case '+': value = value +1; case '-': value = value -1; case '0'... '9': value=(ch-'0')*20; if (CurrentServo=='A') { servo_left.write(value); else if (CurrentServo=='B') { servo_right.write(value); else if (CurrentServo=='C') {

3 servo_scanner.write(value); Serial.print("current servo: "); Serial.println(CurrentServo); Serial.println(value); int find_closest() { int i; int m = 1000; int index = 0; for (i=1;i<=17;i+=1) { if (scanned_values[i] < m) { index = i; m = scanned_values[i]; Serial.print("closest: "); Serial.print(index); Serial.print(" "); Serial.println(scanned_values[index]); return index; void scan_horizon() { int i, j; // return to full left. send command and then give it time to move. servo_scanner.write(0); for (j=0;j<40;j++) { delay(20); // take 19 distance readings for (i=0;i<=18;i+=1) {

4 servo_scanner.write(i*10); millisset(0); while(millis() <140) delay(20); scanned_values[i] = ping(); delay(10); // read out scanned values to serial port: Serial.println("new scan:"); for (i=0;i<=18;i++) { Serial.print(scanned_values[i]); Serial.print(' '); Serial.println(); void drive_straight(int distance) { // our servos have slightly different centers, and are oriented oppositely attach_drive_servos(); if (distance > 0) { servo_left.write(left_center - SPEED); servo_right.write(right_center + SPEED); else if (distance < 0) { servo_left.write(left_center + SPEED); servo_right.write(right_center - SPEED); else { delay(abs(distance)*100); // stop drive servos void turn(int distance) { // our servos have slightly different centers, and are oriented oppositely

5 // we attach and detach because it was impossible to get a perfect "center" or "off" position attach_drive_servos(); if (distance > 0) { servo_left.write(left_center - SPEED); servo_right.write(right_center - SPEED); else if (distance < 0) { servo_left.write(left_center + SPEED); servo_right.write(right_center + SPEED); else { delay(abs(distance)*100); // stop drive servos long ping() { pinmode(pingpin, OUTPUT); digitalwrite(pingpin, LOW); delaymicroseconds(2); digitalwrite(pingpin, HIGH); delaymicroseconds(5); digitalwrite(pingpin, LOW); // The same pin is used to read the signal from the PING))): a HIGH // pulse whose duration is the time (in microseconds) from the sending // of the ping to the reception of its echo off of an object. // According to Parallax's datasheet for the PING))), there are // microseconds per inch (i.e. sound travels at 1130 feet per // second). // See: pinmode(pingpin, INPUT); return pulsein(pingpin, HIGH) / 72 / 2; void attach_drive_servos() { servo_left.attach(10); servo_right.attach(9); void detach_drive_servos() {

6 servo_left.detach(); servo_right.detach();