Beruflich Dokumente
Kultur Dokumente
h>
#include <Keypad.h>
#include <Ultrasonic.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>
//servo
const int scrap_pin = 41; //
Servo scrap;
//ultrasonic
const int trig_pin_top = 50;
const int echo_pin_top = 48;
Ultrasonic ultrasonic_top(trig_pin_top,echo_pin_top);
Ultrasonic ultrasonic_right(trig_pin_right,echo_pin_right);
Ultrasonic ultrasonic_left(trig_pin_left,echo_pin_left);
//keypad
const byte rows = 4; //four rows
const byte cols = 3; //three columns
char keys[rows][cols] = {
{'1','2','3'},
{'4','5','6'},
{'7','8','9'},
{'#','0','*'}
};
byte rowPins[rows] = {22, 24, 26, 28}; //connect to the row pinouts of the keypad
byte colPins[cols] = {31, 33, 35}; //connect to the column pinouts of the keypad
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, rows, cols );
// Parameters
const int drive_distance = 70; // cm
const int turn_distance = 23.5; // 30cm * pi /4
const int left_motor_speed = 200; // 0-255
const int right_motor_speed = 200; // 0-255
const int motor_offset = 5; // Diff. when driving straight, when the speed is
different
const int wheel_d = 103; // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel circumference (mm) = 323.5
const int counts_per_rev = 12592.8; // (36 holes + 36 black) * (174.9:1 gearbox)
* (2 falling/rising edges) = 25185.6
// Pins
const int encoder_leftA_pin = 18; // Motor A
const int encoder_rightA_pin = 2; // Motor B
// Globals
char encoder_leftA, encoder_leftB;
char encoder_rightA, encoder_rightB;
char key;
String tileLength, startDirection, operation;
float val;
int c=1;
boolean flag = false;
void setup() {
//servo pin
scrap.attach(scrap_pin);
pinMode(lcd_light_pin,OUTPUT);
digitalWrite(lcd_light_pin,LOW);
// Set up pins
pinMode(encoder_leftA_pin, INPUT);
pinMode(encoder_rightA_pin, INPUT);
pinMode(right_pwm_pin, OUTPUT);
pinMode(right_motor_pin1, OUTPUT);
pinMode(right_motor_pin2, OUTPUT);
pinMode(left_pwm_pin, OUTPUT);
pinMode(left_motor_pin1, OUTPUT);
pinMode(left_motor_pin2, OUTPUT);
pinMode(pump, OUTPUT);
pinMode(brush, OUTPUT);
display.begin();
display.setContrast(50);
digitalWrite(brush,LOW);
digitalWrite(pump,LOW);
scrap.write(90);
//length
while(1){
if (flag){
break;
}else {
enterLength();
}
}
//roba or brush
while(1){
if (flag){
break;
}else {
robaOrBrush();
}
}
display.println("Length:");
display.print(" ");
display.println(val);
display.println("Start at:");
display.print(" ");
display.println(startDirection);
display.println("operation");
display.print(" ");
display.println(operation);
display.display();
delay(2000);
// Set up interrupts
attachInterrupt(digitalPinToInterrupt(encoder_leftA_pin), countLeft, CHANGE);
attachInterrupt(digitalPinToInterrupt(encoder_rightA_pin), countRight, CHANGE);
// Drive straight
delay(1000);
void loop() {
if (startDirection == "LEFT"){
driveTillWall();
turnRight();
driveHorizontal();
turnRight();
driveTillWall();
turnLeft();
driveHorizontal();
turnLeft();
driveTillWall();
turnRight();
driveHorizontal();
turnRight();
}
char d = dir;
unsigned long num_ticks_l;
unsigned long num_ticks_r;
// Drive
drive(speed_left, speed_right, d);
// // Number of ticks counted since last time
diff_l = num_ticks_l - enc_l_prev;
diff_r = num_ticks_r - enc_r_prev;
// Brake
brake();
}
if ( direc == 'l' ) {
digitalWrite(right_motor_pin1, HIGH);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, LOW);
digitalWrite(left_motor_pin2, HIGH);
}else {
digitalWrite(right_motor_pin1, HIGH);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, HIGH);
digitalWrite(left_motor_pin2, LOW);
}
// Set speed
analogWrite(left_pwm_pin, abs(power_a));
analogWrite(right_pwm_pin, abs(power_b));
}
void brake() {
digitalWrite(right_motor_pin1, LOW);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, LOW);
digitalWrite(left_motor_pin2, LOW);
analogWrite(right_pwm_pin, 0);
analogWrite(left_pwm_pin, 0);
}
void countLeft() {
enc_l++;
void countRight() {
enc_r++;
void enterLength(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Distance?");
display.println("#:to continue");
display.display();
delay(2000);
//keypad
while(1){
key = keypad.getKey();
if (key){
if (key!='#'){
tileLength += key;
display.print(key);
display.display();
}
else break;
}
}
display.println();
display.println("Save?");
display.println("#:yes, *:no");
display.display();
while(1){
key = keypad.getKey();
if (key){
if (key == '#'){
flag = true;
break;
}
else if (key == '.'){
flag = false;
break;
}
}
}
val = tileLength.toFloat();
void enterDirection(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Direction?");
display.println("1:left");
display.println("2:right");
display.display();
delay(2000);
while(1){
key = keypad.getKey();
if (key){
if (key == '1' || key == '2'){
if (key == '1')startDirection = "LEFT";
else if(key == '2') startDirection = "RIGHT";
flag = false;
break;
}
}
}
void robaOrBrush(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Select:");
display.println("1:roba");
display.println("2:brush");
display.display();
delay(2000);
while(1){
key = keypad.getKey();
if (key){
if (key == '1' || key == '2'){
if (key == '1') operation = "Roba";
else if(key == '2') operation = "Brush";
flag = true;
break;
}
}
}
void turnRight(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Turning?");
display.println("Right");
display.display();
delay(2000);
void turnLeft(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Turning?");
display.println("Left");
display.display();
delay(2000);
void driveHorizontal(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Driving?");
display.println("Horezontal");
display.display();
delay(2000);
void driveTillWall(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Moving?");
display.println("Forward");
display.display();
delay(2000);
if (operation == "Roba"){
digitalWrite(pump,HIGH);
scrap.write(45);
digitalWrite(brush,LOW);
}else if (operation == "Brush"){
digitalWrite(brush,HIGH);
digitalWrite(pump,LOW);
scrap.write(90);
}
if (operation == "Roba"){
digitalWrite(pump,LOW);
scrap.write(90);
digitalWrite(brush,LOW);
}else if (operation == "Brush"){
digitalWrite(brush,LOW);
digitalWrite(pump,LOW);
scrap.write(90);
}
void finish(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("FINISHED");
display.display();
delay(2000);
while(1);
}