Tym razem chciałem wam pokazać ok 1% możliwości czyli prosty ześwirowany robocik.
Podwoziem robota jest platforma DF Robot MINI 2WD
z 2ma silnikami POLOLU 50:1
którą opisałem tutaj :
http://sunduino.elektroda.eu/wordpress/ ... z-dfrobot/
Do poruszania czujnikiem PING użyłem Mini serwa SG90
opisanego tu:
http://sunduino.elektroda.eu/wordpress/ ... delarskie/
oraz czujnika ping HC-SR04 który opisałem tu :
http://sunduino.elektroda.eu/wordpress/ ... kowe-ping/
-----------------------------
Podstawę całości stanowi płytka SunDuino 1.7 z prockiem m328 pędzonym 16Mhz
oraz
Arduino Motor Shield rev3
program jest jak na duino twory bardzo prosty:
[syntax=cpp]#include <NewPing.h>
#include <Servo.h>
#define SENSOR_TRIG_PIN 7
#define SENSOR_ECHO_PIN 6
#define SENSOR_MIN_DIST 40
#define SENSOR_MAX_DIST 200
#define MOTOR_CH_A 12
#define BRAKE_CH_A 9
#define SPEED_CH_A 3
#define MOTOR_CH_B 13
#define BRAKE_CH_B 8
#define SPEED_CH_B 11
#define HI_SPEED 255
#define LO_SPEED 128
NewPing sonar(SENSOR_TRIG_PIN, SENSOR_ECHO_PIN, SENSOR_MAX_DIST);
// servowatość :
int pos = 0;
int servoCenter = 45; // środkowa pozycja
int servoRight = 180; // 180 = pełny obrót w prawo
int servoLeft = 0; // 0 = pełny obrót w lewo
Servo myservo; // obiekt servo
void setup() {
Serial.begin(9600); // w celach debugowych
// definicje pinów silników
pinMode(MOTOR_CH_A, OUTPUT);
pinMode(BRAKE_CH_A, OUTPUT);
pinMode(SPEED_CH_A, OUTPUT);
pinMode(MOTOR_CH_B, OUTPUT);
pinMode(BRAKE_CH_B, OUTPUT);
pinMode(SPEED_CH_B, OUTPUT);
myservo.attach(5); // serwo podłączone do pinu D5
myservo.write(servoCenter); // ustawienie środkowe na starcie
}
void loop() {
// delay(50);
int ping = sonar.ping_cm();
int dir;
Serial.println(ping);
if ( ping > SENSOR_MIN_DIST ) {
go_forward();
}
if ( ping <= SENSOR_MIN_DIST ) {
// if ( ping != 0 ) {
brake();
delay(20);
dir = go_look();
delay(20);
go_backwards();
delay(650);
if (dir = 1) {
//Serial.println(“Lewa zablokowana - smigam w prawo.”);
Serial.println(dir);
turn_right();
delay(550);
}
if (dir = 2) {
//Serial.println(“Prawa zablokowana - smigam w lewo”);
Serial.println(dir);
turn_left();
delay(550);
}
brake();
delay(20);
// }
}
}
// wyyszukienie wolnej drogi
int go_look () {
myservo.write(servoRight);
int anglel = myservo.read();
Serial.println(anglel);
delay(1000);
myservo.write(servoLeft);
int angler = myservo.read();
Serial.println(angler);
delay(1000);
myservo.write(servoCenter);
int anglec = myservo.read();
Serial.println(anglec);
if ( anglel <= SENSOR_MIN_DIST ) {
return 1;
}
if ( angler <= SENSOR_MIN_DIST ) {
return 2;
}
if (anglec <= SENSOR_MIN_DIST){
return 3;
}
}
void go_forward() {
//Serial.println(“Do przodu ...”);
digitalWrite(MOTOR_CH_A, HIGH);
digitalWrite(BRAKE_CH_A, LOW);
analogWrite(SPEED_CH_A, HI_SPEED);
digitalWrite(MOTOR_CH_B, HIGH);
digitalWrite(BRAKE_CH_B, LOW);
analogWrite(SPEED_CH_B, HI_SPEED);
}
void go_backwards() {
//Serial.println(“Do tylu ....”);
digitalWrite(MOTOR_CH_A, LOW);
digitalWrite(BRAKE_CH_A, LOW);
analogWrite(SPEED_CH_A, LO_SPEED);
digitalWrite(MOTOR_CH_B, LOW);
digitalWrite(BRAKE_CH_B, LOW);
analogWrite(SPEED_CH_B, HI_SPEED);
}
void turn_left() {
//Serial.println(“W lewo”);
digitalWrite(BRAKE_CH_A, LOW);
analogWrite(SPEED_CH_A, HI_SPEED);
digitalWrite(MOTOR_CH_B, HIGH);
digitalWrite(BRAKE_CH_B, LOW);
analogWrite(SPEED_CH_B, HI_SPEED);
}
void turn_right() {
//Serial.println(“W prawo”);
digitalWrite(MOTOR_CH_A, HIGH);
digitalWrite(BRAKE_CH_A, LOW);
analogWrite(SPEED_CH_A, HI_SPEED);
digitalWrite(BRAKE_CH_B, LOW);
analogWrite(SPEED_CH_B, HI_SPEED);
}
void brake() {
//Serial.println(“Silniki STOP”);
digitalWrite(BRAKE_CH_A, HIGH);
digitalWrite(BRAKE_CH_B, HIGH);
}[/syntax]
Zasilanie stanowi pakiet Ni-MH 2200mAh 8,4V zasilanie na silnikach 6V
Całość prezentuje się następująco :
czyli nic specjalnego ani urodziwego , ot wygląda jakby go zrobiono z tego co się znalazło
w podręcznym zlewie
a tak się zachowuję spuszczony ze smyczy
iframe
Miłej zabawy ...
Statystyki: Napisane przez SunRiver — 16 lip 2013, o 20:17
]]>