19. Oktober 2012
![fbbg](../../../../files/2012/10/fbbg-1024x575.png)
![Entwürfe](../../../../files/2012/10/2012-10-12-08.28-1024x768.png)
![Lego Technik Prototyp](../../../../files/2012/10/2012-10-09-15.54.42-1024x768.jpg)
![cut](../../../../files/2012/10/cut-1024x768.png)
![Kevins Montage](../../../../files/2012/10/2012-10-10-15.53.47-1024x768.jpg)
![Kevin](../../../../files/2012/10/2012-10-12-08.52.09-1024x768.jpg)
#include <Servo.h> #define SENSOR_LEFT A2 #define SENSOR_RIGHT A1 #define SENSOR_FRONT A3 #define SENSOR_BACK A0 int signal_left; int signal_right; int signal_front; int signal_back; int signal_left_old; int signal_right_old; int signal_front_old; int signal_back_old; int tempoRechts = 0; int tempoLinks = 0; boolean alleine = false; int alleinTolleranz = 5000; int alleinZeit = 0; Servo myservo; int motor01AnschlussRechts = 5; int motor01AnschlussLinks = 3; int motor02AnschlussRechts = 11; int motor02AnschlussLinks = 6; long lastDebounceTime = 0; int debounceDelay = 2000; int servoState = 0; void setup(){ pinMode(motor01AnschlussLinks, OUTPUT); pinMode(motor01AnschlussRechts, OUTPUT); pinMode(motor02AnschlussLinks, OUTPUT); pinMode(motor02AnschlussRechts, OUTPUT); myservo.attach(10); myservo.write(0); Serial.begin(9600); } void loop(){ //motor01Rechts(); //motor01Links(); //motor01SpeedRechts(); //motor01SpeedLinks(); //roboVorwaerts(); //roboDrehungRechts(); signal_left = map(analogRead(SENSOR_LEFT), 80, 620, 100, 0); signal_right = map(analogRead(SENSOR_RIGHT), 80, 620, 100, 0); signal_front = map(analogRead(SENSOR_FRONT), 80, 620, 100, 0); signal_back = map(analogRead(SENSOR_BACK), 80, 620, 100, 0); /* if(millis()-lastDebounceTime > debounceDelay){ servoState += 30; myservo.write(servoState); lastDebounceTime = millis(); } */ Serial.println(); Serial.print("left "); Serial.print(analogRead(SENSOR_LEFT)); Serial.print(" "); Serial.print("right "); Serial.print(analogRead(SENSOR_RIGHT)); Serial.print(" "); Serial.print("font "); Serial.print(analogRead(SENSOR_FRONT)); Serial.print(" "); Serial.print("back "); Serial.print(analogRead(SENSOR_BACK)); Serial.println(); drive(); verlassen(); } void roboDrehungRechts(){ digitalWrite(motor01AnschlussLinks, HIGH); digitalWrite(motor01AnschlussRechts, LOW); digitalWrite(motor02AnschlussLinks, LOW); digitalWrite(motor02AnschlussRechts, HIGH); } void roboDrehungLinks(){ digitalWrite(motor01AnschlussLinks, HIGH); digitalWrite(motor01AnschlussRechts, LOW); digitalWrite(motor02AnschlussLinks, LOW); digitalWrite(motor02AnschlussRechts, HIGH); } void roboVorwaerts(){ digitalWrite(motor01AnschlussLinks, HIGH); digitalWrite(motor01AnschlussRechts, LOW); digitalWrite(motor02AnschlussLinks, HIGH); digitalWrite(motor02AnschlussRechts, LOW); } void motor01Rechts(){ digitalWrite(motor01AnschlussLinks, LOW); digitalWrite(motor01AnschlussRechts, HIGH); } void motor01Links(){ digitalWrite(motor01AnschlussLinks, HIGH); digitalWrite(motor01AnschlussRechts, LOW); } void motor01SpeedRechts(){ for(int i=0; i<255; i++) { analogWrite(motor01AnschlussRechts, i); // Den Wert i an LED_PIN setzen delay(10); } for(int i= 255; i>0; i--) { analogWrite(motor01AnschlussRechts, i); // Den Wert i an LED_PIN setzen delay(10); } } void motor01SpeedLinks(){ for(int i=0; i<255; i++) { analogWrite(motor01AnschlussLinks, i); // Den Wert i an LED_PIN setzen delay(10); } for(int i= 255; i>0; i--) { analogWrite(motor01AnschlussLinks, i); // Den Wert i an LED_PIN setzen delay(10); } } void drive(){ tempoRechts = signal_front + signal_right - signal_left - signal_back; tempoLinks = signal_front + signal_left - signal_right - signal_back; tempoRechts = map(tempoRechts, -100, 100, -255, 255); tempoLinks = map(tempoLinks, -100, 100, -255, 255); Serial.print("tempoRechts "); Serial.print(tempoRechts); Serial.print(" "); Serial.print("tempoLinks "); Serial.print(tempoLinks); Serial.println(); if(tempoRechts > 40){ analogWrite(motor01AnschlussLinks, LOW); digitalWrite(motor01AnschlussRechts, tempoRechts); alleine = false; } if(tempoRechts < -40){ int tempoRechtsMinus = 0 - tempoRechts; digitalWrite(motor01AnschlussLinks, tempoRechtsMinus); analogWrite(motor01AnschlussRechts, LOW); alleine = false; } if(tempoRechts > - 40 && tempoRechts < 40){ digitalWrite(motor01AnschlussLinks, LOW); analogWrite(motor01AnschlussRechts, LOW); if(alleine == false){ istAlleine(); } } if(tempoLinks > 40){ analogWrite(motor02AnschlussLinks, LOW); digitalWrite(motor02AnschlussRechts, tempoLinks); alleine = false; } if(tempoLinks < -40){ int tempoLinksMinus = 0 - tempoLinks; digitalWrite(motor02AnschlussLinks, tempoLinksMinus); analogWrite(motor02AnschlussRechts, LOW); alleine = false; } if(tempoLinks > -40 && tempoLinks < 40){ digitalWrite(motor02AnschlussLinks, LOW); analogWrite(motor02AnschlussRechts, LOW); if(alleine == false){ istAlleine(); } } } void istAlleine(){ Serial.println("+++++++++++++++++++++++++++++++++HERE+++++++++++++++++++++++++++++++++++++++++"); alleine = true; alleinZeit = millis(); } void verlassen(){ if(alleine && millis() - alleinZeit > alleinTolleranz && servoState <= 70){ servoState += 10; myservo.write(servoState); delay(2000); alleinZeit = millis(); } }